arm-limitting gyro PD loop producing some strange results

[ZZII 527]

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