problem with code for controlling Motors and encoders
Kris Cardoen
Posts: 46
Hi,
I have two HB25 motor controllers and two quadrature encoders installed on my propeller robot.
I'm trying to write code to make sure that both DC motors run at the same speed, so that I am sure that the robot runs perfectly strait forward.
To control this I read the data from the two encoders (left and right), and modify my pulse send to the controllers(motor). I guess there is something wrong with my approach because the code doesn't work as it suppose to.
The result is that while processing either the left motor works of the right one.
When I put a time.pause(200) in the control loop both engines run at the same time, but the motors doesn't run at the same speed.(the speed corrections) happen to slow
Any help would be much appropriated
Regards,
Kris
I have two HB25 motor controllers and two quadrature encoders installed on my propeller robot.
I'm trying to write code to make sure that both DC motors run at the same speed, so that I am sure that the robot runs perfectly strait forward.
To control this I read the data from the two encoders (left and right), and modify my pulse send to the controllers(motor). I guess there is something wrong with my approach because the code doesn't work as it suppose to.
connections:
PIN Cable Device detail
0 1 Left quadrature encoder fase A
1 2 Left quadrature encoder fase B
2 3 right quadrature encoder fase A
3 4 right quadrature encoder fase B
4 5 right motor controller
5 6 left motor controller
6 7 left Ping utrasonic
7 8 front Ping utrasonic
8 9 right Ping utrasonic
}
CONS
MTR = 19100 'number of clicks per/wheel rotation
Forw = 1600 'setting for motor controller forward
OBJ
pin : "Input Output Pins" 'instance for using I/O pins
time : "Timing" 'instance for timing
system : "Propeller Board of Education" 'instance for board of education
pst : "Parallax Serial Terminal Plus" 'instance for serial port comm
HB25_1 : "HB-25" 'instance for motor controller 1
HB25_2 : "HB-25" 'instance for motor controller 2
Encoder_1 : "Quadrature Encoder" 'instance for Left encoder
Encoder_2 : "Quadrature Encoder" 'instance for right encoder
VAR
long PosLeft[3] 'Create buffer for two encoders (plus room for delta position support of 1st encoder)
long PosRight[3] 'Create buffer for two encoders (plus room for delta position support of 1st encoder)
long vRightSet 'var to controle speed RightMotor deviation of the cons Forw
long vLeftSet 'var to controle speed LeftMotor deviation of the cons Forw
PUB Main
'INIT
system.Clock(80_000_000) 'Initialise clock
pst.Str(String("Starting system...."))
pst.newline
HB25_1.config(4,0,1) 'init HB25 controller signal pin 4
HB25_2.config(5,0,1) 'init HB25 controller signal pin 5
Encoder_1.Start(0, 1, 1, @PosLeft) 'Start continuous two-encoder reader
Encoder_2.Start(2, 1, 1, @PosRight) 'Start continuous two-encoder reader
runForwarClicks
pub runForwarClicks
vRightSet := 1600 'sets vRightSet to cons Forw
vLeftSet := 1600 'sets vLeftSet to cons Forw
repeat
HB25_1.set_motor1(vRightSet) 'start right motor ok
HB25_2.set_motor1(vLeftSet) 'start left motor
'compensate motor rotation to keep left and right at the same rotation speed
pst.Str(String("leftClick = "))
pst.dec(PosLeft[0])
pst.Str(String(" rightClick = "))
pst.dec(PosRight[0])
pst.Str(String(" "))
if PosRight[0] > PosLeft[0]
pst.Str(String(" correct to left"))
vRightSet--
vLeftSet++
if PosRight[0] < PosLeft[0]
pst.Str(String(" correct to right"))
vRightSet++
vLeftSet--
pst.Str(String(" (right setting ="))
pst.dec (vRightSet)
pst.Str(String(" left setting ="))
pst.dec(vLeftSet)
pst.Str(String(")"))
pst.newline
stopMotors
pub stopMotors 'This procedure will stop both engines by setting the imput to 1500
HB25_1.set_motor1(1500) 'stop right motor
HB25_2.set_motor1(1500) 'stop left motor
The result is that while processing either the left motor works of the right one.
When I put a time.pause(200) in the control loop both engines run at the same time, but the motors doesn't run at the same speed.(the speed corrections) happen to slow
Any help would be much appropriated
Regards,
Kris
Comments
Not sure why you re using two HB25 objects... The HB25 object code includes methods for setting two HB25s (individually or together: SetMotor1, SetMotor2 and SetMotorS) to different speeds. Why waste a cog? And, you can run the HB25s from a single prop pin (check the HB25 docs). Also, you do not need two encoder objects.
Here are the HB25 driver methods: PUB SetMotor1(Motor1WidthNew_) 'Note: A negative Width value indicates Ramp mode Motor1WidthNew := Motor1WidthNew_ ' A positive Width value indicates Immediate mode PUB SetMotor2(Motor2WidthNew_) 'Note: A negative Width value indicates Ramp mode Motor2WidthNew := Motor2WidthNew_ ' A positive Width value indicates Immediate mode ' A "Zero" Width value deselects Motor2 PUB SetMotorS(Motor1WidthNew_,Motor2WidthNew_) SetMotor1(Motor1WidthNew_) SetMotor2(Motor2WidthNew_) Here is the Encoder driver comments: PUB Start(StartPin, NumEnc, NumDelta, PosAddr): Pass ''Record configuration, clear all encoder positions and launch a continuous encoder-reading cog. ''PARAMETERS: StartPin = (0..63) 1st pin of encoder 1. 2nd pin of encoder 1 is StartPin+1. '' Additional pins for other encoders are contiguous starting with StartPin+2 but MUST NOT cross port boundry (31). '' NumEnc = Number of encoders (1..16) to monitor. '' NumDelta = Number of encoders (0..16) needing delta value support (can be less than NumEnc). '' PosAddr = Address of a buffer of longs where each encoder's position (and deta position, if any) is to be stored. ''RETURNS: True if successful, False otherwise.As for your motors not auto-correcting to match speed, the single increment (++) and decrement (--) operators are very small considering the scale of motor speed input (approximately 1500-2200 forward and 800-1500 backward) as well as the scale of the encoders (19,100 ticks per revolution).
As a quick test, change the increment/decrements to:
[COLOR=#323333][FONT=Parallax] if PosRight[0] > PosLeft[0] [/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] pst.Str(String(" correct to left"))[/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] vRightSet -= 50[/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] vLeftSet += 50[/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] if PosRight[0] < PosLeft[0] [/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] pst.Str(String(" correct to right"))[/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] vRightSet += 50[/FONT][/COLOR] [COLOR=#323333][FONT=Parallax] vLeftSet -= 50[/FONT][/COLOR]See if the bot reacts quicker. Then, modify the increment/decrement value to optimize.
dgately