Getting final vector from left & right drive encoders

[eat4fun]

Hey all,

I’ve been racking my brain trying to solve this problem, and despite not being able to find anything with some light searching, I suspect others may have attempted the same things, so I’m looking for input.

This is the first year our team has used encoders at all; nevertheless, I have been successful in wiring them up, and getting the distance traveled in metres. So on the technical side, everything is a-okay.

I have the distanced traveled on the left and right sides for each time the code loops. What I want to do with this is somehow add the vectors so I get the change in robot heading and displacement since the loop last ran. I would then want to add each of the vectors together to get a final displacement/heading of the robot, relative to the start position. With this I want to figure out where on the field the robot is.

From there I would, presumably, set waypoints for the robot to move towards sequentially.

So let’s say the left side has moved 4 arbitrary distance units, and the right side has moved 5 arbitrary distance units. I want to know the direction/heading that the robot has moved along, and the distance that the robot has traveled in that direction. I would then add them (using vector addition, I presume) to know the approximate location of the robot on the field.

I hope I’m explaining myself clearly. Also, we use LabVIEW in production, but I can interpret C++ and JAVA (and pseudocode, of course) so really the question is platform-independent. If it's any help, the 'bot is 6-wheel with a drop-centre off six CIM motors, all controlled by Victor 888s & an FRC cRIO-II.

Thanks,
Eric Bryce

[flameout]

It will be very difficult to make this accurate. First, let me point out that wheel slip can and does occur (to limit this, you'll pretty much need to limit your acceleration severely). In particular, this will make heading measurement difficult.

As a result, I'd recommend adding a heading gyro as well, and use that for your heading reference.

Then, each cycle, average the distances from the left and right encoders, and use that as the magnitude for the delta position vector. Use the gyro to obtain the heading for this vector. Then add this delta position vector to the current position each time. You will still get errors if wheel slip occurs -- this is why I suggested limiting acceleration (perhaps Jaguars in current control mode, or pseudo-current-control through velocity feedback?)

I'd explain the math and wheel slip-related caveats to this in more depth, but I simply do not have the time to do so right now. I wish you success (please let us know if you succeed!)

[Chris Hibner]

Download the code samples here: http://www.chiefdelphi.com/media/papers/2553. Look in the exampleRobotCode folder and open Periodic Tasks.vi. Near the bottom of the fast loop you'll see calculations that is stored into global variables called "Heading" and "AvgDist". These do exactly what you're looking for. As a bonus, you'll also find "CalcXY.vi" which calculates your x,y coordinate position on the field.

Note: The global "EncTrack" is the distance that separates the encoder wheels.

[Chris Hibner]

Note: you won't get great heading calculations (and therefore good x,y coordinate calculations) with the encoders on your drive wheels. If you want really good heading calculations you'd be better off with a gyro to calculate heading or putting your encoders on follower wheels.

[xraymypanda]

I hope you have some knowledge of Trigonometry, that will help a little in this explanation. I am going to walk you through the formation of forumlae which work individually to find your deltaΘ and deltaLinearPosition. You can log these changes to create an always up to date heading or just utilize them individually to set your targets. With a little bit of manipulation these will give you vectors given that we have our angle and magnitude.
For deltaLinearPositiononly along one axis, basically vector magnitude)
We know that the distance traveled by the robot is relative to the change in ticks from our initial position. We can create a formula to find the change in inches based on a single tick using some simple math.
currentTicks = (leftEncoderTicks + rightEncoderTicks())/2
deltaTicks = currentTicks - initTicks
inchesPerTick = wheelCircumference/encoderTicksPerRevolution
deltaLinearPosition = deltaTicks*inchesPerTick

For deltaΘ:
We can derive our change in angle using our previously calculated formula for inches traveled based on ticks.
Using the formula s= rΘ, where s is the inches traveled by our drive base, we can find our change in angle.
our radius is the width of our robot (the distance between the two sets of wheels)
We know then that our s can be calculated much the same as our delta Linear Position, so we will use deltaTicks*inchesPerTick in place of s
deltaΘ = s/r = deltaTicks*inchesPerTicks/r
deltaΘ is in radians, so we can convert to degrees by multiplying by 180/pi
We create a new constant, degreesPerTick
degreesPerTick = inchesPerTick/r *180/pi
thus,
deltaΘ = deltaTicks*degreesPerTick

I hope this helps you out in your quest.

[eat4fun]

Originally Posted by flameout

It will be very difficult to make this accurate. First, let me point out that wheel slip can and does occur (to limit this, you'll pretty much need to limit your acceleration severely). In particular, this will make heading measurement difficult.

As a result, I'd recommend adding a heading gyro as well, and use that for your heading reference.

I have a gyro presently mounted & wired in the bellypan to log temperature data, as I was afraid that having six CIM motors in close proximity could cause heat problems (turns out it didn't!). So I'll use that data. I was thinking I would try to hash out some sort of voting system to get an accurate heading figure.

Originally Posted by xraymypanda

I hope you have some knowledge of Trigonometry, that will help a little in this explanation. I am going to walk you through the formation of forumlae which work individually to find your deltaΘ and deltaLinearPosition. You can log these changes to create an always up to date heading or just utilize them individually to set your targets. With a little bit of manipulation these will give you vectors given that we have our angle and magnitude.

I'll implement your pseudocode and see how it turns out. Thanks for the explanation!

Originally Posted by Chris Hibner

Download the code samples here: http://www.chiefdelphi.com/media/papers/2553. Look in the exampleRobotCode folder and open Periodic Tasks.vi. Near the bottom of the fast loop you'll see calculations that is stored into global variables called "Heading" and "AvgDist". These do exactly what you're looking for. As a bonus, you'll also find "CalcXY.vi" which calculates your x,y coordinate position on the field.

Thanks for the code. I'll pore over it tomorrow. The CalcXY.vi looks like the real meat of the solution.