'-----[ Title ]----------------------------------------------------- 'Robotics with the Boe-Bot - RightServoTest.bs2 'Boe-Bot uses whiskers to detect objects, and navigates around them. ' {$STAMP BS2} 'Stamp Directive ' {$PBASIC 2.5} 'PBASIC Directive DEBUG "Program Running!" ' ----[ Variables ]------------------------------------------------- pulseCount VAR Byte 'FOR...NEXT loop counter ' ----[ Initialization ]-------------------------------------------- FREQOUT 4, 2000, 3000 'Signal Program Start/Reset ' ----[ Main Routine ]---------------------------------------------- DO IF (IN5 = 0) AND (IN7 = 0) THEN 'Both Whiskers Detect Object GOSUB Back_Up 'Back Up & U-Turn GOSUB Turn_Left GOSUB Turn_Left ELSEIF (IN5 = 0) THEN 'Left Whisker Contacts GOSUB Back_Up 'Back Up & Turn Right GOSUB Turn_Right ELSEIF (IN7 = 0) THEN 'Right Whisker Contacts GOSUB Back_Up 'Back Up & Turn Left GOSUB Turn_Left ELSE 'Both Whiskers 1, No Contact GOSUB Forward_Pulse 'Apply A Forward Pulse ENDIF 'Check Again LOOP ' ----[ Subroutines ]---------------------------------------------- Forward_Pulse: 'Send A Single Forward Pulse PULSOUT 13, 850 PULSOUT 12, 660 PAUSE 20 RETURN Turn_Left: 'Left Turn, About 90 Degrees FOR pulseCount= 0 TO 20 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT RETURN Turn_Right: 'Right Turn, About 90 Degrees FOR pulseCount = 0 TO 20 PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 20 NEXT RETURN 'Back Up. Back_Up: FOR pulseCount = 0 TO 40 PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 20 NEXT RETURN