' ========================================================================= ' ' File...... followme.bs2 ' Purpose... Roam And Follow the Closest Object ' Author.... Parallax, Inc. and St. Norbert College CS 225 ' Updated... 11-23-2008 ' ' {$STAMP BS2} ' {$PBASIC 2.5} ' ' ========================================================================= ' ' ' -----[ I/O Definitions ]------------------------------------------------- Piezo PIN 4 ' Piezo Speaker RightServo PIN 12 ' Right Servo LeftServo PIN 13 ' Left Servo PingServo PIN 14 ' PING))) Servo Ping PIN 15 ' PING))) Sensor ' -----[ Variables ]------------------------------------------------------- irDetectLeft VAR Bit ' Variable For Left IR Input irDetectRight VAR Bit ' Variable For Right IR Input pulseCount VAR Byte ' Used For Measuring Turns distance VAR Word ' Current Distance Of Object oldDistance VAR Word ' Old Distance Value counter VAR Word ' PING))) Cycle Counter task VAR Nib ' Current Task ' -----[ Initialization ]-------------------------------------------------- FREQOUT Piezo, 2000, 3000 ' Signal Program Start/Reset counter = 10 ' -----[ Program Code ]---------------------------------------------------- Main: DO GOSUB Ping_Out ' Get first distance IF (distance > 15) THEN 'If the object in front of boebot is greater than 15 cm DO GOSUB Forward_Pulse ' move forward GOSUB Ping_Out ' check distance IF (distance <= 15) THEN ' until distance is less than 15 cm FREQOUT Piezo, 500, 1500 DO PAUSE 10 ' wait GOSUB Ping_Out ' get distance IF (distance > 15) THEN ' until object is greater than 15 cm away PAUSE 1000 ' wait to find where object moved GOSUB Ping_Around ' find the closest object ' GOSUB Forward_Pulse ' EXIT ENDIF LOOP ENDIF LOOP ENDIF ' IF (distance > 15) THEN ' Is Object Farther Than 30 cm? ' GOSUB Ping_Around 'ELSE 'GOSUB Forward_Pulse ' Otherwise Scan For Clear Path LOOP ' -----[ Subroutines ]----------------------------------------------------- ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT MOVE FORWARD * ' * WHILE THE PING))) IS FACING FORWARD. * ' ************************************************************************* Forward_Pulse: ' Send A Single Forward Pulse PULSOUT PingServo, 750 ' Ping Servo Forward Pulse Value PULSOUT LeftServo, 890 ' Left Servo Forward Pulse Value PULSOUT RightServo, 610 ' Right Servo Forward Pulse Value PAUSE 20 ' Refresh Delay RETURN ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT TURN LEFT 90 * ' * DEGREES. USE THE SAME VALUE AS ABOVE FOR THE PING))) BRACKET SERVO. * ' ************************************************************************* Turn_Left: ' Left Turn, About 45 Degrees FOR pulseCount = 0 TO 7 ' Number Of Pulses To Turn PULSOUT PingServo, 750 ' Ping Servo Forward Pulse Value PULSOUT LeftServo, 650 ' Left Servo Left Pulse Value PULSOUT RightServo, 650 ' Right Servo Left Pulse Value PAUSE 20 ' Refresh Delay NEXT RETURN ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT TURN RIGHT 90 * ' * DEGREES. USE THE SAME VALUE AS ABOVE FOR THE PING))) BRACKET SERVO. * ' ************************************************************************* Turn_Right: ' Right Turn, About 45 Degrees FOR pulseCount = 0 TO 7 ' Number Of Pulses To Turn PULSOUT PingServo, 750 ' Ping Servo Forward Pulse Value PULSOUT LeftServo, 850 ' Left Servo Right Pulse Value PULSOUT RightServo, 850 ' Right Servo Right Pulse Value PAUSE 20 ' Refresh Delay NEXT RETURN ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUES TO MAKE YOUR BOE-BOT MOVE BACKWARD * ' * WHILE THE PING))) IS FACING FORWARD. * ' ************************************************************************* Back_Up: ' Back Up FOR pulseCount = 0 TO 40 ' Number Of Pulses To Backup PULSOUT PingServo, 750 ' Ping Servo Forward Pulse Value PULSOUT LeftServo, 650 ' Left Servo Backup Pulse Value PULSOUT RightServo, 850 ' Right Servo Backup Pulse Value PAUSE 20 ' Refresh Delay NEXT RETURN Ping_Out: ' PING))) counter = 0 ' Reset Passive Delay Counter LOW Ping ' Force PING))) Line Low PULSOUT Ping, 5 ' Activate PING))) Pulse PULSIN Ping, 1, distance ' Receive Return Pulse distance = distance ** 2257 ' Calculate Distance RETURN Ping_Around: ' Start 180 Degree Pan-Scan 'counter = 0 ' Reset Passive Delay Counter oldDistance = 60 ' Current Old Distance Values task = 0 ' Current Task Priority ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUE TO MAKE YOUR PING))) * ' * TURN 90 DEGREES LEFT. * ' ************************************************************************* FOR pulseCount = 0 TO 20 ' Number Of Pulses To Spin LOW Ping ' Force PING))) Line Low PULSOUT PingServo, 1085 ' Ping Servo 90 Left Pulse Value PULSOUT Ping, 5 ' Activate PING))) PULSIN Ping, 1, distance ' Receive Distance Value PAUSE 20 ' Refresh Delay NEXT distance = distance ** 2257 ' Calculate Distance In cm IF distance < oldDistance THEN ' Is distance > Last Clear Path oldDistance = distance ' Update oldDistance Value task = 1 ENDIF ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUE TO MAKE YOUR PING))) * ' * TURN 45 DEGREES LEFT. * ' ************************************************************************* FOR pulseCount = 0 TO 20 ' Number Of Pulses To Spin LOW Ping ' Force PING))) Line Low PULSOUT PingServo, 850 ' Ping Servo 45 Left Pulse Value PULSOUT Ping, 5 ' Activate PING))) PULSIN Ping, 1, distance ' Receive Distance Value PAUSE 20 ' Refresh Delay NEXT distance = distance ** 2257 ' Calculate Distance In cm IF distance < oldDistance THEN ' Is distance > Last Clear Path oldDistance = distance ' Update oldDistance Value task = 2 ENDIF ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUE TO MAKE YOUR PING))) * ' * TURN 45 DEGREES RIGHT. * ' ************************************************************************* FOR pulseCount = 0 TO 20 ' Number Of Pulses To Spin LOW Ping ' Force PING))) Line Low PULSOUT PingServo, 400 ' Ping Servo 45 Right Pulse Value PULSOUT Ping, 5 ' Activate PING))) PULSIN Ping, 1, distance ' Receive Distance Value PAUSE 20 ' Refresh Delay NEXT distance = distance ** 2257 ' Calculate Distance In cm IF distance < oldDistance THEN ' Is distance > Last Clear Path oldDistance = distance ' Update oldDistance Value task = 3 ENDIF ' ************************************************************************* ' * USE THE APPROPRIATE PULSOUT VALUE TO MAKE YOUR PING))) * ' * TURN 90 DEGREES RIGHT. * ' ************************************************************************* FOR pulseCount = 0 TO 20 ' Number Of Pulses To Spin LOW Ping ' Force PING))) Line Low PULSOUT PingServo, 225 ' Ping Servo 90 Right Pulse Value PULSOUT Ping, 5 ' Activate PING))) PULSIN Ping, 1, distance ' Receive Distance Value PAUSE 20 ' Refresh Delay NEXT distance = distance ** 2257 ' Calculate Distance In cm IF distance < oldDistance THEN ' Is distance > Last Clear Path oldDistance = distance ' Update oldDistance Value task = 4 ENDIF ON task GOSUB Task0, Task1, Task2, Task3, Task4 distance = 50 ' Prevent Scan From Looping RETURN 'End Ping_Around Task0: DEBUG "TASK0", CR ' Forward Was Clearest Path GOSUB Forward_Pulse RETURN Task1: ' 90 Degrees Left Was Clearest DEBUG "TASK1", CR GOSUB Turn_Left GOSUB Turn_Left RETURN Task2: ' 45 Degrees Left Was Clearest DEBUG "TASK2", CR GOSUB Turn_Left RETURN Task3: ' 45 Degrees Right Was Clearest DEBUG "TASK3", CR GOSUB Turn_Right RETURN Task4: ' 90 Degrees Right Was Clearest DEBUG "TASK4", CR GOSUB Turn_Right GOSUB Turn_Right RETURN