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?