'{$STAMP BS2p} '{$PBASIC 2.5} 'Theremin Vision Robot Control Program 'Revision 1.70 Jan. 05, 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 VAR Nib 'platform = PIN 5 + 2 * PIN 6 'when switch is in platform = 0 'define plateform type '0=test 1=sumo 2=rover 3=2lb Critter Crunch calscale CON 30000 'the scale of the calibration incal VAR Bit 'calibration flag edgedetect VAR Bit 'edge detection flag edgetrigger CON -100 'edge detect level objecttrigger CON 100 'object detect level antennaFL PIN 15 'antenna to pin mapping antennaFR PIN 14 antennaRL PIN 12 antennaRR PIN 13 led PIN 4 'LED pin 4 control 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 ' ' ' 0 0000 stop ' 1 0001 right turn outside wheel ' 2 0010 left turn inside wheel ' 3 0011 na ' 4 0100 left turn outside wheel ' 5 0101 forward ' 6 0110 spin left ' 7 0111 na ' 8 1000 right turn inside wheel ' 9 1001 spin right ' 10 1010 reverse ' 11 1011 na ' 12 1100 na ' 13 1101 na ' 14 1110 na ' 15 1110 na mstop CON 0 'movement command set mrightout CON 1 mleft CON 2 mleftout CON 4 mforward CON 5 mspinleft CON 6 mright CON 8 mspinright CON 9 mreverse CON 10 randtime VAR Byte 'random timing variable '=====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 randtime = 127 'randome time seed value 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 'switch input ?? PAUSE 5 'time to get back before cal GOSUB calibrate 'calibrate sensors motor = mforward 'start out going forward wander1: 'wonder around GOSUB edgefind 'check for edge IF senseFL > objecttrigger THEN motor = mleftout IF senseFR > objecttrigger THEN motor = mrightout IF senseRL > objecttrigger THEN motor = mright IF senseRR > objecttrigger THEN motor = mleft IF senseFL > objecttrigger AND senseFR > objecttrigger THEN motor = mforward IF senseRL > objecttrigger AND senseRR > objecttrigger THEN motor = mreverse 'attack objects RANDOM randtime IF randtime > 5 AND edgedetect = 0 AND (motor = 5 OR motor = 10) THEN GOTO wander1 'usually skip changing directions 'unless there is an edge or in a turn randtime = randtime / 16 'divide to a random nibble to get a 'new random motor directin IF randtime = 0 OR randtime = 3 OR randtime = 7 OR randtime >10 THEN wander1 'skip bad values for motor motor = randtime 'change direction GOTO wander1 END rover: 'rover control program PAUSE 3 'time to get back before cal GOSUB calibrate 'calibrate sensors motor = mforward 'start out going forward wander2: 'wonder around GOSUB edgefind 'check for edge IF senseFL > objecttrigger THEN motor = mright IF senseFR > objecttrigger THEN motor = mleft IF senseRL > objecttrigger THEN motor = mleftout IF senseRR > objecttrigger THEN motor = mrightout IF senseFL > objecttrigger AND senseFR > objecttrigger THEN motor = mreverse IF senseRL > objecttrigger AND senseRR > objecttrigger THEN motor = mforward 'avoid objects RANDOM randtime IF randtime > 5 AND edgedetect = 0 AND (motor = 5 OR motor = 10) THEN GOTO wander2 'usually skip changing directions 'unless there is an edge or in a turn randtime = randtime / 16 'divide to a random nibble IF randtime = 0 OR randtime = 3 OR randtime = 7 OR randtime >10 THEN wander2 'skip bad values for motor motor = randtime 'change direction GOTO wander2 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 = calscale / senseFL 'set scale factors scaleFR = calscale / senseFR scaleRL = calscale / senseRL scaleRR = calscale / 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 edgefind: 'if there is an edge, back off edgedetect = 0 IF senseFL < edgetrigger THEN motor = mleft : edgedetect = 1 IF senseFR < edgetrigger THEN motor = mright : edgedetect = 1 IF senseRL < edgetrigger THEN motor = mrightout : edgedetect = 1 IF senseRR < edgetrigger THEN motor = mleftout : edgedetect = 1 IF senseFL < edgetrigger AND senseFR < edgetrigger THEN motor = mreverse : edgedetect = 1 IF senseRL < edgetrigger AND senseRR < edgetrigger THEN motor = mforward : edgedetect = 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