PID turning help - balance robot

Discussion in 'General Electronics Chat' started by bug13, Nov 17, 2013.

  1. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    NOTE: I got a typo in my title, it should be PID turning help. (I don't seem to find a way to fix it)
    Opps, it's fixed now

    Hi guys

    My new dc motors arrived last week, and I replaced my old noise motors with the new ones, more info here.

    Now I need to turn the PD loop to control my little robot so it can stand up. I have no experience on turning any PID/PD loop, all I know is what they are from reading some material.

    So I have a few tried myself, no luck, I got to a point where it oscillate for a couple seconds and fall over.

    Any advice/suggestion on how should I solve this problem?

    Thanks guys
     
    Last edited: Nov 17, 2013
  2. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    We will need some info about how many readings you are taking per second (or duration between reads). Also, how much change do you expect between reads?

    What kind of sensors (time constant or how long will it take from the time an error first starts until the motors can respond to start correcting the change).

    Also, here is a general from Wikipedia...

    Pseudocode

    Here is a simple software loop that implements a PID algorithm:[18]
    In this example, two variables that will be maintained within the loop are initialized to zero, then the loop begins. The current error is calculated by subtracting the measured_value (the process variable or PV) from the current setpoint (SP). Then, integral and derivative values are calculated and these and the error are combined with three preset gain terms – the proportional gain, the integral gain and the derivative gain – to derive an output value. In the real world, this is D to A converted and passed into the process under control as the manipulated variable (or MV). The current error is stored elsewhere for re-use in the next differentiation, the program then waits until dt seconds have passed since start, and the loop begins again, reading in new values for the PV and the setpoint and calculating a new value for the error.


    Note: if your updates happen at a regular interval, then you can ignore the dt factor since you can just assume it is 1 (no need for multiplication).
     
    bug13 likes this.
  3. bertus

    Administrator

    Apr 5, 2008
    15,647
    2,346
    bug13 likes this.
  4. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    I am currently doing 100 reading per second

    I have no idea, how do I find out?

    I am using MPU6050 breakout board

    Sorry, I don't know about the time constant, I will read the datasheet and add some code to measure the time, will report back if I find something.

    Thanks, good to know.

    here is my code:

    Code ( (Unknown Language)):
    1. float updatePID(float angle, float gyro_angle, float setPoint)
    2. {
    3.     float pTerm;
    4.     float error;
    5.     float dTerm;
    6.  
    7.     error = angle - setPoint;
    8.  
    9.     pTerm = P_GAIN * error;
    10.     dTerm = D_GAIN * gyro_angle;
    11.  
    12.  
    13.     return pTerm + dTerm;
    14. }
    the setPoint is 0 at the moment, instead of calculate the dTerm from:

    I use the data from gyro.

    The float angle is the fusion angle from accelerometer and gyroscope with a complementary filter.
     
  5. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    I am using StampPlot to visualize the info
     
  6. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    I would start with just PGain = 1 and Dgain = 0

    Increase PGain until you see it make too much correction. Then you can add the Dgain until it stops over correcting.
     
    bug13 likes this.
  7. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    What do you mean by too much correction? Do you mean it can stand up (not falling on the floor) but oscillating?
     
  8. GetDeviceInfo

    Senior Member

    Jun 7, 2009
    1,571
    230
    There are many excellent documents on PID tuning. A picture paints a thousand words. Look at dynamic loop response charts. You might try initial tuning by stepping your drive mechanism unloaded to obtain a classic response, then adding gain as load is introduced.
     
  9. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    Yes, oscillating. A self-balancing robot will usually just fall over if pGain is not high enough and you will look like it is just falling over. As you turn up gain, then you see it falling slightly slower. Finally, you turn it up so much that it starts oscillating or falling the opposite direction (always use a common starting point (e.g. 5-degrees off balance forward)).

    Once you have that defined, you can start adding D and I gain.

    If you start adding bumps and other obstacles, you may have to add additional PGain for faster correction.
     
    bug13 likes this.
  10. Markd77

    Senior Member

    Sep 7, 2009
    2,803
    594
    I've not used PID much, but the few times I have, I've noticed an improvement by putting a limit on the maximum value of the I term. It reduces integral wind up:
    http://en.wikipedia.org/wiki/Integral_windup

    <ed> Did you mean PID tuning help? </ed>
     
    Last edited: Nov 18, 2013
  11. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    Bug13 plans to use only P and D terms for a start. Integral terms can get tricky.

    I like to use I-term from only the last X number of readings.
     
  12. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    Now I can turn the PD control loop to make my robot stand up reasonably well, but I got a new problem.

    At some conditions, the robot will go faster and faster, and to a point where the motors is not fast enough to keep the robot balance.

    So I need to somehow limit the top speed, make the motors go faster to push back before it fall over.

    here have discussed how it was done, but I don't understand it, can someone please explain it to me, thanks.

    you will find the codes about half way down the page.
     
  13. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    I don't see any code. Do you mean one of the links at the end of the first link?
     
  14. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    That's how he discussed it his words:

    That's his example codes: ( I have added some formatting, hope it's readable)

    Code ( (Unknown Language)):
    1.  
    2. //Inputs
    3. //angle, angle_rate: the tilt angle of the scooter in radians and its derivative in radians/sec
    4. //steer_knob: the reading from the steering knob, between -1 and +1.
    5.  
    6. /*   Balance   */                                          
    7. balance_torque = 5.0 * (angle - rest_angle) + 0.4 * angle_rate
    8.  
    9. // Limit top speed by tilting back
    10. overspeed = max(0, cur_speed - 0.5)
    11. if (overspeed > 0)
    12. {
    13.        overspeed_integral = min(0.4, overspeed_integral + min(0.2, overspeed+0.05) * dt)
    14. }
    15. else
    16. {
    17.        overspeed_integral = max(0, overspeed_integral - 0.04*dt)
    18. }
    19.  
    20. rest_angle = 0.4*overspeed + 0.7*overspeed_integral
    21.  
    22. //Steer. Decrease steering rate at high speed
    23. steer_cmd = 0.07/(0.3+abs(cur_speed)) * steer_knob
    24.  
    25. //Track current speed
    26.  
    27. cur_speed += 1.2 * balance_torque * dt
    28.  
    29. //Differential steering
    30. left_motor_pwm = balance_torque + cur_speed + steer_cmd
    31. right_motor_pwm = balance_torque + cur_speed - steer_cmd
    32.  
    33. //Outputs
    34. //left_motor_pwm and right_motor_pwm directly
    35. //set the duty cycle of the pulse width modulator for the wheel controller,
    36. //and range from -1 to +1 (+1 is 100% forward, -1 is 100% reverse.)
     
  15. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    Is your project an autonomous bot, a radio controlled bot or a rideable Segway-type device?
     
  16. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    The way I understand his program is, his PGain is set to 50% of max gain, then he has some "headspace" to make corrections with the D and I terms.

    ========================================
    Balance
    balance_torque = 5.0 * (angle - rest_angle) + 0.4 * angle_rate
    ========================================

    I read (angle - rest_angle) to be the ERROR. where "angle" is the current position and "rest_angle" being the target or reference set point.

    I read angle_rate as the derivative term. (Current error - previous error).

    The 5 and 0.4 are the gain (importance) of each factor.
     
  17. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    Hi GopherT

    My project goal is building a rideable Segway-type device, this is only a small prototype, I want to learn as much as I can with is small prototype first, before spending money on a big one.
     
  18. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    Ok, then you really have no way to pull forward/back on the handle. Do you just want to go some speed forward while maintaining balance? For the prototype?
     
  19. bug13

    Thread Starter Well-Known Member

    Feb 13, 2012
    1,208
    38
    Say my robot will fall over if the angle is greater than +/-5 degree, no matter how fast my motors go.

    I want to say limit the tilt angle no more than +/-4 degree, by over correcting it before it's too late, so the angle is within +/-5 degree.

    Does it make sense? Or there is something else I don't know here?
     
    Last edited: Nov 18, 2013
  20. GopherT

    AAC Fanatic!

    Nov 23, 2012
    6,006
    3,763
    I think you will need to make a slight correction constantly, no matter how much or how little.
    Your mind is very good at doing his. A weak office worker with no athletic activity tries to stand on one foot and they wobble all over and fall quicky. An athletic person with no experience stands on one foot and wobbles a bit but soon over corrects and has to put the other foot down. A ballet dancer stands on one foot and can stay for a very long time with no problem. But, if you look closely, you see small muscle movements all over her leg and foot. She is correcting constantly while keeping her head completely still - hundreds of small, imperceptible corrections each minute.

    You might want to set full correction (full power) if you are already at 4-degrees. What is really important is the Derivative factor, or, how fast is it falling when it passes 4-degrees?

    Do you have full forward and reverse PWM control of your motors?
     
Loading...