Here is my code.
Is it OKAY to post code in this manner?
Sorry I can’t make the commands stand out like they do on
the MicroCode Studio page.
-------------start snip--------------16F887 PICBASIC PRO code–
symbol trigright = PORTB.0 ’ Define output pin for Trigger pulse
symbol trigfront = PORTB.1 ’ Define output pin for Trigger pulse
symbol echoright = PORTC.6 ’ Define input pin for Echo pulse
symbol echofront = PORTC.5 ’ Define input pin for Echo pulse
SYMBOL channel3 = PORTC.4 ’ Define input pin for Channel 3 PWM
TRISC = %11110000
’OSCCON = %01100001 ’ 4mhz clock - default
’define OSC 4 ‘not needed but does no harm
CNT VAR BYTE
’------------
b0 var word
rangeright var word
rangeright = 100
rangefront var word
oldrangeright var word
differencerangeright var word
frontfreetostopreverse var word
reversecount VAR WORD
reversecount = 0
switchtoPIC VAR WORD
switchtoPIC = 1
radiocontrol var word
radiocontrol = 0
symbol frontdanger = 500 ’ is about 34 inches. We
'realize we are getting too close. Back up. (note that breaking
‘takes a lot, so this should be much higher than the real distance)
symbol frontfreetostopreversesetup = 502’ must be
'higher than “frontdanger” - higher value means more
‘backing out after close encounters
symbol frontfree = 600’ is about 40 inches.
'I set higher to turn earlier when something is in front
’fritsl says it get less skid-turning,
'but more likeliness to miss sharp right turns. My racing
’model has no sharp right turns.
symbol desiredtrack = 360 ’ about 24 inches.
symbol outertrack = 600 ‘about 40 inches
HPWM 1,100,50 ’ Straignt discovered by experiment.
’ slow to center at 100
’--------start----------
gosub blinkloop01 ‘This delay is to let me get ahead of the car.
‘portd = %00000000
low portc.0 ’ Set up initially as PIC control
’–Beginning run–
’ Steering already straight. Make wheels 1/2forward.
main:
hpwm 2,113,50 'WheelsHalfForward
oldrangeright = rangeright
aftermain:
gosub puls
gosub RCcontrolsense
if switchtoPIC > 150 then
'radiocontrol = 1
gosub MakeRadioControl
else
'radiocontrol = 0
gosub MakePICControl
endif
goto skidandreverse
goingforward:
frontfreetostopreverse = frontfreetostopreversesetup
’Here we are going forward.
if rangefront < frontfree then 'turn left as there is
'something ahead in front
HPWM 1,70,50 'SteerFullLeft
goto aftermain 'this is second priority above all loop till this is secured
endif
if rangeright > outertrack then 'turn right as we are too far away
Hpwm 1,110,50 'SteerQuarterRight
’gosub SteerQuarterRight
endif
if rangeright < desiredtrack and oldrangeright < rangeright then
hpwm 1,100,50 ‘SteerStraight
’ drive straight forward as we are on the wrong
'side of desired track, but we are moving the right direction
endif
if rangeright > desiredtrack and oldrangeright > rangeright then
hpwm 1,100,50 ‘SteerStraight
’ drive straight forward as we are on the wrong
'side of desired track, but we are moving the right direction
endif
if rangeright > desiredtrack and oldrangeright < rangeright then
Hpwm 1,110,50 'SteerHalfRight
’gosub SteerHalfRight 'turn right as we are in the outer lane and getting away
endif
if rangeright < desiredtrack and oldrangeright > rangeright then
’if rangeright < desiredtrack then
HPWM 1,85,50 'SteerHalfLeft
’gosub SteerHalfLeft 'turn left as we are in the inner lane and getting closer
endif
goto aftermain
’
’Skidright: 'Nothing calls this subroutine
’gosub blinkloop7
’b0 = 0
’while rangefront > frontfreetostopreverse and rangeright > outertrack and b0 < 20
’gosub puls
’b0 = b0 +1
’wend
’low forrev 'set in forward gear
’hpwm 2,112,50 'WheelsHalfForward
’gosub WheelsHalfForward
’pause 300 why did I put in this pause?
'return
’
reverseoutoftrouble:
'hpwm 2,100,50 'WheelsStop
HPWM 1,140,50 'SteerFullRight
HPWM 2,92,50 'WheelsHalfBack
Pause 100
go_reverse:
frontfreetostopreverse = frontfreetostopreversesetup
keepreversing:
frontfreetostopreverse = frontfreetostopreverse - 25
’Making sure we are not backing and reversing and going
‘nowhere, as we have a wall behind us.
if frontfreetostopreverse < 50 then
goto stuckerror
endif
gosub puls
if rangefront < frontfreetostopreversesetup then
’ We have not reversed far enough
if rangeright < desiredtrack and oldrangeright < rangeright and rangefront > frontdanger then
hpwm 1,110,50 'SteerQuarterRight
’ Going backwards. On the correct side.
'Moving away the wall. Turn right to get parallel with the wall.
'gosub puls
'goto go_reverse
endif
if rangeright > desiredtrack and oldrangeright > rangeright and rangefront > frontdanger then
hpwm 1,100,50 'SteerStraight
''Going backwards. On far side of desired track. Moving
''towards the wall. Go straight.
endif
if rangeright > desiredtrack and oldrangeright < rangeright and rangefront > frontdanger then
hpwm 1,110,50 'SteerQuarterRight
'Going backwards. On far side of desired track. Moving
'away from the wall. Steer to get back on desired track.
endif
if rangeright < desiredtrack and oldrangeright > rangeright then
'Going backwards. On correct side of desired track. Moving
'toward the wall. Steer left to get parallel to wall.
hpwm 1,80,50 'SteerQuarterLeft
endif
goto keepreversing
'hpwm 2,100,50 'WheelsStop
‘Pause 500
endif ’ This refers back five “IF’s” ago. It means
’ it is time to stop reversing and try going forward.
Hpwm 1,70,50 'SteerFullLeft
hpwm 2,112,50 'start going forward again.
goto aftermain 'was RETURN ‘from gosub to reverseoutoftrouble. There is
’ no gosub there.
'
skidandreverse:
if rangefront < frontdanger then ’ too close
goto reverseoutoftrouble
goto aftermain ’ this is first priority above all - loop till this is secured
endif
goto goingforward
’********
puls: ’ produce 10uS trigger pulse (must be minimum of 10uS)
low trigright
HIGH trigright
PAUSE 1
LOW trigright
’measures the range in 10uS steps
’zero could be legal here
pulsin echoright,1,rangeright ’ measures the range in 10uS steps
pulsf: ’ produce 10uS trigger pulse (must be minimum of 10uS)
low trigfront
hIGH trigfront
PAUSE 1
LOW trigfront
pulsin echofront,1,rangefront ’ measures the range in 10uS steps
if rangefront = 0 then goto pulsf
return
RCcontrolsense:
define pulsin_max 10000
PULSIN channel3,1,switchtoPIC
return
stuckerror:
HPWM 1,100,50 'SteerStraight
hpwm 2,100,50 'WheelsStop
dadaa:
high portd.6
pause 5000 'Indicates trouble.
'RCControl or PICControl pulse sensing. Prefer RCControl if stuck.
PULSIN channel3,1,switchtoPIC
if switchtoPIC > 150 then
radiocontrol = 1
gosub MakeRadioControl
else
radiocontrol = 0
GOSub MakePICControl
’PORTD = %00000000
endif
goto aftermain
’goto dadaa
MakeRadioControl:
high portc.0
return
MakePICControl:
low portc.0
RETURN
blinkloop01:
FOR CNT = 1 TO 5
HIGH PORTD.0
LOW PORTD.1
PAUSE 500
LOW PORTD.0
HIGH PORTD.1
PAUSE 500
NEXT CNT
RETURN
blinkloop6:
'PORTD = %00000000
HIGH PORTD.6
PAUSE 500
LOW PORTD.6
RETURN
