View Single Post
  #3   Spotlight this post!  
Unread 15-02-2005, 07:04
Unsung FIRST Hero
Mike Betts Mike Betts is offline
Electrical Engineer
no team
Team Role: Engineer
 
Join Date: Dec 2001
Rookie Year: 1995
Location: Homosassa, FL
Posts: 1,442
Mike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond reputeMike Betts has a reputation beyond repute
Re: arm-limitting gyro PD loop producing some strange results

Quote:
Originally Posted by 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:

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:

Code:
 
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).
__________________
Mike Betts

Alumnus, Team 3518, Panthrobots, 2011
Alumnus, Team 177, Bobcat Robotics, 1995 - 2010
LRI, Connecticut Regional, 2007-2010
LRI, WPI Regional, 2009 - 2010
RI, South Florida Regional, 2012 - 2013

As easy as 355/113...