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

'Bit Value can be 0 OR 1
'Nib Value can be 0 TO 15
'Byte Value can be 0 TO 255
'Word Value can be 0 TO 65535


'Go forward until either bmper hits something.
'If hit, backup, turn away from obstacle.
'Go forward again.
'After going forward uninterrupted for a moment, start vacuuming.
'If pause button is pushed, stop everything and wait for another push.
'Upon another pause button push, resume normal activities.

'(Pin allocations are somewhere in the middle of the code.)


counter VAR Word 'used for duh, counting; 0-65535
counter = 0


GOSUB reset   'start in resting state




'START OF PROGRAM
PAUSE 5000 'Delay a bit, so that I can plug the programming cable in.
DO         'start of forever loop






HIGH 5  'go forward
HIGH 7

LOW 12 'LED on


'Brushing motor is off untill robot has gone forward uninterrupted
'for X amount of time. A counter is used for this delay. Brush motor is
'off until it is X amount, then turns on. Any obstacle sensor prevents
'useless brushing while going backwards, by resetting the counter back to 0.

'The uninterrupted forward stuff is there because when the robot hits an obstacle,
'chances are, it will need several back-up-and-turns. If it turned the beater-bar motor
'on while it goes straight, then it will be turning it on and off too much.


IF (counter < 500) THEN
counter = counter + 1 'increment counter
LOW 13    'until delay is met, no brushing
ELSE
HIGH 13   'Brush motor on after delay
ENDIF




'CHECK LEFT BUMPER
IF (IN15) THEN    'if left bumper is hit
   HIGH 12 'LED off
     LOW 13  'Brush motor off
     counter = 0 'reset brush counter
  LOW 5 'stop going forward
  LOW 7

  HIGH 4 'go backward
  HIGH 6

  PAUSE 1500 'wait 1.5 seconds

  LOW 4
  LOW 6

  HIGH 5 'turn right
  HIGH 6

  PAUSE 1000 'wait 1 second

  LOW 6 'get ready for going forward

ENDIF




'CHECK RIGHT BUMPER
IF (IN14) THEN   'if right bumper is hit
   HIGH 12 'LED off
     LOW 13  'Brush motor off
     counter = 0 'reset brush counter
  LOW 5 'stop going forward
  LOW 7

  HIGH 4 'go backward
  HIGH 6

  PAUSE 1500 'wait 1.5 seconds

  LOW 4
  LOW 6

  HIGH 4 'turn left
  HIGH 7

  PAUSE 1000 'wait 1 second

   LOW 4  'get ready for going forward

ENDIF






'CHECK CHARGER
IF(IN11) THEN  'if pin is high...
 GOSUB charger
ENDIF







LOOP 'end of main loop)
GOSUB reset 'go into resting state
END 'die
'START OF SUBROUTINES CALLED BY MAIN FUNCTION




'Go into a state of rest, with everything off.
reset:
LOW 4    'L back
LOW 5    'L forward
LOW 6    'R back
LOW 7    'R forward
LOW 13  'Brush motor; low = on, high = on; input = do nothing ie "float" (for testing only)

INPUT 15 'left bumper; high = closed
INPUT 14 'right bumper; same.
INPUT 12 'Blue LED; low = on, input (or high) = off
INPUT 11 'charger detector(pause button for now); H = pushed
RETURN   'go back to whereever called this subrountine




'sit there and let the juice flow
charger:







GOSUB reset 'stop moving
PAUSE 500   'delay to allow button to be released

  DO
  PAUSE 50
  TOGGLE 12         'Blink LED slowly to indicate charging
  LOOP UNTIL (IN11) '"do nothing" loop until button high again


'when button is finally pushed again...
LOW 12 'LED fast blink
PAUSE 1000  'delay

RETURN 'return





END'just in case