'{$STAMP BS2p}
'{$PBASIC 2.5}
'Theremin Vision Robot Control Program
'Revision 1.50  Jan. 04, 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 scale of the calibration

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 per 74HCT4040 division
                         '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 is
                         '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 OUTA           'motor control nib  3210
                         '                   ||||
                         '                   ||||--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

DIRA = 1                                  'set motor drive pins for output

motor = 0                                 '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
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
 scaleFR = normal / senseFR
 scaleRL = normal / senseRL
 scaleRR = normal / senseRR

 senseFL = senseFL * scaleFL
 senseFR = senseFR * scaleFR
 senseRL = senseRL * scaleRL
 senseRR = senseRR * scaleRR

 offsetFL = - senseFL                     'find the offsets
 offsetFR = - senseFR
 offsetRL = - senseRL
 offsetRR = - senseRR

 incal = 1                                'calibration is valid
RETURN


scan:                                     'scan the antenna array
 LOW antennaFL                            'turn on the sensor
 PAUSE 2                                  'processor ringdown time
 PULSIN pulseget,1,senseFL                'measure the pulse width
'STOP                                     'stop with it on for testing
 HIGH antennaFL                           'turn off the sensor

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

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

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

IF incal = 0 THEN RETURN

'the readings will normally be 0.  an object will make the reading go
'higher while a platform edge will make it go lower.
 senseFL = -(senseFL * scaleFL + offsetFL)
 senseFR = -(senseFR * scaleFR + offsetFR)
 senseRL = -(senseRL * scaleRL + offsetRL)
 senseRR = -(senseRR * scaleRR + offsetRR)

'auto zero function
 IF senseFL>32768 THEN offsetFL=offsetFL - 1 ELSE offsetFL=offsetFL + 1
 IF senseFR>32768 THEN offsetFR=offsetFR - 1 ELSE offsetFR=offsetFR + 1
 IF senseRL>32768 THEN offsetRL=offsetRL - 1 ELSE offsetRL=offsetRL + 1
 IF senseRR>32768 THEN offsetRR=offsetRR - 1 ELSE offsetRR=offsetRR + 1

RETURN


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

                                          'make a bar graph
 IF incal = 0 THEN RETURN
 DEBUG CRSRXY,0,9
 DEBUG "    0         2         4         6          8          0",CR
 DEBUG 11,"FL ",REP"O"\ABS(senseFL) / 20,CR
 DEBUG 11,"FR ",REP"O"\ABS(senseFR) / 20,CR
 DEBUG 11,"RL ",REP"O"\ABS(senseRL) / 20,CR
 DEBUG 11,"RR ",REP"O"\ABS(senseRR) / 20,CR
 DEBUG 12
RETURN