'{$STAMP BS2p} '{$PBASIC 2.5} 'Theremin Vision Robot Control Program 'Revision 1.86 Jan. 10, 2004 ' 'Fully released to the Public Domian with NO copyrights reserved. ' '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======================================================= platform VAR Nib 'define plateform type incal VAR Bit 'calibration flag edgedetect VAR Bit 'edge detection flag objectdetect VAR Bit 'object detection flag roverdir VAR Bit 'rover direction flag calscale CON 30000 'the scale of the calibration zerospeed CON 5 'the speed of the auto zero function edgetrigger CON 1000 'edge detect level objecttrigger CON 500 'object detect level objectclose CON 3000 'object is close 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 led PIN 4 'LED pin 4 control 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 left forward ' 2 0010 left reverse ' 3 0011 na ' 4 0100 right forward ' 5 0101 forward ' 6 0110 spin left ' 7 0111 na ' 8 1000 right reverse ' 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 'motor movement command set mleftfor CON 1 mleftrev CON 2 mrightfor CON 4 mforward CON 5 mspinleft CON 6 mrightrev CON 8 mspinright CON 9 mreverse CON 10 '=====SYSTEM SETUP======================================================== scaleFL = 1 'set scale factors initially scaleFR = 1 scaleRL = 1 scaleRR = 1 DIRA = 15 'set motor drive pins for output motor = 0 'turn drive off now DIRB = 1 'pin4 is LED out, pin5 and pin6 is 'personality input, Pin7 is button led = 0 'turn LED off 'platform = PIN 5 + 2 * PIN 6 'use when switch is installed platform = 2 '0=test 1=sumo 2=rover 3=follower incal = 0 'system not in calibration objectdetect = 0 'no objects detected edgedetect = 0 'no edges detected IF platform = 0 THEN test 'platform jumpoff IF platform = 1 THEN sumo IF platform = 2 THEN rover IF platform = 3 THEN follower '=====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 led = 1 'led on 'IF controlswitch = 0 THEN sumo 'wait for button press led = 0 'led off PAUSE 5000 'time to get back before cal GOSUB calibrate 'calibrate sensors motor = mforward 'start out going forward wander1: 'wonder around 'loop time measured at 20/sec GOSUB scan 'scan array GOSUB edgefind 'check for edge ' GOSUB display 'test outputs - normally off objectdetect=0 'attack objects IF ABS(senseFL) > objecttrigger AND ABS(senseFR) > objecttrigger THEN motor = mforward : objectdetect = 1 : GOTO wander1 IF ABS(senseRL) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mreverse : objectdetect = 1 : GOTO wander1 IF ABS(senseFL) > objecttrigger AND ABS(senseRL) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander1 IF ABS(senseFR) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander1 IF ABS(senseFL) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander1 IF ABS(senseFR) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander1 IF ABS(senseRL) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander1 IF ABS(senseRR) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander1 'forward if no object IF edgedetect = 0 AND objectdetect = 0 THEN motor = mstop GOTO wander1 END rover: 'rover control program led = 1 'led on 'IF controlswitch = 0 THEN rover 'wait for button press led = 0 'led off PAUSE 3000 'time to get back before cal GOSUB calibrate 'calibrate sensors motor = mforward 'start out going forward wander2: 'wonder around GOSUB scan 'scan array GOSUB edgefind 'check for edge 'GOSUB display 'test outputs - normally off objectdetect=0 IF ABS(senseFL) > objecttrigger AND ABS(senseFR) > objecttrigger THEN motor = mreverse : objectdetect = 1 : roverdir = 1 : GOTO wander2 IF ABS(senseRL) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mforward : objectdetect = 1 : roverdir = 0 : GOTO wander2 IF ABS(senseFL) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander2 IF ABS(senseFR) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander2 IF ABS(senseRL) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander2 IF ABS(senseRR) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander2 'forward if no object IF edgedetect = 1 OR objectdetect = 1 THEN GOTO wander2 IF roverdir = 0 THEN motor = mforward ELSE motor = mreverse GOTO wander2 END follower: 'follower control program led = 1 'led on 'IF controlswitch = 0 THEN follower 'wait for button press led = 0 'led off PAUSE 3000 'time to get back before cal GOSUB calibrate 'calibrate sensors motor = mforward 'start out going forward wander3: 'wonder around GOSUB scan 'scan array GOSUB edgefind 'check for edge ' GOSUB display 'test outputs - normally off 'if too close then stop IF ABS(senseFL) > objectclose THEN motor = mstop : objectdetect = 1 : GOTO wander3 IF ABS(senseFR) > objectclose THEN motor = mstop : objectdetect = 1 : GOTO wander3 IF ABS(senseRL) > objectclose THEN motor = mstop : objectdetect = 1 : GOTO wander3 IF ABS(senseRR) > objectclose THEN motor = mstop : objectdetect = 1 : GOTO wander3 objectdetect=0 'follow objects IF ABS(senseFL) > objecttrigger AND ABS(senseFR) > objecttrigger THEN motor = mforward : objectdetect = 1 : GOTO wander3 IF ABS(senseRL) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mreverse : objectdetect = 1 : GOTO wander3 IF ABS(senseFL) > objecttrigger AND ABS(senseRL) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander3 IF ABS(senseFR) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander3 IF ABS(senseFL) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander3 IF ABS(senseFR) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander3 IF ABS(senseRL) > objecttrigger THEN motor = mspinright : objectdetect = 1 : GOTO wander3 IF ABS(senseRR) > objecttrigger THEN motor = mspinleft : objectdetect = 1 : GOTO wander3 'stop if no object IF edgedetect = 0 AND objectdetect = 0 THEN motor = mstop GOTO wander3 END '=====SUBROUTINES========================================================= calibrate: 'calibrate the sensors PAUSE 1000 GOSUB scan 'scan the array GOSUB scan GOSUB scan GOSUB scan 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 led = 1 'led on durring scan 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 led = 0 'led off 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 'don't auto zero if edge or object is detected IF edgedetect = 1 OR objectdetect = 1 THEN RETURN IF senseFL>32768 THEN offsetFL=offsetFL - zerospeed ELSE offsetFL=offsetFL + zerospeed IF senseFR>32768 THEN offsetFR=offsetFR - zerospeed ELSE offsetFR=offsetFR + zerospeed IF senseRL>32768 THEN offsetRL=offsetRL - zerospeed ELSE offsetRL=offsetRL + zerospeed IF senseRR>32768 THEN offsetRR=offsetRR - zerospeed ELSE offsetRR=offsetRR + zerospeed RETURN edgefind: 'if there is an edge, back off edgedetect = 0 IF (senseFL + edgetrigger) > 32768 AND (senseFR + edgetrigger) > 32768 THEN motor = mreverse : edgedetect = 1 : RETURN IF (senseRL + edgetrigger) > 32768 AND (senseRR + edgetrigger) > 32768 THEN motor = mforward : edgedetect = 1 : RETURN IF (senseFL + edgetrigger) > 32768 THEN motor = mleftrev : edgedetect = 1 : RETURN IF (senseFR + edgetrigger) > 32768 THEN motor = mrightrev : edgedetect = 1 : RETURN IF (senseRL + edgetrigger) > 32768 THEN motor = mleftfor : edgedetect = 1 : RETURN IF (senseRR + edgetrigger) > 32768 THEN motor = mrightfor : edgedetect = 1 : RETURN RETURN display: 'display sensor data to screen DEBUG CRSRXY,0,0 DEBUG "Motor= ", BIN4 motor,CR DEBUG "Platform= ", BIN1 platform,CR DEBUG "Calibration= ",DEC1 incal,CR DEBUG "Edge Detect= ",DEC1 edgedetect,CR DEBUG "Object Detect= ",DEC1 objectdetect,CR DEBUG "Switch= ",DEC1 controlswitch,CR DEBUG "Led= ",DEC1 led,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 OR platform > 0 THEN RETURN DEBUG CRSRXY,0,12 DEBUG " 0 1 2 3 4 5",CR DEBUG 11,"FL ",REP"O"\ABS(senseFL) / 50,CR DEBUG 11,"FR ",REP"O"\ABS(senseFR) / 50,CR DEBUG 11,"RL ",REP"O"\ABS(senseRL) / 50,CR DEBUG 11,"RR ",REP"O"\ABS(senseRR) / 50,CR DEBUG 12 RETURN