'{$STAMP BS2p}
'{$PBASIC 2.5}
'Theremin Vision Robot Control Program - calibration
'Revision 1.83  Jan. 06, 2004
'NOT fully tested!!
'
'Fully released to the Public Domian with NO copyrights reserved.
'
'This program helps adjust the sensor pots
'


'====VARIABLE SETUP=======================================================
startup:

antennaFL PIN 15         'antenna to pin mapping
antennaFR PIN 13
antennaRL PIN 14
antennaRR PIN 12
pulseget CON 9           'pulse input pin mapping per 74HCT4040 division
                         'Pin 8 = / 64
                         'Pin 9 = / 256
                         'Pin 10 = / 1024
                         'Pin 11 = / 1

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


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

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


'=====PLATFORM CONTROL FUNCTIONS==========================================
test:                                     'test/adjust the sensors
 GOSUB scan                               'scan the array
 GOSUB display
GOTO test
END


'=====SUBROUTINES=========================================================

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
RETURN


display:                                  'display sensor data to screen
 DEBUG CRSRXY,0,0
 DEBUG "Sensor Pulse Widths",CR,CR
 DEBUG "Front Left=  ",DEC5 senseFL * 3 / 4," uS",CR
 DEBUG "Front Right= ",DEC5 senseFR * 3 / 4," uS",CR
 DEBUG "Rear Left=   ",DEC5 senseRL * 3 / 4," uS",CR
 DEBUG "Rear Right=  ",DEC5 senseRR * 3 / 4," uS",CR,CR
 DEBUG "As hand approaches sensor, pulse",CR
 DEBUG "width should decrease",CR


 DEBUG CRSRXY,60,7,DEC5 senseRR * 3 / 4,"  "
 DEBUG CRSRXY,50,7,DEC5 senseRL * 3 / 4,"  "
 DEBUG CRSRXY,50,2,DEC5 senseFL * 3 / 4,"  "
 DEBUG CRSRXY,60,2,DEC5 senseFR * 3 / 4,"  "
                                            'make a bar graph
 DEBUG CRSRXY,0,12
 DEBUG "                 CCW    4         6    CW",CR
 DEBUG "========================|---------|=================",CR
 DEBUG 11,"FL ",REP"O"\senseFL / 250,CR
 DEBUG 11,"FR ",REP"O"\senseFR / 250,CR
 DEBUG 11,"RL ",REP"O"\senseRL / 250,CR
 DEBUG 11,"RR ",REP"O"\senseRR / 250,CR
 DEBUG 12
RETURN