'{$STAMP BS2} '{$PBASIC 2.5} 'MARS ROVER CONTROL PROGRAM 'PIN 0 = STEERING SERVO OUTPUT 'PIN 1 = SENSOR SERVO OUTPUT 'PIN 2 = RUN/STEP OUTPUT TO EDE1204 'PIN 3 = DIRECTION OUTPUT TO EDE1204 'PIN 4 = IR SENSOR INPUT 'PIN 5 = IR LED SELECTOR OUTPUT 'PIN 6 = HIGH/LOW SPEED OUTPUT TO EDE1204 'PIN 7 = STALL SENSOR INPUT SENSPERIOD VAR Word 'VARIABLE FOR PULSOUT TO SENSOR SERVO STEERPERIOD VAR Word 'VARIABLE FOR PULSOUT TO STEERING SERVO LEFT VAR Bit 'OBJECT DETECTED TO LEFT LEFTCTR VAR Bit 'OBJECT DETECTED TO LEFT-CENTER CENTER VAR Bit 'OBJECT DETECTED STRAIGHT AHEAD RIGHTCTR VAR Bit 'OBJECT DETECTED TO RIGHT-CENTER RIGHT VAR Bit 'OBJECT DETECTED TO RIGHT FLOOR VAR Bit 'DROPOFF DETECTED STRAIGHT AHEAD BACKTURN VAR Bit 'VARIABLE FOR ALTERNATING BACKING TURNS SERVTURN VAR Bit 'VARIABLE FOR ALTERNATING SENSOR SWEEPS MOVE VAR Word 'VARIABLE FOR STARTUP PAUSE STALLSTATE VAR Bit 'STORES STALL SENSOR STATE STALLCOUNT VAR Nib 'COUNTS STALL SENSOR PULSES LASTCOUNT VAR Nib 'STORES LAST STALL SENSOR PULSE COUNT LASTBACK VAR Bit '1 IF LAST MOVEMENT WAS A BACKUP DIRS = %0000000001101111 'PINS 0-3, 5, 6 SET TO OUTPUTS HIGH 2 'STOPS STEPPER MOTOR LOW 3 'SETS FORWARD DIRECTION FOR STEPPER MOTOR LOW 6 'SELECTS LOW SPEED HIGH 5 'SELECTS FORWARD IR LED SERVTURN = 0 'SETS DIRECTION OF FIRST SENSOR SWEEP BACKTURN = 0 'SETS DIRECTION OF FIRST BACKING TURN STEERPERIOD = 600 'KEEPS STEERING CENTERED DURING FIRST SENSOR SWEEP STALLSTATE = IN7 'STORES INITIAL STATE OF STALL SENSOR LASTCOUNT = 0 'INITIALIZES VARIABLE STALLCOUNT = 0 'INITIALIZES VARIABLE LASTBACK = 0 'INITIALIZES VARIABLE FOR MOVE = 1 TO 1500 'STARTUP PAUSE TO SECURE BATTERY PULSOUT 0, 600 'REFRESHES SERVOS PULSOUT 1, 300 PAUSE 2 NEXT MOVING: LOW 2 'STARTS STEPPER MOTOR IF SERVTURN = 1 THEN SWEEPBACK LEFT = IN4 'READS AND STORES SENSOR INPUT FOR SENSPERIOD = 300 TO 450 'PANS SENSOR TO LEFTCTR PAUSE 4 'SLOWS SENSOR SWEEP DOWN PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD 'REFRESHES STEERING SERVO NEXT LEFTCTR = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 'INCREMENTS COUNTER IF STALL SENSOR STATE HAS CHANGED STALLSTATE = IN7 'READS STALL SENSOR STATE FOR SENSPERIOD = 450 TO 600 'PANS SENSOR TO CENTER PAUSE 4 PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD NEXT CENTER = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 LOW 5 'SELECTS FLOOR IR LED PAUSE 100 'WAIT FOR DETECTOR TO SETTLE FLOOR = IN4 'READS FLOOR PRESENCE HIGH 5 'SELECTS FORWARD IR LED FOR SENSPERIOD = 600 TO 750 'PANS SENSOR TO RIGHTCTR PAUSE 4 PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD NEXT RIGHTCTR = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 FOR SENSPERIOD = 750 TO 900 'PANS SENSOR TO RIGHT PAUSE 4 PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD NEXT RIGHT = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 SERVTURN = SERVTURN + 1 HIGH 2 GOTO MAIN SWEEPBACK: 'SWEEPS SENSORS RIGHT TO LEFT RIGHT = IN4 'READS AND STORES SENSOR INPUT FOR SENSPERIOD = 900 TO 750 'PANS SENSOR TO RIGHTCTR PAUSE 4 'SLOWS SENSOR SWEEP DOWN PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD 'REFRESHES STEERING SERVO NEXT RIGHTCTR = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 FOR SENSPERIOD = 750 TO 600 'PANS SENSOR TO CENTER PAUSE 4 PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD NEXT CENTER = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 LOW 5 'SELECTS FLOOR IR LED PAUSE 100 'ALLOWS DETECTOR TO SETTLE FLOOR = IN4 'READS FLOOR PRESENCE HIGH 5 'SELECTS FORWARD IR LED FOR SENSPERIOD = 600 TO 450 'PANS SENSOR TO LEFTCTR PAUSE 4 PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD NEXT LEFTCTR = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 FOR SENSPERIOD = 450 TO 300 'PANS SENSOR TO LEFT PAUSE 4 PULSOUT 1, SENSPERIOD PULSOUT 0, STEERPERIOD NEXT LEFT = IN4 IF STALLSTATE <> IN7 THEN STALLCOUNT = STALLCOUNT + IN7 STALLSTATE = IN7 SERVTURN = SERVTURN + 1 HIGH 2 GOTO MAIN MAIN: IF (STALLCOUNT = LASTCOUNT) AND (LASTBACK = 1) THEN IMSTUCK 'STOPS ROVER IF OBSTACLE AHEAD AND STALLS IN REVERSE IF FLOOR = 1 THEN BACKUP 'CHECKS FOR DROPOFF USING FLOOR IR LED IF CENTER = 0 THEN BACKUP 'CHECKS FOR OBJECT DIRECTLY AHEAD IF STALLCOUNT = LASTCOUNT THEN BACKUP 'CHECKS FOR SERVO MOTOR STALL STALLCOUNT = LASTCOUNT 'STORES STALL COUNTER VALUE IF LEFTCTR = 0 THEN HARDRIGHT IF RIGHTCTR = 0 THEN HARDLEFT IF LEFT = 0 AND RIGHT = 0 THEN STRAIGHTAHEAD 'NAVIGATES ROVER DOWN A HALLWAY IF LEFT = 0 THEN SOFTRIGHT IF RIGHT = 0 THEN SOFTLEFT GOTO STRAIGHTAHEAD STEERRIGHT: FOR STEERPERIOD = STEERPERIOD TO 800 'MOVES STEERING FROM CURRENT POSITION TO FULLY RIGHT PULSOUT 0, STEERPERIOD PULSOUT 1, SENSPERIOD 'REFRESHES SENSOR SERVO POSITION NEXT RETURN STEERLEFT: FOR STEERPERIOD = STEERPERIOD TO 400 'MOVES STEERING FROM CURRENT POSITION TO FULLY LEFT PULSOUT 0, STEERPERIOD PULSOUT 1, SENSPERIOD 'REFRESHES SENSOR SERVO POSITION NEXT RETURN BACKUP: IF BACKTURN = 1 THEN GOSUB STEERRIGHT ELSE GOSUB STEERLEFT 'CHECKS DIRECTION OF LAST BACKING TURN HIGH 3 'SETS REVERSE DIRECTION ON EDE1204 HIGH 6 'SELECTS HIGH SPEED BACKTURN = BACKTURN + 1 'COUNTER TO ALTERNATE BACKING TURNS LASTBACK = 1 'REMEMBERS THAT LAST MOVEMENT WAS A BACKUP GOTO MOVING STRAIGHTAHEAD: FOR STEERPERIOD = STEERPERIOD TO 600 'MOVES STEERING FROM CURRENT POSITION TO CENTER PULSOUT 0, STEERPERIOD PULSOUT 1, SENSPERIOD 'REFRESHES SENSOR SERVO POSITION NEXT LOW 3 'SETS FORWARD DIRECTION ON EDE1204 LOW 6 'SELECTS LOW SPEED LASTBACK = 0 GOTO MOVING HARDRIGHT: LOW 3 'SETS FORWARD DIRECTION ON EDE1204 LOW 6 'SELECTS LOW SPEED GOSUB STEERRIGHT LASTBACK = 0 GOTO MOVING HARDLEFT: LOW 3 'SETS FORWARD DIRECTION ON EDE1204 LOW 6 'SELECTS LOW SPEED GOSUB STEERLEFT LASTBACK = 0 GOTO MOVING SOFTRIGHT: FOR STEERPERIOD = STEERPERIOD TO 700 'MOVES STEERING FROM CURRENT POSITION TO HALF-RIGHT PULSOUT 0, STEERPERIOD PULSOUT 1, SENSPERIOD 'REFRESHES SENSOR SERVO POSITION NEXT LOW 3 'SETS FORWARD DIRECTION ON EDE1204 LOW 6 'SELECTS LOW SPEED LASTBACK = 0 GOTO MOVING SOFTLEFT: FOR STEERPERIOD = STEERPERIOD TO 500 'MOVES STEERING FROM CURRENT POSITION TO HALF-LEFT PULSOUT 0, STEERPERIOD PULSOUT 1, SENSPERIOD 'REFRESHES SENSOR SERVO POSITION NEXT LOW 3 'SETS FORWARD DIRECTION ON EDE1204 LOW 6 'SELECTS LOW SPEED LASTBACK = 0 GOTO MOVING IMSTUCK: HIGH 2 'STOPS STEPPER MOTOR END