PDA

View Full Version : arm-limitting gyro PD loop producing some strange results


ZZII 527
02-14-2005, 11:16 PM
In an effort to make our arm a bit more controllable (and less dangerous) we are trying to limit its rate of rotation with a gyroscopic feeback loop. The goal is to map the joystick position to a particular angular rate ranging from -30 degrees/second to 30 degree/second. Then, detect the error between that desired angular rate and the actual angular rate as measured by the gyroscope. By using this error and a PD loop, we want to adjust the PWMs of our arm motors to match the actual angular rate to the desired angular rate.

The problem we have been having is the code is only recognizing error in one direction. The arm will hold itself stationary with no problem. When we pull the arm up (which rotates the gyroscope in its negative direction), the feedback works correctly. When we pull it down, however, it does nothing. So the arm will stay still until it detects upward motion, then begin to correct downwards but not stop.

As far as I can tell, this is not positive feedback. Moving the arm upwards does indeed cause a downwards correction. The problem is that moving it downwards does nothing at all. I'm also almost positive that the gyro is mounted in the correct orientation and the PWMs are correct.

The two most likely things that I can think of are 1) I made some stupid math mistake...this is the possibility I am hoping for...or 2) I am using variable types that for some reason can't handle what I am trying to do with them and is reading negative numbers incorrectly.

Anyway, here is the code:


// variable declarations from top of user_routines.c
unsigned int gyro;
float desired_rate, actual_rate;
float error_new, error_old;
float gyro_center = 502.5; // from calibration
float gyro_range = 493.5; // from calibration
float p, d;
float pwm01_temp = 127;

...

// gyro code

gyro = Get_Analog_Value(rc_ana_in01);

// map joystick to rate between -30 and 30 deg/sec
desired_rate = ((float)p3_y - 127) / 127 * 30;

// detect actual rate from gyro, minus sign accounts for
// gyro mounting orientation (- gyro corresponds to + PWM)
actual_rate = -((float)gyro - gyro_center) / gyro_range * 75;

error_new = actual_rate - desired_rate;
p = error_new; // proportional term
d = (error_new - error_old) / 0.0262; // differential term
error_old = error_new;

// adjustment with no differential term (for testing purposes)
pwm01_temp = pwm01_temp - 0.01*p - 0*d;
if (pwm01_temp > 254) { pwm01_temp = 254; }
if (pwm01_temp < 0) { pwm01_temp = 0; }

pwm01 = (int)pwm01_temp;
pwm02 = 254 - pwm01; // to adjust for backwards-mounted second motor



Any help would be great! Thanks in advance.

-Shane

Chris Hibner
02-15-2005, 06:03 AM
I see three problems:

1) It IS positive feedback. You have error = desired - actual. Therefore, your arm PWM should be pwm_temp = pwm_temp + 0.01*p + 0*d (not -0.01*P etc.). By subtracting the control terms, you are turning your loop back into a positive feedback loop.

2) Your derivative will not work. You must declare error_old to be static or else the value will be lost every time the function ends. You should also initialize it to zero to avoid strange behavior in the first loop. Declare it as: static float error_old = 0;

3) You control gains are set such that the controller does nothing. Your desired desired angular rate is limited to +/- 30 deg/sec. Your actual is limited to +/- 75 deg/sec. The most error you can ever achieve is 105 deg/s For example:

let desired_rate = 30
let actual_rate = -75

this implies: error_new = 105

thus, pwm_temp = 127 + 0.01*105 = 128.05

when you type cast that to an int, it becomes 128. Therefore, the most your controller can output is 1 count worth of PWM. The Victor 884 has a +/- 10 count deadzone. Therefore, your controller is doing nothing.

You should at least guarantee full PWM ouput over your control range. Therefore, 105 deg/sec error should result in at least 127 counts of PWM output. Therfore, the minimum your gain should be is 127/105 which equals 1.21. Using 0.01 for the control gain isn't nearly enough.

Other helpful notes:

1) you probably won't need derivative control. The angular rate sensor signal is pretty noisy so the derivative is not very reliable. If you do need D control, filter the angular rate signal (with a low-pass filter) before you take the derivative.

2) Don't expect your arm to be stationary (unless it is driven by a non-backdrivable motor OR you're using dynamic braking). Since there is no integral control you can expect your arm to move slowly due to gravity when you're not touching the joysticks. Integral control will eliminate that.

Mike Betts
02-15-2005, 06:04 AM
In an effort to make our arm a bit more controllable (and less dangerous) we are trying to limit its rate of rotation with a gyroscopic feeback loop. The goal is to map the joystick position to a particular angular rate ranging from -30 degrees/second to 30 degree/second. Then, detect the error between that desired angular rate and the actual angular rate as measured by the gyroscope. By using this error and a PD loop, we want to adjust the PWMs of our arm motors to match the actual angular rate to the desired angular rate.

The problem we have been having is the code is only recognizing error in one direction. The arm will hold itself stationary with no problem. When we pull the arm up (which rotates the gyroscope in its negative direction), the feedback works correctly. When we pull it down, however, it does nothing. So the arm will stay still until it detects upward motion, then begin to correct downwards but not stop.

As far as I can tell, this is not positive feedback. Moving the arm upwards does indeed cause a downwards correction. The problem is that moving it downwards does nothing at all. I'm also almost positive that the gyro is mounted in the correct orientation and the PWMs are correct.

The two most likely things that I can think of are 1) I made some stupid math mistake...this is the possibility I am hoping for...or 2) I am using variable types that for some reason can't handle what I am trying to do with them and is reading negative numbers incorrectly.

Anyway, here is the code:


// variable declarations from top of user_routines.c
unsigned int gyro;
float desired_rate, actual_rate;
float error_new, error_old;
float gyro_center = 502.5; // from calibration
float gyro_range = 493.5; // from calibration
float p, d;
float pwm01_temp = 127;

...

// gyro code

gyro = Get_Analog_Value(rc_ana_in01);

// map joystick to rate between -30 and 30 deg/sec
desired_rate = ((float)p3_y - 127) / 127 * 30;

// detect actual rate from gyro, minus sign accounts for
// gyro mounting orientation (- gyro corresponds to + PWM)
actual_rate = -((float)gyro - gyro_center) / gyro_range * 75;

error_new = actual_rate - desired_rate;
p = error_new; // proportional term
d = (error_new - error_old) / 0.0262; // differential term
error_old = error_new;

// adjustment with no differential term (for testing purposes)
pwm01_temp = pwm01_temp - 0.01*p - 0*d;
if (pwm01_temp > 254) { pwm01_temp = 254; }
if (pwm01_temp < 0) { pwm01_temp = 0; }

pwm01 = (int)pwm01_temp;
pwm02 = 254 - pwm01; // to adjust for backwards-mounted second motor



Any help would be great! Thanks in advance.

-Shane
Shane,

First of all, I advise against using floats. You can search for this subject as I and others have posted on it repeatedly...

I think you want:


pwm01_temp = 127.0 - 0.01*p - 0*d;
if (pwm01_temp > 254) { pwm01_temp = 254; };
if (pwm01_temp < 0) { pwm01_temp = 0; };

Also, realize that pwm01 is 8 bit not 16 bit (you have cast as int).

ZZII 527
02-15-2005, 07:52 AM
Thank you for your responses! There is definitely some stuff for me to try now. I think I need to clarify a few things, though:

1) I am still pretty sure it is not positive feedback for the reasons I explained in the first post and because I tested it with pwm01_temp + 0.01*p + 0*d and got what definitely seemed more like positive feedback. It isn't error = desired - actual, it's error = actual - desired.

2) All the variables are declared at the top of the file, which I thought made them of global scope (they will hold their values between loops) but I could definitely be very wrong there. I will try changing them to static float, etc.

3) The pwm01_temp value was intended to be adjusted a small amount each loop until it hit the correct value. It is a float, so it can store those small accumulations and then typecast to an (int) (or should it be (unsigned char)?) only when setting the real pwm01. Again, if my variables aren't global as-is than this isn't doing what I think it's doing. But based on what I saw (the arm getting faster and faster), I think it is accumulating over loops, just only in one direction for some reason.

I'm gonna fiddle around with it some today and see if I can get some better results. I'd bet on the fact that I typcasted variables all over the place and some are probably not right. I'd like to be able to debug it and see the variables, but the printf function doesn't like me and especially doesn't like floats. I'll post when (and if) I get it to work properly and let everyone know what changes fixed it. Thanks so much again everyone!

-Shane

Chris Hibner
02-15-2005, 08:28 AM
Thank you for your responses! There is definitely some stuff for me to try now. I think I need to clarify a few things, though:

1) I am still pretty sure it is not positive feedback for the reasons I explained in the first post and because I tested it with pwm01_temp + 0.01*p + 0*d and got what definitely seemed more like positive feedback. It isn't error = desired - actual, it's error = actual - desired.

2) All the variables are declared at the top of the file, which I thought made them of global scope (they will hold their values between loops) but I could definitely be very wrong there. I will try changing them to static float, etc.

3) The pwm01_temp value was intended to be adjusted a small amount each loop until it hit the correct value. It is a float, so it can store those small accumulations and then typecast to an (int) (or should it be (unsigned char)?) only when setting the real pwm01. Again, if my variables aren't global as-is than this isn't doing what I think it's doing. But based on what I saw (the arm getting faster and faster), I think it is accumulating over loops, just only in one direction for some reason.

I'm gonna fiddle around with it some today and see if I can get some better results. I'll post when (and if) I get it to work properly and let everyone know what changes fixed it. Thanks so much again everyone!


-Shane


Shane,

I looked through your code again.

1) You are correct: it is not positive feedback since you have error = actual - desired. However, this is poor control style and every control engineer (like me) will be thrown for a loop unless they read the code very carefully. Error = desired - actual is the standard style, then your control efforts are positive (not negated). I know, "it's just style", but using better style leads to faster solution of your problems (it screwed me up the first time).

2) If you declare your variables at the top of the file (outside of any function) then they are global and you don't need to declare them as static. This wasn't clear to me the first time - I thought the variables were declared at the top of the function (not the top of the file). However, this means your code has another error (see below).

If pwm01_temp is global, then you should NOT have pwm01_temp = pwm01_temp + ... This will cause your control loop to act very strange. Your control effort should NOT accumulate - it should be set new every loop. The only accumulation should be if you decide to add integral control, but that should be done separately with it's own gain. Change this line of your code to be the following:

pwm01_temp = 1.26*p + 0*d + 127;

That should solve your problems. Note that the above line assumes that you've changed to the standard style of error = desired - actual.

To add integral control, do the following:

At the top of the file, declare an integrator variable:

float integralState = 0;

In your control code:

integralState = integralState + error_new * 0.0262;

pwm01_temp = 1.26*p + I_CONSTANT*integralState + 0*d + 127;


DISCLAIMER: I also disagree with using floats in embedded code. However, it is too close to the ship date to completely change your code. For the future I would suggest doing a internet search on "fixed point math" (or come to my seminar at the Conference in Atlanta).

ZZII 527
02-15-2005, 09:27 AM
Ah! Thank you! Yea I don't know what I was thinking with pwm01_temp. I'm still a bit confused though about how I can get it to do exactly what I want. Consider the following example:

With P-control centered at 127:
The motor can hold the arm outward stationary at 127 (they don't backdrive). If you hang a tetra from the arm, though, they will backdrive. This will lead to an upwards correction until the gyro stops rotating. At which point, the PWM goes back to 127. In this case the arm drifts downwards.

What I am trying to get is the PWM to adjust itself until it is providing enough force to the motors to counteract the downward drift. I'm not sure if this is integral action...I don't care if the arm returns to its original angle as long as it just stops drifiting. So in the example, if I hung a tetra and the arm started drifting downwards, the code should add to the PWM until it has stopped the drifting, but not necessarily bring it back to it's original angle.

Is this do-able or am I completely misusing the PD loop?
Thanks again! Sorry to keep changing my questions.


-Shane

Chris Hibner
02-15-2005, 10:08 AM
Ah! Thank you! Yea I don't know what I was thinking with pwm01_temp. I'm still a bit confused though about how I can get it to do exactly what I want. Consider the following example:

With P-control centered at 127:
The motor can hold the arm outward stationary at 127 (they don't backdrive). If you hang a tetra from the arm, though, they will backdrive. This will lead to an upwards correction until the gyro stops rotating. At which point, the PWM goes back to 127. In this case the arm drifts downwards.

What I am trying to get is the PWM to adjust itself until it is providing enough force to the motors to counteract the downward drift. I'm not sure if this is integral action...I don't care if the arm returns to its original angle as long as it just stops drifiting. So in the example, if I hung a tetra and the arm started drifting downwards, the code should add to the PWM until it has stopped the drifting, but not necessarily bring it back to it's original angle.

Is this do-able or am I completely misusing the PD loop?
Thanks again! Sorry to keep changing my questions.


-Shane


Shane,

Add the integral control like I said in my previous post. This will solve the drift problem (that is what integral control does, it integrates the error to determine how much power to give to the motor to get zero steady state error (in your case - zero motion of the arm)).

-Chris

ZZII 527
02-15-2005, 09:32 PM
Thanks so much! I cleaned up the code by making my accumulating variable an actual integral correction and adding a proportional correction like you suggested. Mixing them together produced a pretty effective control loop, although it still needs a lot of tuning. But a cool result was that we were able hold our arm horizontal, put a cup of water on it, and tilt the robot backwards and the arm would self-correct to level without spilling any water:

http://web.mit.edu/scolton/www/tilt1small.jpg

http://web.mit.edu/scolton/www/tilt2small.jpg

Chris Hibner
02-16-2005, 01:30 AM
Very cool. I'm glad you got it to work. Control algorithms are fun stuff.