' Declare variables
ls VAR BYTE 'Left servomotor pulse width adjustment
rs VAR BYTE 'Right servomotor pulse width adjustment
cs VAR BYTE 'Center servomotor pulse width adjustment
ct VAR BYTE 'Count
b0 VAR BYTE 'Count
b1 VAR BYTE 'Count
'Define variables
ls = 150
rs = 150
cs = 150
IF PORTB.7 = 1 Then hold
IF PORTB.6 = 1 Then cs = 144			'Trim to center the center leg
Pause 250
start:
'Read forward sensors
'Front collision?
IF (PORTB.4 = 1 && PORTB.5 = 1) Then
'Both left AND right sensors are hit, move backward
For b0= 1 TO 5
GoSub backstep
Next
For b0= 1 TO 6
GoSub rturn
Next
EndIF
'Collision ON right?
IF (PORTB.4 = 1 && PORTB.5 = 0) Then
'Right sensor hit, collision ON right
For b0= 1 TO 5
GoSub backstep
Next
For b0= 1 TO 6
GoSub lturn
Next
EndIF
'Collision ON left?
IF (PORTB.4 = 0 && PORTB.5 = 1) Then
'Left sensor hit, collision ON left
For b0= 1 TO 5
GoSub backstep
Next
For b0 = 1 TO 6
GoSub lturn
Next
EndIF
'No collision keep moving forward
IF (PORTB.4 = 0 && PORTB.5 = 0) Then
'Sensors Clear
GoSub frward
EndIF
GoTo start
'============================================================
'Function subroutines
'============================================================
backstep: 'Backward STEP
GoSub rtilt
GoSub rlf
GoSub ltilt
GoSub llf
GoSub center
ls = 150: rs = 150
GoSub do_it
Return
frward: 'Forward STEP
GoSub rtilt
GoSub rlb
GoSub ltilt
GoSub llb
GoSub center
ls = 150: rs = 150
GoSub do_it
Return
lturn: 'Left turn
GoSub rtilt
GoSub rlb
GoSub ltilt
GoSub llf
GoSub center
ls = 150: rs = 150
GoSub do_it
Return
rturn: 'Right turn
GoSub rtilt
GoSub rlf
GoSub ltilt
GoSub llb
GoSub center
ls = 150: rs = 150
GoSub do_it
Return
'=============================================================
'Primary subroutines
'=============================================================
do_it: 'Move robotforward,backward, left OR right
For b1 = 1 TO ct
PulsOut PORTB.0, cs
PulsOut PORTB.1, rs
PulsOut PORTB.2, ls
Pause 18
Next b1
Return
center: 'Center tilt servomotor
ct = 15
cs = 150
IF PORTB.6 = 1 Then cs = 144			'Trim to center the center leg
GoSub do_it
Return
rlf: 'Right leg forward
ct = 20
rs = 120
GoSub do_it
Return
rlb: 'Right leg back
ct = 20
rs = 180
GoSub do_it
Return
llf: 'Left leg forward
ct = 20
ls = 180
GoSub do_it
Return
llb: 'Left leg back
ct = 20
ls = 120
GoSub do_it
Return
rtilt: 'Right side tilt
cs = 150
IF PORTB.6 = 1 Then cs = 144
ct = 15
cs = cs - 18
GoSub do_it
Return
ltilt: 'Left side tilt
cs = 150
IF PORTB.6 = 1 Then cs = 144
ct = 15
cs = cs + 18
GoSub do_it
Return

hold:			'This routine holds all the servomotors in center position
ls = 150
rs = 150
cs = 150
IF PORTB.6 = 1 Then cs = 144			'Trim to center the center leg
ct = 2
GoSub do_it
GoTo hold