Датчик человека — различия между версиями

Материал из roboforum.ru Wiki
Перейти к: навигация, поиск
 
(не показана 1 промежуточная версия 1 участника)
Строка 1: Строка 1:
 +
[[Category:Проекты компонентов|Проекты компонентов]]
 +
 
Осноная статья находится здесь http://thereminvision.com/ <br />
 
Осноная статья находится здесь http://thereminvision.com/ <br />
 
схемы датчика <br />
 
схемы датчика <br />
Строка 32: Строка 34:
 
1  ---            BASIC Stamp        ---            BS2P24-ND<br />
 
1  ---            BASIC Stamp        ---            BS2P24-ND<br />
 
</blockquote>
 
</blockquote>
 +
 +
<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>

Текущая версия на 08:57, 22 сентября 2009


Осноная статья находится здесь http://thereminvision.com/
схемы датчика

Schematic-10.jpg
Processor-10.jpg

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>

Источник — «http://wiki.roboforum.ru/index.php?title=Датчик_человека&oldid=8253»