Position Control

[biojae]

Over the fall, I made a simple positioning system using simple Trig and geometry.
That was using the PWM open loop (On the Jaguar at least) voltage control. Now that we are allowed to use CAN, I would like to use the Position control mode. The encoders are hooked directly into the Jaguar.

The simple formulas that I used are as follows:

S = ((Delta Dist Rightside) + (Delta Dist Leftside)) / 2.0;
X += S * cos(theta);
Y += S * sin(theta);


Theta is from a 300 deg/sec yaw rate gyro.

So, once I had a new estimated position, the distance and angle to the next point were calculated.
The angle and distance would then go into 2 PID calculations.
Each side of the drive would then be set from a simple arcade drive function


double angle = MathUtils.atan2(pY-Y, pX-X);
double distance = Math.sqrt(Math.pow(pY-Y,2)+Math.pow(pX-X,2));

double leftSpeed = speed + turn;
double rightSpeed = speed - turn;


That worked nicely, but now I would like to have the Jaguar handle some of the calcs and PID itself.

I would like to use the Jaguar's Position control, tell it how far to turn the wheel and it goes there.

One dimensional movement (forward / backward) is easy, as the distance to tell it is from the distance formula, but once it gets to rotation, I am lost.

From the above equations, I derived these equations:


Dist Left = sqrt((y-y1)^2 + (x-x1)^2) + (atan2(y-y1,x-x1) - theta1)
Dist Right = sqrt((y-y1)^2 + (x-x1)^2) - (atan2(y-y1,x-x1) - theta1)


would this work if the distance from each side were to be scaled to rotations? or did I make a mistake with my math?

[AmoryG]

I don't know how to fully answer your question, but I'll do it the best I can. I'll assume your robot is like ours (I know it probably isn't) and that these equations will work just as well with your robot as it did with ours.

Assuming that all wheels on a single side of your robot travels at the same rate, let's look at my horrible paint job picture:

http://i49.tinypic.com/35bbcz4.jpg

Let R equal the speed of the right wheels (using an encoder on your right wheel),

et L equal the speed of the left wheels (using an encoder on your left wheel),

and let Base equal the width of your robot, or the distance between two wheels.

As you may know, to get angular speed, you can use the equation:

Angular Speed (rad/sec) = velocity (m/sec) / radius (m)

Since we're not recording speed, but angle and position, ignore velocity and use distance for now.

Since R is larger than L, relative to L, the right side of the robot has moved R - L. Let velocity (or distance) equal R - L and make Base the radius.

From that you get:

theta = (R - L) / Base, or the amount your robot has turned.

To get the heading in the x and y direction, you will need to know the point your robot is driving around. The distance traveled is greater on the right side, so your robot will arc (or drive) to the left. That means the point your robot is driving around will be somewhere to the left of your robot:

http://i47.tinypic.com/219x8qr.jpg

The radius of the arc can be calculated by finding the distance between the center of the circle and the center of your robot, or the length of the triangle - half the base.

To calculate the length of the triangle take the ratio between Base and your robot's relative speed (R - L). Multiply that ratio by the height of your triangle (or R) and you get your length:

R * Base / (R - L).

Subtract half the base from that and you get

R * Base / (R - L) - Base / 2
which equals:
Base ( R / ( R - L ) - 1 / 2)
which when R / (R - L) and 1 / 2 put into one fraction simplfies to:
Base ( (2R - R + L) / 2( R - L))
which equals:
Base ( (R + L) / 2(R - L))

Since theta equals (R - L) / Base the equation can then be simplified to:

(R + L) / (2 * theta)

Radius = (R + L) / (2 * theta)

Using your Radius and theta, you can get x and y.

http://i46.tinypic.com/16tg92.jpg

y = Radius * sin(theta)
x = Radius * (1 - cos(theta))

1 - cos(theta) because your actual x is radius minus x



Of course these are all relative values, so you will need to recalculate based on the facing of your robot.

Hope that helps!

[biojae]

Thank you.

I have this working when I use PWM, i set the speed of each wheel, then every loop of the periodic, i calculate the new position based off of the distance traveled.
That part works.

What I would like to do now, is to use the jaguar's position control mode
(you tell it how far to rotate, and it rotates that far), and tell each side exactly how far to go instead of a % voltage.

Using the distance formula for one d.o.f. movement is easy, it calculates the distance between both points, and it travels that far.

The problem is when rotation is involved, with the % voltage I could give precedence to turning because i had two pid equations feeding the outputs to the motors, but with position control, more precedence is given to driving the distance, is there anyway to fix that?