My robot Darwin

'    Darwin robot picaxe 18x controller code
'    Written by jes1510
'    September 2, 2008

'    This software is for obstacle avoidance for the Darwin robot.
'    
'    It first looks for an obstacle to the front of the robot.  If an obstacle is detected then it turns
'    in a random direction.  It also monitors the button on the botton of the robot.  Once the button has
'    been pressed 3 times then the motors are stopped and the robot turns the servo to look to the left, 
'    right, and then center, taking ir readings in all 3 positions.  It picks a path based on the IR readings.


'    Setup variables and Symbols
symbol leds = 3            ' LED eyes on output pin 3
symbol irPin = 0            ' IR on input pin 0
symbol buttonPresses = b3    ' Variable to hold button press count
symbol irLeft = b0        ' IR readings on left variable
symbol irCenter = b1        ' IR readings for center variable
symbol irRight = b2        ' IR readings on right variable
symbol servoPin = 0        ' Servo is connected to output pin 0
symbol buttonPin = pin2        ' Button connected to input pin 2


'    Initialize
init:
    servo servoPin, 120          '    Initializes a servo on output pin0
    let buttonPresses = 0     '    Initialize the button count variable to 0
    high leds              '    Turn on the LED eyes
    
    

'    Main Loop
main:
    random w4    '    initialize a random number in word 4
    
    if buttonPin = 1 then inc buttonPresses endif    'increment the var if the button is pressed

    if buttonPresses = 3 then 
        let buttonPresses = 0    'reset the button press variable
        gosub stopMotors
        servopos servoPin, 75        'Far left
    
        pause 750
        readadc irPin, irLeft
        gosub blink
        servopos servoPin, 120        'center
    
        pause 750
        readadc irPin, irCenter
        gosub blink
        servopos servoPin, 165        'far right
        
        pause 750
        readadc irPin, irRight
        gosub blink
    
        servopos servoPin, 120    '    back to center
        
        debug 
    
        if irLeft > irCenter and irLeft > irRight and irLeft > 20 then gosub right    ' Turn right
        if irCenter > irLeft and irCenter > irRight and irCenter > 20 then gosub rvs    ' Backup and turn
        if irRight > irLeft and irRight > irCenter and irRight > 20 then gosub left    ' Turn left
    
        
        irLeft = 0
        irCenter = 0
        irRight = 0

        endif
'    
    
    readadc irPin, irCenter        '
    if irCenter > 45 then gosub randDir        ' Go in a random direction if there is an obstacle straight ahead
    gosub fwd
    
    goto main    ' Loop forever
    

    

fwd:        ' Drive both motors forward
    
    high 5
    low 4
    high 6
    low 7
    pause 500
    return
    
    goto main 
    
left:        ' Left motor reverse, right motor forward
    high 4
    low 5
    high 6
    low 7
    pause 1500
    goto main
    
right:    ' Right motor reverse, left motor forward
    high 5
    low 4
    high 7
    low 6    
    pause 1500    
    goto main
    

stopMotors:    ' Both motors stopped
    low 5
    low 4
    low 6
    low 7
    return
    
rvs:        ' Reverse both motors, turn in random direction
    high 4
    low 5
    high 7
    low 6
    pause 3000    
    gosub randDir
    return 

blink:    ' Blinks the LED eyes
    low leds
    pause 50
    high leds
    return

randDir:    ' pick a direction based on the random number
    if w4 > 32766 then goto right
    if w4 <= 32766 then goto left
    return