'{$STAMP BS2p}
'{$PBASIC 2.5}
'Theremin Vision Robot Control Program
'Revision 1.40  Jan. 03, 2004
'
'This program controls multiple robot plateforms or behaviors.  The robots
'are assumed to have tank style drive, one control input switch, and
'Theremin Vision sensors (version 10). The target processor is the Basic
'Stamp BS2p24 but just about any compatible processor would work to with
'some modifications to the code.
'


'====VARIABLE SETUP=======================================================
startup:
platform CON 0           'Define plateform type
                         '0=test 1=sumo 2=rover 3=2lb Critter Crunch

normal CON 30000         'normal is the typical reading returned by the
                         'scan function if nothign is there

incal VAR Bit            'Calibration flag

antennaFL PIN 15         'antenna to pin mapping
antennaFR PIN 14
antennaRL PIN 12
antennaRR PIN 13

pulseget CON 9           'pulse input pin mapping
                         'Pin 8 = / 64
                         'Pin 9 = / 256
                         'Pin 10 = / 1024
                         'Pin 11 = / 1

controlswitch PIN 7      'control switch pin mapping.  control switch is
                         'hard wired goto 1 state when pressed.  there
                         'no debounce!

senseFL VAR Word         'sensor input Front Left
senseFR VAR Word         'sensor input Front Right
senseRL VAR Word         'sensor input Rear Left
senseRR VAR Word         'sensor input Rear Right

scaleFL VAR Byte         'sensor scale calibrations
scaleFR VAR Byte
scaleRL VAR Byte
scaleRR VAR Byte

offsetFL VAR Word        'sensor offset calibrations
offsetFR VAR Word
offsetRL VAR Word
offsetRR VAR Word

motor VAR Byte           'motor control byte 76543210
                         '                   ||||||||
                         '                   ||||||||--Left Forward pin 0
                         '                   |||||||---Left Reverse pin 1
                         '                   ||||||----Right Forward pin 2
                         '                   |||||-----Right Reverse pin 3
                         '                   ||||------
                         '                   |||-------
                         '                   ||--------
                         '                   |---------
                         '


'=====SYSTEM SETUP========================================================
setup:                                    'initialize everything

scaleFL = 1                               'set scale factors initially
scaleFR = 1
scaleRL = 1
scaleRR = 1

motor = 0                                 'motor drive is off

DIR0 = 1                                  'set motor drive pins for output
DIR1 = 1
DIR2 = 1
DIR3 = 1
GOSUB motordrive                          'turn drive off now

incal = 0                                 'system not in calibration

antennaFL = 1                             'disable sensors
antennaFR = 1
antennaRL = 1
antennaRR = 1

IF platform = 0 THEN test                 'platform jumpoff
IF platform = 1 THEN sumo
IF platform = 2 THEN rover
IF platform = 3 THEN crittercrunch


'=====PLATFORM CONTROL FUNCTIONS==========================================
test:                                     'test/adjust the sensors
 GOSUB scan                               'scan the array
 GOSUB display
 IF incal = 0 THEN GOSUB calibrate
 GOTO test
END


sumo:                                     'sumo control program
GOSUB calibrate            'just to test loop timing now
t VAR Byte
PAUSE 2000
DEBUG "start",CR
FOR t=0 TO 250
GOSUB scan
motor = 5
GOSUB motordrive
NEXT
DEBUG "stop"
END


rover:                                    'rover control program
END


crittercrunch:                            'critter crunch control program
END


'=====SUBROUTINES=========================================================
calibrate:                                'calibrate the sensors
 GOSUB scan                               'scan the array three times
 GOSUB scan                               'seems to blow the bugs out the
 GOSUB scan                               'first time

 scaleFL = normal / senseFL                'set scale factors so
 scaleFR = normal / senseFR                'reading is ~normal
 scaleRL = normal / senseRL
 scaleRR = normal / senseRR
 senseFL = senseFL * scaleFL
 senseFR = senseFR * scaleFR
 senseRL = senseRL * scaleRL
 senseRR = senseRR * scaleRR

 offsetFL = normal - senseFL              'find the offsets from normal
 offsetFR = normal - senseFR
 offsetRL = normal - senseRL
 offsetRR = normal - senseRR
 incal = 1
RETURN


scan:                                     'scan the antenna array
 LOW antennaFL
 PAUSE 5
 PULSIN pulseget,1,senseFL
'STOP
 HIGH antennaFL

 LOW antennaFR
 PAUSE 5
 PULSIN pulseget,1,senseFR
'STOP
 HIGH antennaFR

 LOW antennaRL
 PAUSE 5
 PULSIN pulseget,1,senseRL
'STOP
 HIGH antennaRL

 LOW antennaRR
 PAUSE 5
 PULSIN pulseget,1,senseRR
'STOP
 HIGH antennaRR

IF incal = 0 THEN RETURN

'The readings will normally be 30000.  An object will make the reading go
'lower while a platform edge will make it go higher.
 senseFL = senseFL * scaleFL + offsetFL
 senseFR = senseFR * scaleFR + offsetFR
 senseRL = senseRL * scaleRL + offsetRL
 senseRR = senseRR * scaleRR + offsetRR
RETURN


display:                                  'display sensor data to screen
 DEBUG CRSRXY,0,0,"Sensor Outputs",CR,CR
 DEBUG "Motor = ", BIN4 motor,CR,CR
 DEBUG "Calibration = ",DEC1 incal,CR,CR
 DEBUG "ScaleFL=",DEC5 scaleFL,"   OffsetFL=",DEC5 offsetFL,CR
 DEBUG "ScaleFR=",DEC5 scaleFR,"   OffsetFR=",DEC5 offsetFR,CR
 DEBUG "ScaleRL=",DEC5 scaleRL,"   OffsetRL=",DEC5 offsetRL,CR
 DEBUG "ScaleRR=",DEC5 scaleRR,"   OffsetRR=",DEC5 offsetRR,CR
 DEBUG CRSRXY,60,11,DEC5 senseRR
 DEBUG CRSRXY,50,11,DEC5 senseRL
 DEBUG CRSRXY,50,7,DEC5 senseFL
 DEBUG CRSRXY,60,7,DEC5 senseFR,CR,CR,CR,CR,CR,CR

 DEBUG REP" "\30,"|---|",CR
 DEBUG "FL ",REP"O"\(senseFL+500)/1000,"                 ",CR
 DEBUG "FR ",REP"O"\(senseFR+500)/1000,"                 ",CR
 DEBUG "RL ",REP"O"\(senseRL+500)/1000,"                 ",CR
 DEBUG "RR ",REP"O"\(senseRR+500)/1000,"                 ",CR
 DEBUG REP" "\80,CR
RETURN


motordrive:                               'set motor drive output pins
 OUT0 = motor.BIT0
 OUT1 = motor.BIT1
 OUT2 = motor.BIT2
 OUT3 = motor.BIT3
RETURN