' -----[ Title ]-------------------------------------------------------------- ' Smart Sensors and Applications - Ping)))Dar.bs2 ' Display radar style object distance measurements in the Debug Terminal. ' as the Boe-Bot sweeps the Ping))) Mounting Bracket servo from right ' (0-degrees) to left (180-degrees). ' {$STAMP BS2} ' Target device = BASIC Stamp 2 ' {$PBASIC 2.5} ' Language = PBASIC 2.5 ' -----[ I/O Definitions ]---------------------------------------------------- PingServo PIN 14 ' Servo that directs Ping))) Ping PIN 15 ' Ping))) sensor signal pin ' -----[ Constants ]---------------------------------------------------------- LimitLeft CON 1250 ' Bracket 90-degrees LimitLeft LimitRight CON 250 ' Bracket 90-degrees LimitRight PingDirToAngle CON 8454 ' Servo pulse -> angle with ** CmConstant CON 2260 ' Echo time -> cm with ** SinCosTo256 CON 517 ' For */ -127..127 -> -256..256 Increment CON 15 ' Servo PULSOUT increment value Negative CON 1 ' Negative sign Positive CON 0 ' Positive sign ' -----[ Variables ]---------------------------------------------------------- pingDir VAR Word ' Pulse duration -> direction time VAR Word ' Ping))) echo time distance VAR Time ' Object distance x VAR Word ' x Debug cursor coordinate y VAR Word ' y Debug cursor coordinate angle VAR Byte ' Angle from LimitRight in brads counter VAR angle ' Loop counter sweepInc VAR Nib ' Sweep increment sweepDir VAR Bit ' Increment/decrement pingDir xSign VAR Bit ' x variable sign ySign VAR xSign ' Stores sign of y variable nsign VAR Bit ' Numerator sign dsign VAR Bit ' Denominator sign ' -----[ Initialization ]----------------------------------------------------- pingDir = LimitRight ' Start servo at 0-degrees FOR counter = 1 TO 40 ' Initialize servo position PULSOUT PingServo, pingDir PAUSE 20 NEXT sweepInc = Increment ' Set the sweep increment ' -----[ Main Routine ]------------------------------------------------------- DO IF pingDir <= LimitRight THEN ' Refresh display if far right DEBUG CLS, CRSRXY, 50, 25, "X" ENDIF GOSUB Sweep_Increment ' Move servo by sweepInc ' Calculate angle from far right in brads. angle = pingDir - LimitRight ** PingDirToAngle GOSUB Get_Ping_Cm ' Get cm measurement distance = distance MAX 100 / 4 ' Scale for Debug Terminal GOSUB Polar_To_Cartesian ' distnace @ angle -> (x, y) x = x * 2 ' 2 spaces for every 1 CR DEBUG CRSRXY, 50 + x, 25 - y, "*" ' Display (x, y) coordinate LOOP ' -----[ Subroutine - Get_Ping_Cm ]------------------------------------------- ' Gets Ping))) rangefinder measurement and converts time to centimeters. ' Distance may be declared as time to save variable space. Get_Ping_Cm: PULSOUT Ping, 5 PULSIN Ping, 1, time distance = time ** CmConstant RETURN ' -----[ Subroutine - Polar_To_Cartesian ]------------------------------------ ' Calculates x and y (Cartesian coordinates) given distance and angle ' (polar coordinates). Polar_To_Cartesian: ' Calculate x coordinate. x = COS angle ' Polar to Cartesian xSign = x.BIT15 ' Store sign bit x = ABS(x) */ SinCOsTo256 ' Polar to Cartesian continued x = distance */ x IF xSign = negative THEN x = -x ' Correct sign with sign bit ' Calculate y coordinate. y = SIN angle ' Polar to Cartesian ySign = y.BIT15 ' Store sign bit y = ABS(y) */ SinCOsTo256 ' Polar to Cartesian continued y = distance */ y IF ySign = negative THEN y = -y ' Correct sign with sign bit RETURN ' -----[ Subroutine - Sweep_Increment ]--------------------------------------- ' Increment/decrement the position of the servo that directs the Ping))) ' rangefinder. When pingDir goes outside either LimitRight or LimitLeft, ' the sweep direction toggles. Sweep_Increment: ' Change sweepDir for adding/subtracting increment if at rotation limit. IF pingDir <= LimitRight THEN sweepDir = Positive ELSEIF pingDir >= LimitLeft THEN sweepDir = Negative ENDIF ' Add/subtract increment to/from pingDir. IF sweepDir = negative THEN pingDir = pingDir - sweepInc ELSEIF sweepDir = Positive THEN pingDir = pingDir + sweepInc ENDIF ' Send positioning pulse to Ping))) Mounting Bracket servo. PULSOUT PingServo, pingDir RETURN