Adding PID


I am testing now adding PID to the servo controller code used for tracking.

I am using a very simple PID controller to avoid impact on tracking speed and so far does not seems to affect it much.

The servos used are classic PWM servos: HS-5645MG, servo pulses are triggered every 10 ms and the PID controller recalculates the position every 5 ms. PID settings and servo limits can be adjusted over serial port on the flight.

This is actually the first time I ever used a PID controller, really hopping someone may be able to suggest on how to improve and make it even better. Anyway, the code looks like this:

Setpoint     = (cogX-midX) ((maxPan + minPan)/ linewidth);
long error   = Setpoint - panPeriod;
ITermPan    += (pidKi * error);
long dInput  = (panPeriod - lastInputPan);
lastInputPan = panPeriod;

panPeriod    = (pidKp * error + ITermPan*SampleTime 
                 - pidKd*dInput/SampleTime);

if (panPeriod > maxPan){
  panPeriod = maxPan;
}
if (panPeriod < -minPan){
  panPeriod = -minPan;
}
Advertisements

42 thoughts on “Adding PID

    • Hi Diego, thank you.
      I actually have the limits set on the panPeriod (output) and works pretty well. For performance reasons I need to cut down as much as possible on any operations that may impact the tracking speed unless there is an improvement in functionality.
      By the way, my board can work with either OV7670 or OV7725 too but in order to reduce the cost I am using OV7670 cameras, else will be more expensive.

      • Yes, it works fine, but consider the following scenario: location of the object is slightly beyond the center, panPeriod is in the limit, so servos doesnt actually move, error is constant over time, therefor ITermPan increases until it gets its top value (for example +32767 for int16), then it just goes to the lowest value (-32767 ) and thats a huge glitch to panPeriod. Anyway its just a little bug that may not even happen.

  1. Your Pan/Tilt unit follows Mario perfectly. Nice work. I am trying to accomplish the same with two HS-645MG servos, but I am having trouble calibrating the PID so would you mind sharing your complete code? TIA!

    • The PID code is actually the one posted. The only difference is that between every PID call i can change the kp, ki, kd on the fly through serial port. Once i find the ones that gave me the best results i’ll then store them in EEPROM.

      • I see. So, would you mind posting the values for pidKp, pidKi, and pidKd that worked best for you? Thanks!

      • Values will be different from hardware to hardware. My sampling time is 10 ms. The values i use are kp=1, ki=0.8, kd=0.2, yours may be different….

      • I failed to ask one more thing. In your code you subtract the derivative term, Why? Shouldn’t it be added? e.g.

        Kp*(error) + Ki*(integral) + Kd*(derivative)

      • Thanks for the values. Regarding subtract (Kd * derivative of Input), I found out that you are doing that because Setpoint is always changing so you are using what is known as “Derivative on Measurement”. Thanks again!

  2. I end up here reading the news comments on this post and know I have a question.
    You say that the position is recalculated every 5ms, Does it mean that you can work out the cogX (the position you wanna track) that fast? It would be, 200 fps!!

    Thanks

    • No, i recalculate the COG every 30 ms, i trigger PID every 10 ms then feed it to the servos.
      This way i have 3 PID cycles to get servos in the position, this way i can get the servos moving smooth.

      • This is quite confusing. In the original post you wrote “servo pulses are triggered every 10 ms and the PID controller recalculates the position every 5 ms”. Now you are saying “I recalculate the COG every 30 ms, i trigger PID every 10 ms then feed it to the servos.” So, which one is correct?

        It feels like your October post “Mavericks is cool!” vs the December one “Mavericks sucks…”

        Also, if you trigger the PID every 10 ms (you get 2 extra cycles to adjust the position to your last known destination), then how can you recalculate the panPeriod using the same cogX and Setpoint those extra 2 cycles?

      • The servo pulses are triggered every 10 ms, as per Hitec specs. the COG is calculated every 30 ms, at the end of each frame. there will be 3 PID cycles for each frame. the PID is triggered by an interrupt, the result will update servo position/pulses.
        This is how it works now. there is no use to calculate faster as the servo pulses can not be triggered faster than 10 ms for a Hitec servo as far as i know…

        abt Mavericks: it is “cool”: my machine runs cooler, battery last longer, etc. However, when doing development i am facing problems and the system keep restarting, mostly cause of the serial drivers and sometime some memory leaks in Processing. While for a regular user is an improvement, when doing development it feels like before the upgrade my system was more stable. Hope it make sense…

      • Thanks for the clarification. It makes a little bit more sense now.

        So, just to make sure I understood.

        Are you saying that since the COG is calculated every 30 ms, then you send the same same panPeriod 3 times to the servos (3 PID cycles for each frame) before the interrupt sends a new COG to the PID?

      • I mean i get the COG calculated every frame (30 ms), i have an interrupt every 10 ms calling PID and calculate the servo position using and calls the servo update. This means, the servos are updated 3 times within a frame, using COG as setpoint. this way servo’s position is recalculated and updated 10 times a second, which means servos will move smooth ;-)

    • Ok, let me run some tests since I think I now understand what you are doing.

      One more question.

      Are you getting a feedback from the servos to know what position they are in? Otherwise, what do you need the integral term for? You can’t use anything that expects to be able to reach a set point?

      • Just for clarification. The reason I asked the previous question is because in this project (see link below), the CMUcam4 only uses PD instead of PID and it accomplishes similar result (i.e. servos move smooth).

      • I know the video :-) If you look careful the servo movement is not that smooth, and the tracking speed is quite slow, I guess 10-15 FPS? Towards the end of the clip the CMU lost the tracking then recovered. If you are looking at my video, the target object moves much faster and there is a smooth tracking and no visible delay.

      • I agree. Yours is indeed the best one I have seen too.

        I’m still trying to figure out why you need the Integral Term though. Of course, unless you are getting a feedback from the servos to know what position they are in, but I don’t think that your case.

        I also think that your code above doesn’t tell the whole story since the PID will have to handle 3 cycles with the same CogX and reset it again once you get a new CogX value.

      • The COG is the same, however, the extra 2 cycles will help since I am using them to get the servos into position. Let’s put it this way: if you have to move 30 degrees in 30 ms if you are doing it every 30 ms the servo movement will not be smooth, right? If you move 10 degrees every 10 ms the servo will move smoother. Also, moving smooth will improve the quality of the image captured (less/no movement blur on the image).

        Btw, Hitec specs recommends servo position updates between 10 to 20 ms for a smooth movement, this is where it started ;-)

      • In my tests playing with Ki and using extra cycles will help improve smoothness while reducing the oscillations caused by overshooting of setpoint value.

  3. I think I got it. This has been a challenge though since I am not utilizing the same hardware you are. I’m using the camera from a Nexus S android device and I can only get 15fps at a minimum resolution of 176×144. At that refresh rate it was really tricky to have the servos moving smooth. However, you pointed me to the right direction with your approach of 3 PID cycles for each frame.

    The android device communicates with the IOIO board via Bluetooth; which adds some latency too.

    I’m also using slower Hitec servos. The 422 has a dead-band of 8usec while yours (the 5645MG) has 1usec dead-band. Trust me that makes a bid different, in particular moving vertically.

    The whole implementation felt like someone asked me to run a Formula One car on a track with speed bumps, but I learned a lot though.

    I might try with faster servos at some point, but for now I’m happy with the results.

    Here is a video showing the Android device tracking BugDroid:

    Thanks, and best regards!

    • Hi AI.

      I’ve been tried to finish my project which is similar to yours. Could you help me about this?
      How do you calculate setpoint? I can’t understand why nasulica calculate setpoint like that.

  4. Hi Nasulica.
    I have some questions for you.
    Does midX have the same value with linewidth?
    The second one, What operator do you use to calculate setpoint? There is a missing operator between (cogx-midx) ‘ ? ‘ (bla..bla) .
    I hope you can answer my question quickly, thanks

      • You calculated Setpoint = (cogX-midX) ((maxPan + minPan)/ linewidth), and you just said midX should be linewidth/2 (in your case 160) while cogX is the X coordinate you wanna track. So, there is a condition that cogX < midX. Do you use absolute to ignore the negative sign?

  5. why did you calculate setpoint like that? Is that like a mapping?
    You calculated Setpoint = (cogX-midX) ((maxPan + minPan)/ linewidth), and you just said midX should be linewidth/2 (in your case 160) while cogX is the X coordinate you wanna track. So, there is a condition that cogX < midX. Do you use absolute to ignore the negative sign?

  6. why did you calculate setpoint like that? Is that like a mapping?
    You calculated Setpoint = (cogX-midX) ((maxPan + minPan)/ linewidth), and you just said midX should be linewidth/2 (in your case 160) while cogX is the X coordinate you wanna track. So, there is a condition that cogX < midX. Do you use absolute to ignore the negative sign?

  7. so, midX and midY act like setpoint right?
    And then, did you give an initial condition to your servo before receiving data from the computer?

    • The module is independent from computer, the entire processing and tracking is running on the CTS module. I used the computer to display the image only.

      MidX and midY is only to locate “zero servo” on the image relative to “zero image”, nothing else.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s