' {$STAMP BS2} ' {$PBASIC 2.5} ' -----[ I/O Definitions ]------------------------------------------------- Ping PIN 2 INPUT 0 'left sensor INPUT 1 'right sensor INPUT 3 'switch ' -----[ Constants ]------------------------------------------------------- qtis VAR Nib ' qti black/white states stuck VAR Word' stuck counter rawDist VAR Word ' Distance measurement Dist VAR Word ' distance for ping sensor lastdist VAR Word 'last distance read by ping sensor c VAR Word T VAR Word V VAR Word Z VAR Word M VAR Word homesearch VAR Byte 'home search bool '--- ( initializations}---------------------- stuck=0 lastdist=1000 OUTB = %1111 homesearch=0 '----(main progam)---------------------------- DO WHILE IN1=1 PAUSE 100 'wait for whisker to start LOOP GOSUB start GOTO findmine ' -----[ Subroutines ]----------------------------------------------------- ' This subroutine triggers the Ping sonar sensor and measures ' the echo pulse. The raw value from the sensor is converted to ' microseconds based on the Stamp module in use. This value is ' divided by two to remove the return trip -- the result value is ' the distance from the sensor to the target in microseconds. '-------------------------------------------------------------------------GET_SONAR (SUB) Get_Sonar: Ping = 0 ' make trigger 0-1-0 PULSOUT Ping, 5 ' activate sensor PULSIN Ping, 1, rawDist ' measure echo pulse rawDist = rawDist */ $200 ' convert to uS rawDist = rawDist / 2 ' remove return trip Dist=rawDist**889 RETURN '---------------------------------------------------------------- '-------------------------------------------------------------------------CHECK_QTIS (SUB) Check_Qtis: ' Result -> qtis variable. 0 means white surface, 1 means black surface. DIRB = %1111 ' P7..P4 -> output PAUSE 0 ' Delay = 230 us DIRB = %0000 ' P7..P4 -> input PAUSE 0 ' Delay = 230 us ' PULSOUT UnusedPin, 0 ' Delays = 208 + (Duration*2) us qtis = INB ' Store QTI outputs in INB RETURN '-------------------------------------------------------------------------START (SUB) start: T=0 FOR T=1 TO 16 'turn right PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 50 NEXT T=0 FOR T=1 TO 170 'drive straight IF (IN0 = 0) OR (IN1=0) THEN GOTO testmine ENDIF PULSOUT 13, 850 PULSOUT 12, 650 PAUSE 50 NEXT GOSUB Get_sonar M=0 FOR M=1 TO 50 M=M+1 IF Dist<30 THEN GOTO doublecheck: ENDIF T=0 FOR T=1 TO 5 'turn right PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 50 NEXT PAUSE 100 GOSUB Get_sonar NEXT GOTO checkagain: doublecheck: lastdist=Dist T=0 FOR T=1 TO 5 GOSUB Get_sonar IF (Dist>lastdist+2) OR (Dist 45 IF (IN0 = 0) OR (IN1=0) THEN 'check wiskers GOTO testmine ENDIF T=0 FOR T=1 TO 20 'move 20 times then check sonar GOSUB Check_Qtis 'Get QTI states GOSUB linefollow NEXT PAUSE 60 GOSUB Get_sonar LOOP 'make sure distance stays the same for 5 iterations to take out the crap lastdist=Dist T=0 FOR T=1 TO 5 GOSUB Get_sonar IF (Dist>lastdist+2) OR (Dist10 THEN FOR T=1 TO (Dist-5)*2 'drive straight --Do we still need this FOR loop?-Matt IF (IN0 = 0) OR (IN1=0) THEN GOTO testmine ENDIF PULSOUT 13, 850 PULSOUT 12, 650 PAUSE 50 NEXT ENDIF lastdist=Dist M=0 DO UNTIL ((IN0 = 0) OR (IN1=0)) 'added parentheses-Matt PAUSE 50 GOSUB Get_Sonar 'get sensor value PAUSE 100 IF (Dist5 THEN 'full speed PULSOUT 13, 850 PULSOUT 12, 650 PAUSE 50 ELSE 'if close slow down PULSOUT 13, 800 PAUSE 10 PULSOUT 12, 700 PAUSE 10 ENDIF IF (IN0 = 0) OR (IN1=0) THEN '----Redundant??-Matt GOTO testmine ENDIF NEXT lastdist=Dist M=0 ELSE 'look right if mine not found should drive foward first, then look left then loook right never turn around. DO WHILE (Dist>lastdist+7 ) IF (IN0 = 0) OR (IN1=0) THEN GOTO testmine ENDIF T=0 IF M<15 THEN FOR T=1 TO 2 PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 30 NEXT ELSEIF M>=15 AND M<=45 THEN FOR T=1 TO 2 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 30 NEXT ELSE GOTO checkagain ENDIF M=M+1 GOSUB Get_Sonar PAUSE 100 LOOP ENDIF LOOP GOTO testmine '-------------------------------------------------------------------------LINEFOLLOW (SUB) linefollow: SELECT qtis ' Control servo speeds/directions CASE %1000 'Rotate Left PULSOUT 13, 650 PULSOUT 12, 650 stuck=0 CASE %1100 'Pivot Left PULSOUT 13, 800 PULSOUT 12, 650 stuck=0 CASE %0100 'Curve Left PULSOUT 13, 800 PULSOUT 12, 650 stuck=0 CASE %0110 'Straight ahead PULSOUT 13, 850 PULSOUT 12, 650 stuck=0 CASE %0010 'Curve Right PULSOUT 13, 850 PULSOUT 12, 700 stuck=0 CASE %0011 'Pivot Right PULSOUT 13, 850 PULSOUT 12, 750 stuck=0 CASE %0001 'Rotate Right PULSOUT 13, 850 PULSOUT 12, 850 stuck=0 CASE %0111 '??? curve right GOSUB checkhome stuck=0 CASE %1110 '??? rotate Left B0=1 FOR B0=1 TO 25 PULSOUT 13, 780 PULSOUT 12, 650 PAUSE 40 NEXT stuck=0 CASE %0000 '?? no tape stuck=stuck+1 IF stuck < 10 THEN 'FORWARED PULSOUT 13,850 PULSOUT 12, 700 PAUSE 50 ELSEIF stuck >=11 AND stuck <=30 THEN PULSOUT 13,700 PULSOUT 12, 850 PAUSE 50 ELSE PULSOUT 13,700 PULSOUT 12,800 ENDIF CASE %1111 'all tape PULSOUT 13, 850 'turnright PULSOUT 12, 850 stuck=0 CASE ELSE 'turn right PULSOUT 13, 850 PULSOUT 12, 850 ENDSELECT RETURN '----------------------------------------------------TESTMINE (SUB) testmine: M=0 FOR M=1 TO 100 'try and center the robot IF IN1=0 AND IN0=1 THEN PULSOUT 13, 780 PULSOUT 12, 780 'turn right ELSEIF IN1=1 AND IN0=0 THEN PULSOUT 12, 720 PULSOUT 13, 720 'turn left ELSEIF IN1=1 AND IN0=1 THEN 'straight PULSOUT 13, 780 PULSOUT 12, 720 ELSEIF IN1=0 AND IN0=0 THEN GOTO minecentered ENDIF PAUSE 30 NEXT minecentered: 'if mine is found first time M=0 FOR M=1 TO 40 PULSOUT 14,650 'down PAUSE 50 NEXT PAUSE 100 M=0 FOR M=1 TO 47 PULSOUT 14,850 'up PAUSE 50 NEXT M=0 FOR M=1 TO 3 PULSOUT 14,650 'down release tension PAUSE 50 NEXT IF IN3=0 THEN GOTO findhome ELSE 'Mine is a decoy 'Added-qtis probably isn't starting at 0000 unless this is here-Matt checkagain: GOSUB Check_Qtis DO WHILE qtis=%0000 'back up and find tape PULSOUT 13, 700 PULSOUT 12,800 GOSUB Check_Qtis PAUSE 50 LOOP GOSUB Check_Qtis IF qtis=%0000 THEN PULSOUT 13, 650 PULSOUT 12,850 PAUSE 50 GOSUB Check_Qtis GOTO checkagain ELSE M=0 FOR M=1 TO 3 PAUSE 50 PULSOUT 13, 850 PULSOUT 12,650 NEXT ENDIF M=0 FOR M=1 TO 12 PULSOUT 15,800 'turn sensor right PAUSE 50 NEXT IF qtis=%0001 OR qtis=%0011 OR qtis=%0111 THEN M=0 FOR M=1 TO 30 'turn sharp left PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 50 NEXT ELSE M=0 FOR M=1 TO 20 'turn left PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 50 NEXT ENDIF M=0 FOR M=1 TO 50 'line follow for a little bit so we dont think were home GOSUB Check_Qtis 'Get QTI states GOSUB linefollow 'WAS COMMENTED OUT NEXT GOTO findmine 'Added-Matt-Haven't tested it, but I think this is needed ENDIF '-------------------------------------------------------------------------FINDHOME (LBL) findhome: GOSUB Check_Qtis DO WHILE qtis=%0000 'back up and find tape PULSOUT 13, 650 PULSOUT 12,850 GOSUB Check_Qtis LOOP IF qtis=%0001 OR qtis=%0011 THEN M=0 FOR M=1 TO 30 'turn sharp left PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 50 NEXT ELSE M=0 FOR M=1 TO 20 'turn left PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 50 NEXT ENDIF Z=0 FOR Z=1 TO 50 'line follow for a little bit so we dont think were home GOSUB Check_Qtis 'Get QTI states GOSUB linefollow NEXT GOSUB Get_Sonar IF Dist<5 THEN 'check sonar reading is less then 5 '---------------------------------------------------------------------WASWRONG (LBL) waswrong: 'is this label needed at all?-Matt homesearch=1 'start searching for home DO GOSUB Check_Qtis 'Get QTI states GOSUB linefollow LOOP ELSE PAUSE 500 GOSUB Get_Sonar IF Dist>5 THEN 'Redundant due to ELSE Statement above?-Matt GOTO findmine ENDIF GOTO waswrong ENDIF '-------------------------------------------------------------------------CHECKHOME (SUB) checkhome: V=0 FOR V=1 TO 20 'straight past a corner PULSOUT 13, 850 PULSOUT 12, 650 PAUSE 30 NEXT GOSUB Check_Qtis IF ((qtis <> %0000) AND (homesearch=1)) THEN 'found some tape GOTO endprogram ELSEIF qtis=%0000 THEN 'go back and keep driving V=0 FOR V=1 TO 20 'back PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 30 NEXT B0=1 FOR B0=1 TO 25 PULSOUT 13, 850 PULSOUT 12, 780 PAUSE 40 NEXT ENDIF 'turn on homesearch again RETURN '-------------------------------------------------------------------------ENDPROGRAM (LBL) endprogram: T=0 FOR t=1 TO 10 PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 50 NEXT T=0 FOR T=1 TO 40 PULSOUT 14,650 'down PAUSE 50 NEXT END