Help with a autopilot
hank van buren
Posts: 57
I am working on a auto pilot for a small car I am using a fluxgate compass with a update 7 times a second rs 332, 4800 baud·output I want to use a stamp bs2 to drive with pulsout a hobby steering servo I have not been very successful could·any of you·refer me to any programs that have been already written. Or any other ideas how to go about this?? Hank
Comments
You say that you haven't been successful.· What have you tried?· Post your code and let's see what's wrong.
Mike
Dear Sir, Thank you for taken interest in my problem.· I am trying to steer a gps controlled vehicle. Please visit my webpage www.gpscontrolledvehicle.com it will show the progress I made so far, as you will see I am limited on speed 2.5 to 3 mph is maximum speed. When this speed is exceeded the update of the gps at 1 second becomes to slow causing the vehicle to lose control. In order to correct this situation I will have to increase the update to the steering servo on a continues basis and every second and look at the gps and correct the heading, this is where the problem is.
The compass used is a Ritchie fluxgate RS-232, 4800 baud update 7 times a second.
A modified Futaba digital servo driving a OSMC motor controller will work on PWM or pulses.
I hope this will be enough information any thing else you need to know about this system please contact me· respectfully yours Hank van Buren.
'{$STAMP BS2}
'02-20-05
'stamp #2
'm3 input from fluxgate compass
'm2 input from stamp #1
'cmd command from stamp #1
'pin5··· serial in
'pin12·· servo driver
'pin14·· serial buzzy
f· VAR· Word
b· VAR· Word
c· VAR· Word
d· VAR· Word
e· VAR· Word
l· VAR· Word
g· CON· 215
a· CON· 770
'GOTO m3
· HIGH 0
· FOR l=0 TO 250
· PULSOUT 12,a
· PAUSE 20: NEXT
· c=a: l=a
cmd:
· 'DEBUG "ok",CR
· IF IN14=1 THEN m3
· SERIN 5,16572,1200,cmd,[noparse][[/noparse]DEC3 e, SDEC4 d]
· IF IN14=0 THEN m2
· GOTO cmd
m3:
· SERIN 9,16572,[noparse][[/noparse]DEC3 b]
· 'DEBUG DEC b,CR
· f=(360-b)-(360-g)*5·· 'replace 1 with e··· replace g with d
· FOR l=0 TO 20
· PULSOUT 12,(f+a)
· NEXT
· IF IN14=0 THEN cmd········· '15
· DEBUG SDEC f,CR
· GOTO m3
m2:
· 'DEBUG "steer",CR
· f=l: c=a+(d*2)
· FOR l=f TO c
· PULSOUT 12,l
· PAUSE 2: NEXT
· GOTO cmd
END
'{$STAMP BS2}
'stamp #1
'servo controller 08/05/04
· p VAR Word: a VAR Word
· b VAR Word: c VAR Word
· d VAR Word: e VAR Word
· f VAR Word: g VAR Word
· h VAR Word: j VAR Word
· k VAR Word: 'n var byte
· l VAR Byte: 'm var word
· q CON·· 755
'pulsin·· gear shift···· (p12),(3)
'pulsin·· throtle······· (p13),(4)
'pulsin·· clutch-brake·· (p14),(2)
'pulsin·· steering······ (p15),(1)
'pulsout· gear shift···· (p08),(3)
'pulsout· throtle······· (p09),(4)
'pulsout· clutch-brake·· (p10),(2)
'pin assigment
'pin0· serial out
'pin1· serial in
'pin2· shift control
'pin15 steering data input
'pin7· serial out
'pin6· serial buzy
'pin11 manual steering
· PAUSE 500
set:
· DEBUG"no signal",CR
· PULSIN 13,1,b
· IF b=0 THEN set
· HIGH 2
· FOR l=0 TO 50
· PULSOUT 8,765······················· 'gear-shift·· (4)
· PAUSE 10
· PULSOUT 9,983······················· 'throttle···· (3)
· PAUSE 10
· PULSOUT 10,547······················ 'brake-clutch (2)
· PAUSE 10
· NEXT:· LOW 2
· LOW 5
test:
· DEBUG" ",CLS
· GOSUB scale_shift
· IF b<393 OR b>460 THEN test··· 'gear-shift
· GOSUB scale_trotlle
· IF b<947 OR b>998 THEN test··· 'throtlle
· PULSIN 14,1,b
· IF b<525 OR b>575 THEN test··· 'cluth-brake
· DEBUG "ok",CR
inp:
· DEBUG "run",CR
· PULSIN 14,1,b
· IF b>945 THEN start
· IF b=0 THEN set
· GOSUB brake_clutch
· GOSUB scale_shift
· IF ABS(f-b)>60 THEN shift
· GOSUB scale_trotlle
· IF ABS(e-b)>10 THEN trotlle
· PULSIN 15,1,h: 'if abs(h-q)<3 then inp
· SEROUT 7,16572,[noparse][[/noparse]"75",13,SDEC h-q,13]
· 'serout 7,16572,[noparse][[/noparse]sdec h-q, 13]
· GOTO inp
scale_trotlle:
· PULSIN 13,1,b
· b=(b*4)-1960
· RETURN
scale_shift:
· PULSIN 12,1,b
· b=(b*2)-750
· RETURN
start:
· HIGH 2
· FOR l=0 TO 75
· PULSOUT 8,1050··············· 'shift f········ (8)
· PAUSE 10
· PULSOUT 10,950··············· 'clutch-brake j· (10)
· PAUSE 10
· NEXT
· LOW 2
· PAUSE 250
· FOR l=0 TO 100
· PULSOUT 9,900················ 'throttle e····· (9)
· PAUSE 15
· NEXT
· PAUSE 10: HIGH 5
· 'PAUSE 350: LOW 5
cmd:
· PULSIN 14,1,c
· IF c<945 THEN set
cmd1:
· SERIN 1,16572,2500,cmd,[noparse][[/noparse]WAIT("/"),DEC4 g, SDEC4 a, DEC4 k]
· 'debug dec g,"· ", sdec a, cr
· IF a=255 THEN set
· HIGH 5: PAUSE 15
· SEROUT 7\6,16572,[noparse][[/noparse]DEC g, 13, SDEC a, 13]
· FOR l=0 TO 10
· PULSOUT 9,900
· PAUSE 10: NEXT
· PAUSE (600-k)
· LOW 5: PAUSE 25
· PULSIN 14,1,c
· IF c=0 THEN cmd1
· GOTO cmd
brake_clutch:··········· '(2)
· PULSIN 14,1,b
· IF ABS(j-b)>125 THEN do
· RETURN
do:
· FOR l=0 TO 3
· LOOKUP l,[noparse][[/noparse]550,699,875,975],g
· IF b<=g+10 THEN ready
· NEXT
ready:
· FOR l=0 TO 20
· PULSOUT 10,g
· PAUSE 25
· NEXT: j=b
· RETURN
trotlle:··························· '(3)
· FOR l=0 TO 20
· GOSUB scale_trotlle
· PULSOUT 9,b
· PAUSE 5
· NEXT:e=b
· GOTO inp
shift:·············· '(4)
· GOSUB scale_shift
· FOR l=0 TO 7
· LOOKUP l,[noparse][[/noparse]340,463,580,702,830,950,1068,1180],g
'· debug ?b
· IF b<=g+20 THEN done
· NEXT
done:
· BRANCH l,[noparse][[/noparse]gs_R,gs_N,gs_1,gs_2,gs_3,gs_4,gs_5,gs_6]
· gs_R: g=450: GOTO bug
· gs_N: g=700: GOTO bug
· gs_1: g=845: GOTO bug
· gs_2: g=915: GOTO bug
· gs_3: g=985: GOTO bug
· gs_4: g=1055: GOTO bug
· gs_5: g=1125: GOTO bug
· gs_6: g=1200: GOTO bug
bug:
· p=0: IF b<f THEN comp
move:
· HIGH 2
· FOR l=0 TO 75
· PULSOUT 8,(g-p)
· PAUSE 10: NEXT
· f=b: LOW 2
· GOTO inp
comp:
· p=25: GOTO move
Post Edited (hank van buren) : 2/18/2005 7:38:31 PM GMT
A few questions... is the servo a PWM servo? Secondly, do you have a PN for the compass? It looks like your math is a little off for the steering determination, but I'd like to check out that datasheet to see what the input/output is exactly.
Dave