' -----[ Title ]--------------------------------------------------------------
'Class Project ME/EE
' Boe-Bot uses whiskers to detect objects, and when  bumping into them tries to maneuver around them..
'Boe-bot will run while in a light area.
'Boe-bot when in a shaded area will
'stop moving
'activate a servo clockwise for a set amount of time.
'stop for ten seconds
'REVERSE the servo
'Then do a U-turn and continue on its course (start the main routine again..

' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "I Hope im Running!!"
' -----[ Variables ]----------------------------------------------------------
pulseCount VAR Byte ' FOR...NEXT loop counter.
' -----[ Initialization ]-----------------------------------------------------
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
' -----[ Main Routine ]-------------------------------------------------------
DO
IF (IN5 = 0) AND (IN7 = 0) THEN ' Both whiskers detect obstacle
GOSUB Back_Up ' Back up & U-turn (left twice)
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN ' Left whisker contacts
GOSUB Back_Up ' Back up & turn right
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN ' Right whisker contacts
GOSUB Back_Up ' Back up & turn left
GOSUB Turn_Left
ELSE ' Both whiskers 1, no contacts
GOSUB Forward_Pulse ' Apply a forward pulse
ENDIF ' and check again
LOOP

'DO
'IF (IN6 = 0) AND (IN3 = 0) THEN ' Both photoresistors detects           ''''PHOTO RESISTOR CODE
'Pulsout 14, 650   ' activates servo thats plugged into 14...
'pause for ten seconds.


'ELSEIF (IN6 = 0) THEN ' Left photoresistor detects
'GOSUB Back_Up ' shadow, back up & turn right.
'GOSUB Turn_Right
'ELSEIF (IN3 = 0) THEN ' Right photoresistor detects
'GOSUB Back_Up ' shadow, back up & turn left.
'GOSUB Turn_Left
'ELSE ' Neither photoresistor detects
'GOSUB Forward_Pulse ' shadow, apply a forward pulse.   '''Do i need this section still...

'ENDIF
'LOOP


'DO      WITH LIGHTS
'IF (IN5 = 0) AND (IN7 = 0) THEN
'HIGH 10
'HIGH 1
'GOSUB Back_Up
'GOSUB Turn_Left
'GOSUB Turn_Left
'ELSEIF (IN5 = 0) THEN
'HIGH 10
'GOSUB Back_Up
'GOSUB Turn_Right
'ELSEIF (IN7 = 0) THEN
'HIGH 1
'GOSUB Back_Up
'GOSUB Turn_Left
'ELSE
'LOW 10
'LOW 1
'GOSUB Forward_Pulse
'ENDIF
'LOOP

' -----[ Subroutines ]--------------------------------------------------------
Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 40
RETURN
Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 40
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 40
NEXT
RETURN
Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 40
NEXT
RETURN