Hi,
I am a little desperate. I a
here is the Hardware.
It is The steering part that bugs me.
I made a triangular 3 wheel Line following dolly (90cm X 120cm) and the front wheel is the steering wheel. It is belt driven from a stepper motor via a belt in full step mode. The stepper needs to do 350 steps from middle to full left and from middle to full right. (from left to right 700 steps.)
There are 6 sensors 10mm apart on a 15mm wide black line on a white background.
I have made a program But I just cant get is to work properly. could one of you have a look pleas.
Here is de code
I am a little desperate. I a
here is the Hardware.
It is The steering part that bugs me.
I made a triangular 3 wheel Line following dolly (90cm X 120cm) and the front wheel is the steering wheel. It is belt driven from a stepper motor via a belt in full step mode. The stepper needs to do 350 steps from middle to full left and from middle to full right. (from left to right 700 steps.)
There are 6 sensors 10mm apart on a 15mm wide black line on a white background.
I have made a program But I just cant get is to work properly. could one of you have a look pleas.
Here is de code
Code:
;Line following Buggy
'28x2
' picaxe pin assignment for stepper driver inputs
' enablepin = pinC.0
' Dirpin = pinC.1 -> High - CCW, Low - CW
' Clckpin = pinC.2
' MS0pin = pinC.3
' MS1pin = pinC.4
' MS2pin = pinC.5
' Full stepping - MS0,MS1,MS2 low -> for high speed
' 1/16th stepping - MS0, MS1, MS2 high - for tracking speed
' 1/8th stepping - MS0 high, MS1 high, MS2 low - for low speed slew
setfreq m16
#picaxe 28x2 'Identify the PICAXE being used as an 14M2.
#no_data 'Prevent data from being downloaded to PICAXE._
symbol sen1_in = 0 'Assign symbol sen1_in to anlog input 0
symbol sen2_in = 1 'Assign symbol sen1_in to anlog input 1
symbol sen3_in = 2 'Assign symbol sen2_in to anlog input 2
symbol sen4_in = 3 'Assign symbol sen3_in to anlog input 3
symbol sen5_in = 9 'Assign symbol sen19_in to anlog input 9
symbol sen6_in = 11 'Assign symbol sen11_in to anlog input 11
;Values of the inputs of IR sensors 1 2 3 4 5 6.
symbol sen1_val = b1 'Define variable sensor values "sen1_val = b1
symbol sen2_val = b2 'Define variable sensor values "sen2_val = b2
symbol sen3_val = b3 'Define variable sensor values "sen3_val = b3
symbol sen4_val = b4 'Define variable sensor values "sen4_val = b4
symbol sen5_val = b5 'Define variable sensor values "sen5_val = b5
symbol sen6_val = b6 'Define variable sensor values "sen6_val = b6
symbol level = b9 ;Define mean value of sensors
;below are the different positions of the stepper positions.
;coresponding to the sensors R50 is middel position for straight
;forward movement. R0 most left position of steering wheel and R100
;is most right position of steering wheel.
symbol R0 = w9 ;angle of stepper (steering wheel) set in number of steps
symbol R10 = w10 ;angle of stepper (steering wheel) set in number of steps
symbol R30 = w11 ;angle of stepper (steering wheel) set in number of steps
symbol R50 = w12 ;angle of stepper (steering wheel) set in number of steps
symbol R70 = w13 ;angle of stepper (steering wheel) set in number of steps
symbol R90 = w14 ;angle of stepper (steering wheel) set in number of steps
symbol r100 = w15 ;angle of stepper (steering wheel) set in number of steps
;oldpos and newpos is broagt in to know the movement direction.
symbol oldpos = w17 'remember previous position of stepper in number of steps
symbol newpos = w18 'remember new position of stepper in number of steps
symbol motor = c.2 ;motor output c.2
symbol motor_dir = c.1 ;motor direction HIGH IS FROM LEFT TO RIGHT c.1
;LOW IS FROM RIGHT TO LEFT
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; EXPLANATIONS ARE IN THE SYMBOL AREA
; PROGRAMS
; steering wheel starts in midle and is set to midle position 50 multyply
level = 110 ; =((210-10)/2)+10
R0 = 650 ;Ro to R100 max setings gor that sensors
R10 = 790
R30 = 930 ;with 0 in next line this line shoud be -70.
R50 = 1000 ;should be 0 but picaxe is not alowing - like -70 in previous line
R70 = 1070
R90 = 1210
r100 = 1350
mainloop:
readadc 0,sen1_val; read sensors an place in storage ,sen1_val;
readadc 1,sen2_val; read sensors an plase in storage ,sen2_val;
readadc 2,sen3_val; read sensors an plase in storage ,sen3_val;
readadc 3,sen4_val; read sensors an plase in storage ,sen4_val;
readadc 9,sen5_val; read sensors an plase in storage ,sen5_val;
readadc 11,sen6_val;read sensors an plase in storage ,sen6_val;
;there are alwais 2 sensors higher than "level" if they are on top of the line.
;below is testing witch 2 are just that.
If sen1_val >= level and sen2_val >= level then goto test_sens1_and_2 ;is stepper position here?
If sen2_val >= level and sen3_val >= level then goto test_sens2_and_3 ;is stepper position here?
If sen3_val >= level and sen4_val >= level then goto test_sens3_and_4 ;is stepper position here?
If sen4_val >= level and sen5_val >= level then goto test_sens4_and_5 ;is stepper position here?
If sen5_val >= level and sen6_val <= level then goto test_sens5_and_6 ;is stepper position here?
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;if newpos (new position) is lower than the old position then the stepper went from
;right to left. and visa versa.
test_sens1_and_2: ;if 1 & 2 are over the sensor than
if newpos = r0 then goto mainloop ;if new pos is R0 then goto begin and start again.
If newpos < oldpos and newpos > r0 then goto turnleft ; explanation on top of this section.
if newpos > oldpos and newpos < r10 then goto turnright
if newpos = r10 then goto mainloop ;;if new pos is R10 then goto begin and start again.
test_sens2_and_3:
if newpos = r10 then goto mainloop
If newpos < oldpos and newpos > r10 then goto turnleft
if newpos > oldpos and newpos < r30 then goto turnright
if newpos = r30 then goto mainloop
test_sens3_and_4: ;midle position
if newpos = r30 then goto mainloop ; this is center of movement
If newpos < oldpos and newpos > r30 then goto turnleft
if newpos > oldpos and newpos < r50 then goto turnright
if newpos = r50 then goto mainloop
test_sens4_and_5:
If newpos = r70 then goto mainloop
If newpos < oldpos and newpos > r30 then goto turnleft
if newpos > oldpos and newpos < r50 then goto turnright
if newpos = r90 then goto mainloop
test_sens5_and_6:
If newpos = r90 then goto mainloop
If newpos < oldpos and newpos > r70 then goto turnleft
if newpos > oldpos and newpos < r90 then goto turnright
if newpos = r100 then goto mainloop
turnLeft:
oldpos = newpos ;set old pos to new position
high motor_dir ; change direction of motor
newpos = newpos -1 ;if running to the left, than the step related to the center decrease leaving
;the oldposition to be the previous new position.
goto stepmotor
return
turnRight:
oldpos = newpos ;set old pos
low motor_dir ; change direction of motor
newpos = newpos +1 ;if running to the left, than the step related to the center decrease leaving
;the oldposition to be the previous new position.
goto stepmotor
return
Stepmotor:
high motor ;high and low pin c.2 to form a pulstrain to stepper
pause 2
low motor ;high and low pin c.2 to form a pulstrain to stepper
pause 1
goto mainloop
Last edited: