PID crossing 0

[Ether]

Originally Posted by ratdude747

I crunched both examples and got 360. maybe my calculator got it wrong, but I think the floor is the issue since it will gives either 0 or 1 and thus either 0 or 360.

For Example1,

0.5+(-357)/360 is equal to -0.4917

The floor of -0.4917 is -1.

Are you saying that your calculator returned 0 or 1, instead of -1 ??

[Ether]

Originally Posted by ratdude747

questions?

Yes.

1) What encoder are you using, and why did you have to do this:

use sin and asin to make all values between 0 and 2pi.

2) If the (X,Y, Z) joystick values are (0.2, -0.5, 0.25), would you please crank those numbers through your code and post the 4 target wheel speeds and wheel steering angles that your code calculates.


3) What is the wheelbase and trackwidth of your vehicle

[ratdude747]

1. US digital MA3 encoders. it is to allow for the calibration to work So anything above 2pi is converted to values in the range of 0 to 2 pi. otherwise you might have negative values and vales not going all the way to 2pi.
2.I will when i have time and my graphing calculator.


3. I am not 100% sure but it is close to the scale of a wb=38 and tw=28 (frame was close to max size, wheels in corners). assume that scale for #2's values.

[Strants]

If you plan to use vectors, you could actually just use some properties of vectors to find the target angle. Let U be the vector in the direction the wheels are currently facing, and V be the vector in the direction you want the wheels to face. Assuming U and V are unit vectors (that is, they have a length of one), the angle between then (let's call it Q) is found by the equation Q = acos((Ux*Vx)+(Uy*Vy)). Because acos returns a value between 0 and pi, we can subtract Q from pi if Q is greater than pi/2 (because, if Q is greater than pi/2, it is quicker to turn the wheels to face in the opposite direction and rotate them backwards). After this set, Q is between 0 and pi/2, but we don't know which direction to rotate in. Assume a positive value of Q corresponds to a counterclockwise rotation, we can then give Q the same sign as the expression (Ux * Vy - Uy * Vx), which is always positive if the rotation required to move U to V is counterclockwise, and negative if the rotation is clockwise. This will give you the shortest counterclockwise rotation, with negative magnitude benefits.

In psuedo-C:

//To start, we assume magnitude is positive
bool neg_mag_benefits = false; 
double getCorrectionAngle(double currentAngle, double destinationAngle) {
  double currentX = cos(currentAngle);
  double currentY = sin (currentAngle);
  double destinationX = cos(destinationAngle);
  double destinationY = sin(destinationAngle);
  double correctionAngle = acos(currentX * destinationX + currentY * destinationY);
  //correctionAngle is in the range 0 to PI.
  double crossProd = currentX * destinationY - currentY * destinationX;
  if(correctionAngle > PI/2) {
    neg_mag_benefits = true; //We will be turning the wheels in a negative direction
    correctionAngle = PI - correctionAngle; //In range 0 to PI/2
  }
  correctionAngle *= abs(crossProd)/crossProd;
  return correctionAngle;
}

[Ether]

Originally Posted by ratdude747

So anything above 2pi is converted to values in the range of 0 to 2 pi

Yes, but why are you using sine and arcsine to do this?

Originally Posted by ratdude747

2.I will when i have time and my graphing calculator.

I thought you had working code. Can't you just run the code?

[Ether]

Originally Posted by Strants

If you plan to use vectors, you could actually just use some properties of vectors to find the target angle. Let U be the vector in the direction the wheels are currently facing, and V be the vector in the direction you want the wheels to face. Assuming U and V are unit vectors (that is, they have a length of one), the angle between then (let's call it Q) is found by the equation Q = acos((Ux*Vx)+(Uy*Vy)). Because acos returns a value between 0 and pi, we can subtract Q from pi if Q is greater than pi/2 (because, if Q is greater than pi/2, it is quicker to turn the wheels to face in the opposite direction and rotate them backwards). After this set, Q is between 0 and pi/2, but we don't know which direction to rotate in. Assume a positive value of Q corresponds to a counterclockwise rotation, we can then give Q the same sign as the expression (Ux * Vy - Uy * Vx), which is always positive if the rotation required to move U to V is counterclockwise, and negative if the rotation is clockwise. This will give you the shortest counterclockwise rotation, with negative magnitude benefits.

If you don't use vectors, you could do the following:

Let U be the angle (in radians) in the direction the wheels are currently facing,
and V be the angle in the direction you want the wheels to face.

Assuming U and V are both measured in the same direction from the same zero reference,
the angle between them (let's call it Q) is found by the equation:

Q = (V-U) - 2pi*floor(0.5+(V-U)/2pi);

Because the above returns a value for Q between -pi and pi,
it tells you which direction to rotate as well as how much to rotate.

If you want to add logic to reverse direction,
that is straightforward to do at this point.

Be aware that it is not necessarily true that steering angle changes
greater than pi/2 are best handled by reversing wheel speed direction.

It depends on many factors, such as the present value of wheel speed,
and the relative dynamic response of your wheel speed vs steering angle.

[ratdude747]

it does use vectors.

I do not know c or c++ for the record.

it *should* work... but I currently do not have access to robot and if i did, it needs its crio re-installed among other control system parts. bottom line is that It may be a while until I can test it.

I used sin and asin since it always puts it in the correct range. you could also use cos and acos.

[Strants]

Originally Posted by ratdude747

I used sin and asin since it always puts it in the correct range. you could also use cos and acos.

Actually, asin will always return a value between -pi/2 and pi/2, while acos will always return a value between 0 and pi. If you want an angle between 0 and 2pi, you have to consider both acos and asin, as alone neither gives enough information to determine the angle*.

If you want to use trig, this method could work:

Let U be the raw angle given by the encoder, and Q be that same angle put in the range [0, 2pi].

If sin(U) >= 0, then Q = acos(cos(U))
Otherwise, Q = 2pi - acos(cos(U))

* In fact, for sin(x) = k, 0 <= x < 2pi, there are two possible values for x (barring x = pi/2 or 3pi/2). Likewise, cos(x) = m also has two values (barring x = 0 or x = pi). The possible value represented in both equations is the true value of x.

[ratdude747]

i see the point... perhaps my best bet is to use mechanical calibration instead...

[Ether]

Originally Posted by Strants

Let U be the raw angle given by the encoder, and Q be that same angle put in the range [0, 2pi].

If sin(U) >= 0, then Q = acos(cos(U))
Otherwise, Q = 2pi - acos(cos(U))

Yikes! use this:

U = U - 2pi*floor(U/2pi)

[ratdude747]

Originally Posted by Ether

Yikes! use this:

U = U - 2pi*floor(U/2pi)

i checked that. looks good. thank you!

I tried using a formula node for that but i guess you have to declare variables in those... the only 2 text languages i really know are pbasic (unrelated: didn't older IFI contollers use that?) and ti-8x code.

although the team's main programmer hates my lack of formula nodes, using discrete math blocks will have to do for now...