main:
'pause 10 'Assure ringing from previous triggers: are gone.
GOSUB triggers
'-------------------
CheckIfInDanger:
IF rangefront < frontdanger THEN
HPWM 1,Straight,50
HPWM 2,Back,50
PAUSE 250
reversing = 1
GOTO reverseoutoftrouble
' loop till this is secured
ENDIF
'-------------------
tryleftturn:
IF rangefront < frontfree THEN
'something ahead - turn hard left.
HPWM 1,Left,50
HPWM 2,HalfForward,50
steeringleft = 1
GOTO main 'loop till this is secured
ENDIF
'-------------------
goingforward:
HPWM 1,Straight,50
HPWM 2,Forward,50
'-------------------
keepgoingforward: '----Compare to right wall
IF rangeright > outertrack THEN
HPWM 1,QuarterRight,50
ENDIF
IF rangeright < desiredtrack AND oldrangeright < rangeright THEN
HPWM 1,Straight,50
ENDIF
IF rangeright > desiredtrack AND oldrangeright > rangeright THEN
HPWM 1,Straight,50
ENDIF
IF rangeright > desiredtrack AND oldrangeright < rangeright THEN
HPWM 1,QuarterRight,50
ENDIF
IF rangeright < desiredtrack AND oldrangeright >= rangeright THEN
HPWM 1,QuarterLeft,50
ENDIF
GOTO main
'-------------------------
reverseoutoftrouble:
GOSUB triggers
WHILE rangefront < stopreversing
HPWM 1,Right,50
HPWM 2,Back,50
GOSUB triggers
WEND
IF rangeright > outertrack OR rangeright < desiredtrack THEN
HPWM 1,Left,50
steeringleft = 1
HPWM 2,HalfForward,50
ENDIF
GOTO main
'--------------------------
triggers: ' Check three sonars and channel 3 signal.
oldrangeright = rangeright
' produce 10uS trigger pulse (must be minimum of 10uS)
LOW trigright
HIGH trigright
HIGH trigright
HIGH trigright
LOW trigright
'zero could be legal below
PULSIN echoright,1,rangeright 'measures the range in 10uS steps
PAUSE 10 'Wait for ringing to stop - read SF05 spec.
pulsf:
oldrangefront = rangefront
LOW trigfront
HIGH trigfront
HIGH trigfront
HIGH trigfront
LOW trigfront
PULSIN echofront,1,rangefront ' measures the range in 10uS steps
PAUSE 10
Checkpot:
' I think Potread goes from 0 to 6.
ADCIN PORTA.0, Potread
Potread = Potread/37
Forward = 105+Potread
HalfForward = 105+(Potread/2) 'seems to work.
Back = 95-Potread
HalfBack = 95-(Potread/2)
'----------
frontdanger = 210 + Potread*55 '210-540, 14-36 inches
stopreversing = 180 + Potread*25 '180-330, 12-22 inches
frontfree = 700 + Potread*60 '700-1060, 48-70 inches
desiredtrack = 180 + Potread*20 '180-300, 12-20 inches
outertrack = 360 + Potread*15 '360-450, 24-30 inches
'-----Below worked for Potread = 0and2. Not for 5
'frontdanger = 210 + Potread*50 '210-510, 14-34 inches
'stopreversing = 180 + Potread*25 'Fix backup confusion
'frontfree = 700 + Potread*50 '700-1000, 48-72 inches
'desiredtrack = 180 + Potread*12 '180-252, 12-16 inches
'outertrack = 360 + Potread*12 '360-432, 24-28 inches
'----------
'record for posterity
WRITE 2, WORD Potread
WRITE 4, WORD frontdanger
WRITE 6, WORD stopreversing
WRITE 8, WORD frontfree
WRITE 10, WORD desiredtrack
WRITE 12, WORD outertrack
'***********************
checkchannel3:
'works with four AA batteries pack.
'Radio receiver puts pulses
'on this line whenever the transmitter is turned ON.
switchtoPIC = 2
WHILE switchtoPIC >= 2
COUNT channel3,65,switchtoPIC
HIGH portc.0
WEND
LOW portc.0
RETURN
END
Bookmarks