PDA

View Full Version : Help with a autopilot



hank van buren
02-17-2005, 09:05 PM
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

KenLem
02-17-2005, 10:57 PM
small "toy" car right, not a real car that is small?

Paul Baker
02-18-2005, 12:03 AM
Can you be more specific on what problems your having. Are you having trouble reading the compass, controlling the servo, translating compass reading into servo control?

upand_at_them
02-18-2005, 01:43 AM
Hank,

You say that you haven't been successful.· What have you tried?· Post your code and let's see what's wrong.

Mike

hank van buren
02-18-2005, 02:32 AM
upand_at_them said...

Hank,

You say that you haven't been successful.· What have you tried?· Post your code and let's see what's wrong.

Mike


Mike this is the code look at it the unit is controlled with 2 stamps, Hank

hank van buren
02-18-2005, 02:37 AM
KenLem said...
small "toy" car right, not a real car that is small?

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 (http://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,[DEC3 e, SDEC4 d]
· IF IN14=0 THEN m2
· GOTO cmd

m3:
· SERIN 9,16572,[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,["75",13,SDEC h-q,13]
· 'serout 7,16572,[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,[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,[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,[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,[340,463,580,702,830,950,1068,1180],g
'· debug ?b
· IF b<=g+20 THEN done
· NEXT

done:
· BRANCH l,[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

achilles03
02-18-2005, 02:59 AM
Hank,
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