Programming Trouble with PING and IR Navigation

DrumBum535

Hello,
I've been working on a custom Boe Bot for a Engineering project for quite some time now. I have it outfited with the Crawler Kit as well as the Gripper, and on top, I added an extra bread board on standoffs to raise up the IR sensors and PING sensors. The problem I am having though is with the programing. I think I have a solid foundation so far only using the PING and IRs along with the leg servos, but for some reason, I cant figure out whats up with its motion.
Basically, my project is to have this thing navigate a simple maze, pick something up at the end of it, and then turn around and come back out the way it came.I want the program so the IRs make decisions when there is nothing in their path along with the forward judgement of the PING. The PING is not on a servo because the gripper kit is in the way, so it sits up in front. The IRs are on the Left side and Right side of the Bot. When the program starts, the Bots servos move very choppy, almost in a constant back and forth motion and it barely moves anywhere. Ive added Pauses in between different parts of the program but they only delay the choppy-ness of the servos, or give them a constant, but very weak driving force which will make it sit still on the ground.
I am by no means a programmer because this is only the second program I've ever written, but I do have a pretty good understanding of PBASIC. I have been basing it off of the program RoamingWithPING-V1.0.bs2 so far. Unfortunately when i load that program on to the Bs2, it gives a very brief movement and the program constantly restarts itself with a loud beep from the Piezo. Even when i loaded up RoamingWithIR.bs2 the program constantly restarts and the Debug window says "Program Running!" every second.
So can someone please review my current program and point things out to me so I can finally get a smooth motion from the Crawler Bot?
Thanks
**Note: All programs I listed were changed to fit my Boe Bots specs including PINs and Constants. The Project Program's PINs are all correct, but the Variables may not be needed or incorrect.
Sorry, but I had trouble attaching the code, so I just pasted it over.

' {$STAMP BS2}
' {$PBASIC 2.5}

'------------------I/O Definitions------------------
Ping      PIN      0
Piezo     PIN      4
ColorPal  PIN      6
IRsensors  PIN      15
RightIRREC   PIN      11
LeftIRREC    PIN      10
LeftServo    PIN      13
RightServo   PIN      12
Gripper      PIN      14
'---------------------CON-------------------------------
baud CON 119 + 32768

'--------------------VARIABLES------------------------
irDetectLeft    VAR     Bit         'VAR for Left IR Input
irDetectRight   VAR     Bit         'VAR for Right IR Input
pulseCount      VAR     Byte        'Used for measuring turns
distance        VAR     Word        'Current Distance objective
counter         VAR     Word        'PING cycle counter
task            VAR     Nib         'Current task
'----------------------INTIALIZATION---------------
'DO
  'FREQOUT Piezo, 500, 3000
  'FREQOUT Piezo, 500, 4500
  'PAUSE 300
'LOOP
'-------------------MAIN---------------------------
Main:
 
  FREQOUT IRsensors, 15, 40500            'Emit 40500Hz to Left/Right IR
    irDetectLeft= IN10
  FREQOUT IRsensors, 15, 40500
    irDetectRight= IN11

  counter = counter + 1                                  ' Increment Passive Counter
  IF counter > 10 THEN
   GOSUB Ping_Out                                        ' Activate PING)))
  ENDIF
  'IF (distance > 10) THEN               ' Is Object Farther Than 30 cm?
    ' GOSUB Keep_Straight                 ' If Yes Go Forward
    'ELSEIF (distance < 10) THEN
  'ENDIF                                  'Allow FOR IRs TO make descision
  IF (irDetectLeft = 0) AND (irDetectRight = 0) AND (distance > 10)  THEN         'VARYING distance, calculate in Maze later
    GOSUB Keep_Straight                                                           ' Objects Detected via IR, Forward
  ELSEIF (irDetectLeft = 0) AND (distance > 10) THEN
    GOSUB Keep_Straight                                                           ' Object Detected via IR Left
  ELSEIF (irDetectRight = 0) AND (distance > 10) THEN
    GOSUB Keep_Straight                                                           ' Object Detected via IR Right
  ENDIF
  IF (irDetectLeft= 1) AND (irDetectRight= 0) AND (distance < 10) THEN
    GOSUB Turn_Left                                                        'No wall next to LeftIR, Left Turn
  ELSEIF (irDetectLeft = 1) AND (distance < 10) THEN
    GOSUB Turn_Left
  ELSEIF (irDetectRight = 0) AND (distance < 10) THEN
    GOSUB Turn_Left
  ENDIF
  IF (irDetectLeft = 0) AND (irDetectRight = 1) AND (distance < 10) THEN
    GOSUB Turn_Right                                                         'No wall next to RightIR, Turn Right
  ELSEIF (irDetectLeft = 0) AND (distance < 10) THEN
    GOSUB Turn_Right
  ELSEIF (irDetectRight = 1) AND (distance < 10) THEN
    GOSUB Turn_Right
  ENDIF
  IF (irDetectLeft = 1) AND (irDetectRight = 1) AND (distance > 10) THEN
    GOSUB Keep_Straight
  ELSEIF (irDetectLeft = 1) AND (distance > 10) THEN
   GOSUB Keep_Straight
  ELSEIF (irDetectRight = 1) AND (distance > 10) THEN
    GOSUB Keep_Straight
  ENDIF
  'counter = counter + 1                 ' Increment Passive Counter
  'IF counter > 10 THEN
  'GOSUB Ping_Out                         ' Activate PING)))
  'ENDIF
  'IF (distance > 10) THEN               ' Is Object Farther Than 30 cm?
   ' GOSUB Keep_Straight                 ' If Yes Go Forward
 'ELSEIF (distance < 10) THEN
  'ENDIF                                 'Allow for IRs to make descision
 
 

'-------------------SUBROUTINES-----------------------------------------
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
Keep_Straight:
  PULSOUT LeftServo, 850                ' Left Servo Forward Pulse Value
  PULSOUT RightServo, 650               ' Right Servo Forward Pulse Value
  'PAUSE 10                                    ' Refresh Delay
  RETURN
Turn_Left:
   FOR pulseCount = 0 TO 10               ' Number Of Pulses To Turn, a little more than 45 degrees
    PULSOUT LeftServo, 650              ' Left Servo Left Pulse Value
    PULSOUT RightServo, 650             ' Right Servo Left Pulse Value
    'PAUSE 10                            ' Refresh Delay
  NEXT
  RETURN
Turn_Right:
  FOR pulseCount = 0 TO 10               ' Number Of Pulses To Turn, a little more than 45 degrees
    PULSOUT LeftServo, 850              ' Left Servo Right Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Right Pulse Value
    'PAUSE 10                            ' Refresh Delay
  NEXT
  RETURN
Back_Up:
  FOR pulseCount = 0 TO 40              ' Number Of Pulses To Backup
    PULSOUT LeftServo, 650              ' Left Servo Backup Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Backup Pulse Value
    'PAUSE 10                            ' Refresh Delay
  NEXT
  RETURN
Turn_Around:                            'About 180 degree Turn Around to the Right
  FOR pulseCount = 0 TO 170
    PULSOUT LeftServo, 850
    PULSOUT RightServo, 850
    'PAUSE 10
  NEXT
  RETURN
 

Tom C

DrumBum535,
For a start, your program may be so long that it is not refreshing the wheel servos every 20ms.
You might want to consider off loading the wheel servo refresh to a servo pal which will refresh the wheel servos every 20ms irrespective of what your program loop time is.
Just a thought.
Regards,
TCIII

Mike Green

To elaborate on Tom C's suggestion ... R/C Servos, like those on the BoeBot, require a control pulse about 50 times a second. If there's significantly more than 20ms between pulses, the servo shuts down until the next pulse comes in and the movement becomes choppy. If there's significantly less than 20ms between pulses, the servo action can also become choppy. In a complex Stamp program like yours, it's very difficult to ensure that these pulses are issued regularly. The ServoPal is designed to help in these situations. You can also use something like a Propeller Servo Controller which also offloads the task from the Stamp. You can use the information on Stamp execution time here via the "app-notes" link to estimate the execution time of your program between PULSOUTs and possibly rearrange some stuff to smooth that out. Note that it's ok to let the servos power down between completed movements. That's only an issue if you're using a servo such that it has to hold its position and just gear friction won't do that well enough.

erco

DrumBum535 wrote: »

. Unfortunately when i load that program on to the Bs2, it gives a very brief movement and the program constantly restarts itself with a loud beep from the Piezo. Even when i loaded up RoamingWithIR.bs2 the program constantly restarts and the Debug window says "Program Running!" every second.

If your program is resetting, your batteries are likely weak and need to be replaced. But your piezo beeper FREQOUT commands are commented out in the code you posted, so I suspect that's not the exact code you're currently running.

Per prior replies, you need to refresh your servo pulsouts regularly. Your program may be long enough so that you don't need to add the traditional "PAUSE 20". Any DEBUG statements will slow crash your servo refresh and cause the jittery motion you describe.

BTW, the goal you describe isn't particularly easy. Keep plugging and advise of your progress!

SRLM

DrumBum535 wrote: »

I've been working on a custom Boe Bot for a Engineering project for quite some time now...The problem I am having though is with the programing...I am by no means a programmer because this is only the second program I've ever written...

Is this for a mechanical engineering senior design?

GordonMcComb

>Even when i loaded up RoamingWithIR.bs2 the program constantly restarts and the Debug window says "Program Running!" every second.

I'm not sure how the BS2 tokenizer handles short-circuiting IF statements, or how many cycles each evaluation takes up, but as Erco notes in his post, if you're getting a restart with the example program it probably means your power source just isn't up to the task of running servos.

How are you powering the servos? A set of 4 AA's should be okay. Depending on the vintage of your BOE, be sure to set the power block jumper for the servos to Vin, not Vdd. Use FRESH alkaline cells. Don't use a wall adapter, even if you think it delivers enough current (it probably doesn't). If you are getting tired of replacing the batteries, buy a set of rechargeable alkalines (and suitable recharger). These put out 1.5 volts like the non-rechargeable kind. Alternatively, you can use 1.2V rechargeable cells if you get the BOE-Boost 1-cell addon. Parallax sells it.

Start there first, using the example code in the BOE-Bot text. Then once you prove your robot is working properly and not resetting constantly, you can look to ways to trim your code to accommodate the extra cycles from the IF tests, and properly coding for the servo refresh.

-- Gordon

DrumBum535

erco wrote: »

But your piezo beeper FREQOUT commands are commented out in the code you posted, so I suspect that's not the exact code you're currently running.

Per prior replies, you need to refresh your servo pulsouts regularly. Your program may be long enough so that you don't need to add the traditional "PAUSE 20". Any DEBUG statements will slow crash your servo refresh and cause the jittery motion you describe.

BTW, the goal you describe isn't particularly easy. Keep plugging and advise of your progress!

The code I was reffering to was the example code RoamingWithPING-V1.0 with the Initialization beeps. My Codes sounds Loop to mimic a Siren like a Search and Rescue Bot ( the name of my project). lol
And as for progress I will hopefully be able to get some pictures up here soon.

But as for needing to refresh the servo pulses, where would I put those instead of having Pauses?

DrumBum535

SRLM wrote: »

Is this for a mechanical engineering senior design?

No, I'm actually apart of an Engineering Program based out of a Defense Contractor for the military.

DrumBum535

GordonMcComb wrote: »

How are you powering the servos? A set of 4 AA's should be okay. Depending on the vintage of your BOE, be sure to set the power block jumper for the servos to Vin, not Vdd.....Start there first, using the example code in the BOE-Bot text. Then once you prove your robot is working properly and not resetting constantly, you can look to ways to trim your code to accommodate the extra cycles from the IF tests, and properly coding for the servo refresh.

I am using the standard 4 AA batteries, but the Jumper is currently on the side closest to label Vdd. Problem?? Ill change this if it is. I have tried to cut down the code into parts as well to diagnose but havent gotten satisfactory results, due in part to the Servo movement. Still not to sure how to allow the servos to refresh though...

erco

Yes, Vin is your first choice for servo power. Vdd is regulated 5V, which may be choking your voltage regulator. Servos can demand as lot of current.

DrumBum535

Gentlemen, I have fixed the problem! The motion of my Search and Rescue Crawler Bot is superb!!!!! Heres the New Program, used to navigate a maze with IRs on the left and right sides, and a Servo-less PING up front:

' {$STAMP BS2}
' {$PBASIC 2.5}

'------------------I/O Definitions------------------
Ping      PIN      0
Piezo     PIN      4
ColorPal  PIN      6
IRsensors  PIN      15
RightIRREC   PIN      11
LeftIRREC    PIN      10
LeftServo    PIN      13
RightServo   PIN      12
Gripper      PIN      14
'---------------------CON-------------------------------
baud CON 119 + 32768

'--------------------VARIABLES------------------------
irDetectLeft    VAR     Bit         'VAR for Left IR Input
irDetectRight   VAR     Bit         'VAR for Right IR Input
pulseCount      VAR     Byte        'Used for measuring turns
distance        VAR     Word        'Current Distance objective
counter         VAR     Word        'PING cycle counter
task            VAR     Nib         'Current task
'----------------------INITIALIZATION---------------
'DO
  'FREQOUT Piezo, 500, 3000
  'FREQOUT Piezo, 500, 4500
  'PAUSE 300
'LOOP
'-------------------MAIN---------------------------
Main:
DO

  FREQOUT IRsensors, 15, 41500            'Emit 41500Hz to Left/Right IR
    irDetectLeft= IN10
  FREQOUT IRsensors, 15, 41500
    irDetectRight= IN11

  counter = counter + 1                                  ' Increment Passive Counter
  IF counter > 1 THEN
   GOSUB Ping_Out                                        ' Activate PING)))
  ENDIF
  'IF (distance > 20) THEN               ' Is Object Farther Than 20mm?
    ' GOSUB Keep_Straight                 ' If Yes Go Forward
    'ELSEIF (distance < 20) THEN
  'ENDIF                                  'Allow FOR IRs TO make descision
  IF (irDetectLeft = 0) AND (irDetectRight = 0) AND (distance > 20)  THEN         'VARYING distance, calculate in Maze later
    GOSUB Keep_Straight                                                           ' Objects Detected via IR, Forward
  ELSEIF (distance < 20) THEN
    GOSUB Hold_Position
  ENDIF
  IF (irDetectLeft = 1) AND (irDetectRight = 0) AND (distance < 20) THEN
    GOSUB Turn_Left                                                        'No wall next to LeftIR, Left Turn
  ELSEIF (irDetectLeft = 1) AND (irDetectRight = 0) AND (distance > 20) THEN
    GOSUB Back_Up
  ENDIF
  IF (irDetectLeft = 0) AND (irDetectRight = 1) AND (distance < 20) THEN
    GOSUB Turn_Right                                                         'No wall next to RightIR, Turn Right
  ELSEIF (irDetectLeft = 1) AND (irDetectRight = 0) AND (distance > 20) THEN
    GOSUB Back_Up
  ENDIF
  IF (irDetectLeft = 1) AND (irDetectRight = 1) AND (distance > 20) THEN
    GOSUB Keep_Straight
  ELSEIF (distance < 20) THEN
   GOSUB Hold_Position
  ENDIF
  LOOP
 
'-------------------SUBROUTINES-----------------------------------------
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
Keep_Straight:
  FOR pulseCount = 0 TO 20
    PULSOUT LeftServo, 850                ' Left Servo Forward Pulse Value
    PULSOUT RightServo, 650               ' Right Servo Forward Pulse Value
    PAUSE 20
  NEXT                                    ' Refresh Delay
  RETURN
Turn_Left:
   FOR pulseCount = 0 TO 20               ' Number Of Pulses To Turn, a little more than 45 degrees
    PULSOUT LeftServo, 650              ' Left Servo Left Pulse Value
    PULSOUT RightServo, 650             ' Right Servo Left Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN
Turn_Right:
  FOR pulseCount = 0 TO 20               ' Number Of Pulses To Turn, a little more than 45 degrees
    PULSOUT LeftServo, 850              ' Left Servo Right Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Right Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN
Back_Up:
  FOR pulseCount = 0 TO 10              ' Number Of Pulses To Backup
    PULSOUT LeftServo, 650              ' Left Servo Backup Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Backup Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN
Turn_Around:                            'About 180 degree Turn Around to the Right
  FOR pulseCount = 0 TO 170
    PULSOUT LeftServo, 850
    PULSOUT RightServo, 850
    PAUSE 20
  NEXT
  RETURN
Hold_Position:
  FOR pulseCount = 0 TO 5
    PULSOUT LeftServo, 750
    PULSOUT RightServo, 750
    PAUSE 20
  NEXT
  RETURN

Edit it as you please, theres still a lot to be done with this project. I need to incorporate a ColorPal to identify a "Victim" and also have the Gripper Pick up this "Victim". Any Suggestions?

DrumBum535

Heres a picture of it, the ColorPal is just wraped around the back, you can see the loose wires on the side. It wont stay like that though!

erco

XLNT, so what was the big change to make it work properly?

DrumBum535

Im pretty sure its just because I kept in the PAUSE 20 on the turns and movements in the Subroutines, and then made the PulseCount 1 TO 20 intstead of to 10. It smoothed out the motion a lot and removed some unecessary ELSEIFS...now I've added a color identification portion to the program so the gripper will close when it reads a certain Red-Green-Blue color.
Question:
Is there a way to get the Gripper to stay closed once it closes? I cant seem to figure it out. It just pulses closed every 10 counts and then lets up the power for a few seconds. I need it to hold on to something once the ColorPal recognizes it.

erco

The Gripper kit uses a standard servo, which needs a steady stream of pulsouts every 20 mS just like the continuous rotation servos on your wheels do. Of course, with that servo, the pulsout value controls position, not speed. If your gripper is open or there's no load on the gripper, you can stop sending pulsouts and your servo will just shut down and tend to stay where it is. But if you need to keep the grip on it, you need to keep sending pulsouts.

A few options for you: the ServoPal Mike already suggested, which can offloads the overhead of continuous pulsing for two servos, or change your servo to a high torque/lower speed type. It will have lower gearing, which won't backdrive as easily and will keep a tighter grip even if you stop sending pulsouts to it.

DrumBum535

So youre saying if I did a subroutine:

Gripper_Close:
FOR pulseCount 1 TO 20
PULSOUT 14, 650
PAUSE 20
NEXT
RETURN

This would hold the gripper shut?

erco

Well, that routine will suddenly move the gripper towards position 650 for 20 pulses, approximately 420 milliseconds. Should close in that time, assuming position 650 is "closed". But after the subroutine ends returns, the servo is off (there is no more power applied to the servo), thus no more torque to hold the gripper closed. So it may drop whatever it had a grip on. Without power, the servo can be "back-driven", or forced open.

One workaround is to add a spring or rubber band to the gripper to help close it. That way, the gripper works harder to open up (stretching the spring/band) but the spring keeps some grip on even when the servo is off.

DrumBum535

New Programming issue:

So ive been pretty successful with programming so far, but ive come to a problem with what i want the Boe Bot to do.
Currently, I have the bot navigate a maze with the PING and IRs, and if the bot gets too close to a wall, it is set to BACK UP for a few counts and turn to the RIGHT for another few counts. The problem is that the maze has only one way in and the bot needs to turn around to leave the maze. When it does this then, I need the bot to BACK UP and turn LEFT for the same amount of counts instead of turning RIGHT which would send it back the opposite direction.......I really cant wrap my brain around this one and its been bothering me! Thanks for the help though.

**Note that Subroutine: Navigation2 was just an attempt to fix this problem and anything reamed out is unecessary at this point....unless you think it could fix the problem. :blank:

' {$STAMP BS2}
' {$PBASIC 2.5}

'------------------I/O Definitions------------------
Ping      PIN      0
Piezo     PIN      4
ColorPal  PIN      6
IRsensors  PIN      15
RightIRREC   PIN      11
LeftIRREC    PIN      10
LeftServo    PIN      13
RightServo   PIN      12
Gripper      PIN      14
'---------------------CON-------------------------------
baud CON 119 + 32768

'--------------------VARIABLES------------------------
irDetectLeft    VAR     Bit         'VAR for Left IR Input
irDetectRight   VAR     Bit         'VAR for Right IR Input
pulseCount      VAR     Byte        'Used for measuring turns
distance        VAR     Word        'Current Distance objective
counter         VAR     Word        'PING cycle counter
task            VAR     Nib         'Current task
red             VAR     Word          ' Received RGB values from ColorPAL.
grn             VAR     Word
blu             VAR     Word
'----------------------INITIALIZATION---------------
'DO
  'FREQOUT Piezo, 500, 3000
  'FREQOUT Piezo, 500, 4500
  'PAUSE 300
'LOOP
GOSUB Reset
GOSUB Gripper_Close
'-------------------MAIN---------------------------
Main:
DO
  GOSUB Navigation
 
  GOSUB color_check
  'DEBUG "Red: ", DEC4 red, CR
  'DEBUG "Green: ", DEC4 grn, CR
  'DEBUG "Blue: ", DEC4 blu, CR
  'PAUSE 20

  IF (red >= 15) AND (grn >= 5) AND (blu >= 12) THEN
    'GOSUB Gripper_Open
  'ELSEIF (red < 15) AND (grn < 5) AND (blu < 12) THEN
     GOSUB Gripper_Close
  ENDIF
'GOSUB Navigation2
LOOP

'-------------------SUBROUTINES-----------------------------------------
Navigation:
  FREQOUT IRsensors, 15, 37500            'Emit 41500Hz to Left/Right IR
    irDetectLeft= IN10
  FREQOUT IRsensors, 15, 37500
    irDetectRight= IN11

  counter = counter + 1                                  ' Increment Passive Counter
  IF counter > 1 THEN
   GOSUB Ping_Out                                        ' Activate PING)))
  ENDIF

  IF (irDetectLeft = 0) AND (irDetectRight = 0) AND (distance > 20)  THEN         'VARYING distance, calculate in Maze later
    GOSUB Keep_Straight                                                           ' Objects Detected via IR, Forward
  ELSEIF (distance < 20) THEN
    GOSUB Back_Up
  ENDIF
  IF (irDetectLeft = 0) AND (irDetectRight = 1)  THEN
    GOSUB Turn_Right                                                     'No wall NEXT TO RightIR, Turn Right
  ELSEIF (irDetectLeft = 1) AND (irDetectRight = 0) THEN
    GOSUB Turn_Left
  ENDIF
  IF (irDetectLeft = 1) AND (irDetectRight = 1) AND (distance > 20) THEN
    GOSUB Keep_Straight
  ELSEIF (distance < 20) THEN
   GOSUB Back_Up
   GOSUB Turn_Right
  ENDIF
RETURN

Navigation2:
  FREQOUT IRsensors, 15, 37500            'Emit 41500Hz to Left/Right IR
    irDetectLeft= IN10
  FREQOUT IRsensors, 15, 37500
    irDetectRight= IN11

  counter = counter + 1                                  ' Increment Passive Counter
  IF counter > 1 THEN
   GOSUB Ping_Out                                        ' Activate PING)))
  ENDIF

  IF (irDetectLeft = 0) AND (irDetectRight = 0) AND (distance > 20)  THEN         'VARYING distance, calculate in Maze later
    GOSUB Keep_Straight                                                           ' Objects Detected via IR, Forward
  ELSEIF (distance < 20) THEN
    GOSUB Back_Up
  ENDIF
  IF (irDetectLeft = 1) AND (irDetectRight = 0) AND (distance < 20) THEN
    GOSUB Turn_Left                                                        'No wall next to LeftIR, Left Turn
  ELSEIF (irDetectLeft = 1) AND (irDetectRight = 0) AND (distance > 20) THEN
    GOSUB Turn_Left
  ENDIF
  IF (irDetectLeft = 0) AND (irDetectRight = 1) AND (distance < 20) THEN
    GOSUB Turn_Right                                                         'No wall next to RightIR, Turn Right
  ELSEIF (irDetectLeft = 0) AND (irDetectRight = 1) AND (distance > 20) THEN
    GOSUB  Turn_Right
  ENDIF
  IF (irDetectLeft = 1) AND (irDetectRight = 1) AND (distance > 20) THEN
    GOSUB Keep_Straight
  ELSEIF (distance < 20) THEN
   GOSUB Back_Up
   GOSUB Turn_Left
  ENDIF
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
Keep_Straight:
  FOR pulseCount = 0 TO 20
    PULSOUT LeftServo, 850                ' Left Servo Forward Pulse Value
    PULSOUT RightServo, 650               ' Right Servo Forward Pulse Value
    PAUSE 20
  NEXT                                    ' Refresh Delay
  RETURN
Turn_Left:
   FOR pulseCount = 0 TO 20               ' Number Of Pulses To Turn, a little more than 45 degrees
    PULSOUT LeftServo, 650              ' Left Servo Left Pulse Value
    PULSOUT RightServo, 650             ' Right Servo Left Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN
Turn_Right:
  FOR pulseCount = 0 TO 20               ' Number Of Pulses To Turn, a little more than 45 degrees
    PULSOUT LeftServo, 850              ' Left Servo Right Pulse Value
    PULSOUT RightServo, 850             ' Right Servo Right Pulse Value
    PAUSE 20                            ' Refresh Delay
  NEXT
  RETURN
Back_Up:
  FOR pulseCount = 0 TO 10              ' Number Of Pulses To Backup
    PULSOUT LeftServo, 650              ' Left Servo Backup Pulse Value
    PULSOUT RightServo, 850              ' Right Servo Backup Pulse Value
    PAUSE 20                             ' Refresh Delay
  NEXT
  RETURN

Turn_Around:                            'About 180 degree Turn Around to the Right
  FOR pulseCount = 0 TO 170
    PULSOUT LeftServo, 850
    PULSOUT RightServo, 850
    PAUSE 20
  NEXT
  RETURN
Hold_Position:
  FOR pulseCount = 0 TO 5
    PULSOUT LeftServo, 750
    PULSOUT RightServo, 750
    PAUSE 20
  NEXT
  RETURN
Gripper_Open:
  FOR pulseCount = 0 TO 100
   PULSOUT Gripper, 750
   PAUSE 20
  NEXT
  RETURN
Gripper_Close:
  FOR pulseCount = 0 TO 100
    PULSOUT Gripper, 1000
    PAUSE 20
  NEXT
  RETURN
Reset:
  LOW ColorPal                   'Pull sio low to eliminate any residual charge.
  INPUT ColorPal                 'Return pin to input.
  DO UNTIL ColorPal : LOOP       'Wait for pin to be pulled high by ColorPAL.
  LOW ColorPal                   'Pull pin low.
  PAUSE 80                  'Keep low for 80ms to enter Direct mode.
  INPUT ColorPal                 'Return pin to input.
  PAUSE 10                  'Pause another 10ms
  RETURN
color_check:
  SEROUT ColorPal, baud, ["= m !"]
  SERIN ColorPal, baud, [HEX3 red, HEX3 grn, HEX3 blu] ' Receive RGB data back.
  RETURN