The robotic platform will be placed dead center of the workspace. Then robot is then required to perform a 360° rotation to scan its entire environment and then proceed to locate and move towards the farthest object stopping 2 inches in front of the object. The robot must then use it current location as its origin and perform the exact same task as before. The robot should be able to differentiate the wall from objects and not recognize a segment of the wall as obstacle.
' {$STAMP BS2}
' {$PBASIC 2.5}
' -----[ Variables ]----------------------------------------------------------
time VAR Word
DR1 VAR Word
DR2 VAR Word
DR3 VAR Word
DR4 VAR Word
counter VAR Nib
int VAR Word
Ping_Pin CON 15
InConstant CON 890
inDistance VAR Word
i VAR Nib
R1 CON 13
R2 CON 26
R3 CON 39
R4 CON 42
rot VAR Word
Main:
PAUSE 2000
GOSUB ir_direction1
PAUSE 1000
GOSUB ir_direction2
PAUSE 1000
GOSUB ir_direction3
PAUSE 1000
GOSUB ir_direction4
PAUSE 1000
GOSUB Check
'DEBUG CR, "DR1 = ", DR1
'DEBUG CR, "DR2 = ", DR2
GOSUB Check2
END
ir_direction1:
DR1 = 0
PULSOUT 15,5
PULSIN 15,1, DR1
DEBUG HOME
DR1 = DR1 ** 890
DEBUG CR, "Distance1 = ", DEC4 DR1, " in"
'DEBUG CR, "DR1 = ", DR1
PAUSE 1000
RETURN
ir_direction2:
FOR counter = 0 TO 13 ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
DR2 = 0
PULSOUT 15,5
PULSIN 15,1, DR2
DEBUG HOME
DR2 = DR2 ** 890
DEBUG CR, "Distance2 = ", DEC4 DR2, " in"
'DEBUG CR, "DR2 = ", DR2
PAUSE 1000
RETURN
ir_direction3:
FOR counter = 0 TO 13 ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
DR2 = 0
PULSOUT 15,5
PULSIN 15,1, DR3
DEBUG HOME
DR3 = DR3 ** 890
DEBUG CR, "Distance3 = ", DEC4 DR3, " in"
'DEBUG CR, "DR3 = ", DR3
PAUSE 1000
RETURN
ir_direction4:
FOR counter = 0 TO 13 ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
DR2 = 0
PULSOUT 15,5
PULSIN 15,1, DR4
DEBUG HOME
DR4 = DR4 ** 890
DEBUG CR, "Distance4 = ", DEC4 DR4, " in"
'DEBUG CR, "DR2 = ", DR2
PAUSE 1000
RETURN
'_________________________________________________________________'
Check:
PAUSE 2000
IF (DR1 > DR2) AND (DR1 > DR3) AND (DR1 > DR4) THEN
PAUSE 2000
DEBUG "this is DR1"
PAUSE 2000
'rot = 13
PAUSE 2000
FOR counter = 0 TO 13 ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
PAUSE 2000
'GOSUB Check2
ELSEIF(DR2 > DR1) AND (DR2 > DR3) AND (DR2 > DR4) THEN
'FOR counter = 0 TO 23 ' LEFT ABOUT 90 DEGREES
'PULSOUT 13, 650
'PULSOUT 12, 650
'PAUSE 200
'NEXT
'GOSUB Check2
DEBUG "this is DR2"
PAUSE 2000
'rot = 26
PAUSE 2000
FOR counter = 0 TO 26 ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
'GOSUB Check2
ELSEIF(DR3 > DR1) AND (DR3 > DR2) AND (DR3 > DR4) THEN
'PAUSE 2000
'FOR counter = 0 TO 45 ' LEFT ABOUT 90 DEGREES
'PULSOUT 13, 650
'PULSOUT 12, 650
'PAUSE 200
'NEXT
DEBUG "this is DR3"
PAUSE 2000
'rot = 39
FOR counter = 0 TO 39 ' LEFT ABOUT 90 DEGREES
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
'GOSUB Check2
PAUSE 2000
ENDIF
PAUSE 2000
'GOSUB Check2
DEBUG "this is DR4"
PAUSE 2000
'DEBUG " niether"
PAUSE 2000
'GOSUB Check2
RETURN
Check2:
'FREQOUT 4, 2000, 3000
counter = 0
DO UNTIL (int < 4)
GOSUB Readpulse
PAUSE 2000
'GOSUB forward
GOSUB forward
PAUSE 2000
LOOP
PAUSE 1000
Readpulse:
'FREQOUT 4, 2000, 3000 ' Signal program start/reset
PULSOUT Ping_Pin, 5 ' Send short pulse to Ping
PULSIN Ping_Pin, 1, int ' Wait for echo
int = inConstant ** int ' Convert to inches
DEBUG CR, DEC int ' Display result
PAUSE 200 ' Short delay until next read
RETURN
forward:
FOR counter = 1 TO 2
PULSOUT 13, 850
PULSOUT 12,650
PAUSE 20
NEXT
RETURN
|
|
This Thread