Датчик человека
Осноная статья находится здесь http://thereminvision.com/
схемы датчика
Parts list
QTY --- Description --- Digikey#
1 --- PCboard (photo etch) --- PC17P-ND
1 --- CD4093 NAND --- CD4093BCM-ND
1 --- 10nF 50V Capacitor --- PCC103BCT-ND
1 --- 10uF 10V TAN Cap. --- PCS2106CT-ND
2 --- 20pF 50V NPO Cap. --- PCC200CCT-ND
1 --- 20K 1/4" Pot. --- 3266W-203-ND
1 --- 4.7K SMD Resistor --- P4.7KECT-ND
Parts list
QTY --- Description --- Digikey#
1 --- PCboard (photo etch) --- PC17P-ND
1 --- 10nF 50V Capacitor --- PCC103BCT-ND
3 --- 1nF 50V Capacitor --- PCC102BCT-ND
1 --- 10uF 10V TAN Cap. --- PCS2106CT-ND
1 --- 47K SMD Resistor --- P47KECT-ND
1 --- 10K SMD Resistor --- P10KECT-ND
3 --- 4.7K SMD Resistor --- P4.7KECT-ND
4 --- 1N4148 Diodes --- DL4148MSCT-ND
1 --- LT1801 Dual Op-Amp --- LT1801CS8-ND
1 --- 74HCT4040 Counter --- 296-14558-1-ND
1 --- Header 0.1" --- 929400-01-36-ND
1 --- 24 pin IC Socket --- A9424-ND
1 --- BASIC Stamp --- BS2P24-ND
<source lang="vb"> '{$STAMP BS2p} '{$PBASIC 2.5} 'Theremin Vision Robot Control Program 'Revision 1.83 Jan. 06, 2004 'NOT fully tested!! ' '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=======================================================
startup:
platform VAR Nib 'define plateform type 'platform = PIN 5 + 2 * PIN 6 'use when switch is installed platform = 1 '0=test 1=sumo 2=rover 3=follower
calscale CON 30000 'the scale of the calibration zerospeed CON 5 'the speed of the auto zero function incal VAR Bit 'calibration flag edgedetect VAR Bit 'edge detection flag objectdetect VAR Bit 'object detection flag edgetrigger CON 100 'edge detect level objecttrigger CON 100 'object detect level objectclose CON 1500 'object is close 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
'=====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 objectdetect = 0 'no objects detected edgedetect = 0 'no edges detected
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 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 THEN motor = mleftout : objectdetect = 1 IF ABS(senseFR) > objecttrigger THEN motor = mrightout : objectdetect = 1 IF ABS(senseRL) > objecttrigger THEN motor = mright : objectdetect = 1 IF ABS(senseRR) > objecttrigger THEN motor = mleft : objectdetect = 1 IF ABS(senseFL) > objecttrigger AND ABS(senseFR) > objecttrigger THEN motor = mforward : objectdetect = 1 IF ABS(senseRL) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mreverse : objectdetect = 1 IF ABS(senseFL) > objecttrigger AND ABS(senseRL) > objecttrigger THEN motor = mspinleft : objectdetect = 1 IF ABS(senseFR) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mspinright : objectdetect = 1
'forward if no object IF edgedetect = 0 AND objectdetect = 0 THEN motor = mforward 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 'avoid objects IF ABS(senseFL) > objecttrigger THEN motor = mright : objectdetect = 1 IF ABS(senseFR) > objecttrigger THEN motor = mleft : objectdetect = 1 IF ABS(senseRL) > objecttrigger THEN motor = mleftout : objectdetect = 1 IF ABS(senseRR) > objecttrigger THEN motor = mrightout : objectdetect = 1 IF ABS(senseFL) > objecttrigger AND ABS(senseFR) > objecttrigger THEN motor = mreverse : objectdetect = 1 IF ABS(senseRL) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mforward : objectdetect = 1
'forward if no object IF edgedetect = 0 AND objectdetect = 0 THEN motor = mforward 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
objectdetect=0 'toward objects IF ABS(senseFL) > objecttrigger THEN motor = mleftout : objectdetect = 1 IF ABS(senseFR) > objecttrigger THEN motor = mrightout : objectdetect = 1 IF ABS(senseRL) > objecttrigger THEN motor = mright : objectdetect = 1 IF ABS(senseRR) > objecttrigger THEN motor = mleft : objectdetect = 1 IF ABS(senseFL) > objecttrigger AND ABS(senseFR) > objecttrigger THEN motor = mforward : objectdetect = 1 IF ABS(senseRL) > objecttrigger AND ABS(senseRR) > objecttrigger THEN motor = mreverse : objectdetect = 1
'if too close then stop IF ABS(senseFL) > objectclose THEN motor = mstop : objectdetect = 1 IF ABS(senseFR) > objectclose THEN motor = mstop : objectdetect = 1 IF ABS(senseRL) > objectclose THEN motor = mstop : objectdetect = 1 IF ABS(senseRR) > objectclose THEN motor = mstop : objectdetect = 1
'stop if no object IF edgedetect = 0 AND objectdetect = 0 THEN motor = mstop GOTO wander3
END
'=====SUBROUTINES=========================================================
calibrate: 'calibrate the sensors
GOSUB scan 'scan the array two times 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 THEN motor = mleft : edgedetect = 1 IF (senseFR + edgetrigger) > 32768 THEN motor = mright : edgedetect = 1 IF (senseRL + edgetrigger) > 32768 THEN motor = mrightout : edgedetect = 1 IF (senseRR + edgetrigger) > 32768 THEN motor = mleftout : edgedetect = 1 IF (senseFL + edgetrigger) > 32768 AND (senseFR + edgetrigger) > 32768 THEN motor = mreverse : edgedetect = 1 IF (senseRL + edgetrigger) > 32768 AND (senseRR + edgetrigger) > 32768 THEN motor = mforward : edgedetect = 1
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 </source>
