Go to Post People were learning left and right. 14-year olds knew what a pitch diameter is... cool stuff. - Andy Baker [more]
Home
Go Back   Chief Delphi > Technical > Programming
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Closed Thread
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 02-15-2005, 12:16 AM
ZZII 527's Avatar
ZZII 527 ZZII 527 is offline
"Scale Electric Vehicle"
AKA: Shane Colton
FRC #0097
Team Role: College Student
 
Join Date: Feb 2004
Rookie Year: 2003
Location: Cambridge, MA
Posts: 366
ZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond repute
Send a message via AIM to ZZII 527
arm-limitting gyro PD loop producing some strange results

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
__________________
MIT Mechanical Engineering
>> College Mentor, Team 97: Cambridge Rindge and Latin School with The Edgerton Center, MIT Mechanical Engineering, Bluefin Robotics, and Draper Laboratory
>> Alumnus, Team 527: Plainedge HS

Last edited by ZZII 527 : 02-15-2005 at 12:22 AM.
  #2   Spotlight this post!  
Unread 02-15-2005, 07:03 AM
Chris Hibner's Avatar Unsung FIRST Hero
Chris Hibner Chris Hibner is offline
Eschewing Obfuscation Since 1990
AKA: Lars Kamen's Roadie
FRC #0051 (Wings of Fire (on sabbatical))
Team Role: Engineer
 
Join Date: May 2001
Rookie Year: 1997
Location: Canton, MI
Posts: 1,503
Chris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond repute
Re: arm-limitting gyro PD loop producing some strange results

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.
__________________
-
An ounce of perception is worth a pound of obscure.
  #3   Spotlight this post!  
Unread 02-15-2005, 07:04 AM
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...
  #4   Spotlight this post!  
Unread 02-15-2005, 08:52 AM
ZZII 527's Avatar
ZZII 527 ZZII 527 is offline
"Scale Electric Vehicle"
AKA: Shane Colton
FRC #0097
Team Role: College Student
 
Join Date: Feb 2004
Rookie Year: 2003
Location: Cambridge, MA
Posts: 366
ZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond repute
Send a message via AIM to ZZII 527
Re: arm-limitting gyro PD loop producing some strange results

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
__________________
MIT Mechanical Engineering
>> College Mentor, Team 97: Cambridge Rindge and Latin School with The Edgerton Center, MIT Mechanical Engineering, Bluefin Robotics, and Draper Laboratory
>> Alumnus, Team 527: Plainedge HS

Last edited by ZZII 527 : 02-15-2005 at 09:11 AM.
  #5   Spotlight this post!  
Unread 02-15-2005, 09:28 AM
Chris Hibner's Avatar Unsung FIRST Hero
Chris Hibner Chris Hibner is offline
Eschewing Obfuscation Since 1990
AKA: Lars Kamen's Roadie
FRC #0051 (Wings of Fire (on sabbatical))
Team Role: Engineer
 
Join Date: May 2001
Rookie Year: 1997
Location: Canton, MI
Posts: 1,503
Chris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond repute
Re: arm-limitting gyro PD loop producing some strange results

Quote:
Originally Posted by ZZII 527
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).
__________________
-
An ounce of perception is worth a pound of obscure.

Last edited by Chris Hibner : 02-15-2005 at 09:36 AM.
  #6   Spotlight this post!  
Unread 02-15-2005, 10:27 AM
ZZII 527's Avatar
ZZII 527 ZZII 527 is offline
"Scale Electric Vehicle"
AKA: Shane Colton
FRC #0097
Team Role: College Student
 
Join Date: Feb 2004
Rookie Year: 2003
Location: Cambridge, MA
Posts: 366
ZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond repute
Send a message via AIM to ZZII 527
Re: arm-limitting gyro PD loop producing some strange results

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
__________________
MIT Mechanical Engineering
>> College Mentor, Team 97: Cambridge Rindge and Latin School with The Edgerton Center, MIT Mechanical Engineering, Bluefin Robotics, and Draper Laboratory
>> Alumnus, Team 527: Plainedge HS

Last edited by ZZII 527 : 02-15-2005 at 10:29 AM.
  #7   Spotlight this post!  
Unread 02-15-2005, 11:08 AM
Chris Hibner's Avatar Unsung FIRST Hero
Chris Hibner Chris Hibner is offline
Eschewing Obfuscation Since 1990
AKA: Lars Kamen's Roadie
FRC #0051 (Wings of Fire (on sabbatical))
Team Role: Engineer
 
Join Date: May 2001
Rookie Year: 1997
Location: Canton, MI
Posts: 1,503
Chris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond repute
Re: arm-limitting gyro PD loop producing some strange results

Quote:
Originally Posted by ZZII 527
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
__________________
-
An ounce of perception is worth a pound of obscure.
  #8   Spotlight this post!  
Unread 02-15-2005, 10:32 PM
ZZII 527's Avatar
ZZII 527 ZZII 527 is offline
"Scale Electric Vehicle"
AKA: Shane Colton
FRC #0097
Team Role: College Student
 
Join Date: Feb 2004
Rookie Year: 2003
Location: Cambridge, MA
Posts: 366
ZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond reputeZZII 527 has a reputation beyond repute
Send a message via AIM to ZZII 527
Re: arm-limitting gyro PD loop producing some strange results

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:



__________________
MIT Mechanical Engineering
>> College Mentor, Team 97: Cambridge Rindge and Latin School with The Edgerton Center, MIT Mechanical Engineering, Bluefin Robotics, and Draper Laboratory
>> Alumnus, Team 527: Plainedge HS
  #9   Spotlight this post!  
Unread 02-16-2005, 02:30 AM
Chris Hibner's Avatar Unsung FIRST Hero
Chris Hibner Chris Hibner is offline
Eschewing Obfuscation Since 1990
AKA: Lars Kamen's Roadie
FRC #0051 (Wings of Fire (on sabbatical))
Team Role: Engineer
 
Join Date: May 2001
Rookie Year: 1997
Location: Canton, MI
Posts: 1,503
Chris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond reputeChris Hibner has a reputation beyond repute
Re: arm-limitting gyro PD loop producing some strange results

Very cool. I'm glad you got it to work. Control algorithms are fun stuff.
__________________
-
An ounce of perception is worth a pound of obscure.
Closed Thread


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump

Similar Threads
Thread Thread Starter Forum Replies Last Post
Updated: Example Gyro Interface Code Kevin Watson Programming 2 03-28-2005 04:36 AM
Example gyro code released. Kevin Watson Programming 60 03-17-2005 06:32 PM
Gyro Calibration roboticsjenkins Programming 11 02-21-2005 08:39 PM


All times are GMT -5. The time now is 02:57 AM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2018, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi