' Robotics with the Boe-Bot - BoeBotForwardTenSeconds.bs2 ' Make the Boe-Bot roll forward for ten seconds. ' {$STAMP BS2} ' {$PBASIC 2.5} counter VAR Word pulseCount VAR Word qtiLeft VAR Bit qtiCenter VAR Bit qtiRight VAR Bit foundRight VAR Bit foundLeft VAR Bit lastSearch VAR Bit '1 - left, 0 - Right trials VAR Bit FREQOUT 4, 2000, 3000 ' Signal program start/reset. foundRight = 1 foundLeft = 1 lastSearch = 0 trials = 0 cont: DO HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR," main loop -> ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight IF (qtiCenter = 0 AND qtiRight = 1) THEN GOTO Search_Right IF (qtiCenter = 0 AND qtiLeft = 1) THEN GOTO Search_Left IF (qtiCenter = 0 AND qtiRight = 0 AND qtiLeft = 0) THEN IF (lastSearch = 0 AND foundRight = 1) THEN GOTO Turn_Right ELSEIF (foundLeft = 1) THEN GOTO Turn_Left ENDIF GOTO Scan ENDIF DO WHILE (qtiCenter = 1) PULSOUT 13, 850 ' Left servo full speed ccw. PULSOUT 12, 650 ' Right servo full speed cw. PAUSE 100 HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 LOOP LOOP Search_Left: ' right turn subroutine. lastSearch = 1 FOR counter = 1 TO 12 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 650 ' 1.7 ms pulse to left servo. PULSOUT 12, 650 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR, "search left... ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight,CR IF (qtiCenter = 1) THEN foundLeft = 1 GOTO cont ENDIF NEXT NEXT DEBUG CR,"aiurea left..." foundLeft = 0 GOTO cont Search_Right: ' right turn subroutine. lastSearch = 0 FOR counter = 1 TO 12 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 850 ' 1.7 ms pulse to left servo. PULSOUT 12, 850 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR, "search right... ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight,CR IF (qtiCenter = 1) THEN foundRight = 1 GOTO cont ENDIF NEXT NEXT DEBUG CR,"aiurea right..." foundRight = 0 GOTO cont Move_Forward: ' right turn subroutine. FOR counter = 1 TO 12 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 850 ' 1.7 ms pulse to left servo. PULSOUT 12, 650 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR, "moveForward... ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight,CR IF (qtiCenter = 1) THEN foundRight = 1 GOTO cont ENDIF NEXT NEXT DEBUG CR,"aiurea right..." foundRight = 0 GOTO cont '/****************************************************/ Turn_Left: ' right turn subroutine. lastSearch = 0 FOR counter = 1 TO 6 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 650 ' 1.7 ms pulse to left servo. PULSOUT 12, 650 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR, "turn left... ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight,CR IF (qtiCenter = 1 OR qtiLeft = 1 OR qtiRight = 1) THEN GOTO cont ENDIF NEXT NEXT foundLeft = 0 GOTO cont Turn_Right: ' right turn subroutine. lastSearch = 0 FOR counter = 1 TO 6 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 850 ' 1.7 ms pulse to left servo. PULSOUT 12, 850 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR, "turn right... ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight,CR IF (qtiCenter = 1 OR qtiLeft = 1 OR qtiRight = 1) THEN GOTO cont ENDIF NEXT NEXT foundRight = 0 GOTO cont Scan: foundRight = 1 foundLeft = 1 'IF lastSearch = 0 THEN 'FOR counter = 1 TO 12 ' FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. ' PULSOUT 13, 650 ' 1.7 ms pulse to left servo. ' PULSOUT 12, 650 ' 1.7 ms pulse to right servo. 'PAUSE 20 ' Pause for 20 ms. 'NEXT 'NEXT 'ELSE ' FOR counter = 1 TO 12 ' FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. ' PULSOUT 13, 850 ' 1.7 ms pulse to left servo. ' PULSOUT 12, 850 ' 1.7 ms pulse to right servo. ' PAUSE 20 ' Pause for 20 ms. ' NEXT 'NEXT 'ENDIF DO FOR counter = 1 TO 4 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 850 ' 1.7 ms pulse to left servo. PULSOUT 12, 650 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. HIGH 5: PAUSE 1: qtiRight = IN3: INPUT 5 HIGH 6: PAUSE 1: qtiCenter = IN3: INPUT 6 HIGH 7: PAUSE 1: qtiLeft = IN3: INPUT 7 DEBUG CR, "search left... ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight,CR IF (qtiCenter = 1) THEN foundLeft = 1 GOTO cont ENDIF NEXT NEXT FOR counter = 1 TO 6 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 650 ' 1.7 ms pulse to left servo. PULSOUT 12, 650 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. IF (qtiCenter = 1 OR qtiLeft = 1 OR qtiRight = 1) THEN GOTO cont ENDIF NEXT PAUSE 50 NEXT FOR counter = 1 TO 12 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 850 ' 1.7 ms pulse to left servo. PULSOUT 12, 850 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. IF (qtiCenter = 1 OR qtiLeft = 1 OR qtiRight = 1) THEN GOTO cont ENDIF NEXT PAUSE 50 NEXT FOR counter = 1 TO 6 FOR pulseCount = 1 TO 2 ' Send 24 right rotate pulses. PULSOUT 13, 650 ' 1.7 ms pulse to left servo. PULSOUT 12, 650 ' 1.7 ms pulse to right servo. PAUSE 20 ' Pause for 20 ms. IF (qtiCenter = 1 OR qtiLeft = 1 OR qtiRight = 1) THEN GOTO cont ENDIF NEXT PAUSE 50 NEXT DEBUG CR, ? trials, CR trials = trials + 1 LOOP UNTIL (trials = 2) GOTO Finish Finish: DEBUG CR," finish: ",BIN1 qtiLeft, BIN1 qtiCenter, BIN1 qtiRight, CR DEBUG "program stopped !", CR