PID with servos

[Ro-Bot-X]

So here is my questions for anyone who has done this before and is able to clear things up for me.

First, a few words about my setup. I am using ATmega168 @ 16MHz, 2 modified Futaba S3004 servos for driving, Arduino software for the code. The robot is powered from a pack of 6 NiMH batteries. I have quadrature encoders on both wheels, with 32 interrupts (clicks) per wheel rotation, with 0.5 cm per click.

What I want to do is give the robot the Distance it needs to travel or the Angle it needs to turn. A negative distance means go in reverse. A positive turn value means CW turn and a negative means CCW turn (should I reverse these?).

What the robot should do is accelerate to the specified speed (or max speed), drive straight and slow down before it will reach the set distance. Same for turns.

The robot will have a function that will receive commands or calculate new distance segments the robot should travel and the turns. A separate Driving function will take care of the execution of the set distance. I have 2 ISR that will return left and right encoder counts. But I am not sure how to set up the PID routine (should I use an interrupt?) and especially how the PID routine will adjust the speed for the servos. Also, I need a way to do the acc-slow down part of the function.

Any help would be greatly appreciate.

[michaelsane]

First off you are going to need to define your goals.  Lets just say you want your robot to travel forward 1 foot.  Do the math given the CPR of your encoders to determine the amount of counts that will take place during a 1 foot distance.  Create a variable that will contain where you want to be (the counts for 1 ft distance we will call this Cd) and where you are (how many counts the robot has already traveled we will call this Ct).  You may also want to use another variable to calculate the difference between where the robot is and where it is supposed to be, call this variable error.  Use an interrupt to increment the Ct variable for each cycle.  When the error variable = 0 the robot will stop moving.

As for the speed issue you can write code that will change the speed of the motor depending on the Ct variable.  If you want to make your robot slow down at the same distance before the target it would be as simple as checking the Ct variable and when it reaches Cd - the set distance, you would slow down for the rest of the trip.  It would be a little bit more tricky if you wanted the slow down period to be proportional to the distance traveled eg. slowing the robot down when the trip is 9/10 complete.  If you really want to slow it down proportionally just multiply the amount of the trip you want to go full speed (in a decimal) times the Cd value then compare that to Ct to slow the motors down.

[szhang]

What the robot should do is accelerate to the specified speed (or max speed), drive straight and slow down before it will reach the set distance. Same for turns.

The robot will have a function that will receive commands or calculate new distance segments the robot should travel and the turns. A separate Driving function will take care of the execution of the set distance. I have 2 ISR that will return left and right encoder counts. But I am not sure how to set up the PID routine (should I use an interrupt?) and especially how the PID routine will adjust the speed for the servos. Also, I need a way to do the acc-slow down part of the function.

Any help would be greatly appreciate.

I am not sure if I understood you correctly, but see if this helps.

Servos have their own PD controller, so I am assuming you're talking about hacked/continous servos.

I am assuming you know how a PID controller works.  So what you do is compute the distance traveled and your current velocity based on your encoder readings, and then just send a  PWM command=p_gain*distance+d_gain*velocity (optional integral term)  This itself should slow down as you approach the set point. The PID routine could probably just sit in a loop.  Your interrupt handler should instead integrate the position readings and output the distance/velocity.

I have done "trajectory" controllers that are basic PD controllers except they will do a smooth trajectory to servo to the new point in (i.e. accelerate slowly on startup instead of instantly like normal PD controllers).  The controller also takes a time argument and it will servo to the new point in exactly the time specified (or try to).  For that you have to calculate your desired velocity and try to have the PD controller match that.

[brijesh]

Have been working on the same issue for sometime now.

Here are things I have learned so far.

1) "PWM command=p_gain*distance+d_gain*velocity" this one works great for servo position control, where the servo arm is traveling less than 360 degree's. The distance is finite and  bounded. Hence the term p_gain*distance will be finite and bounded. For mobile robots the story is quite different. Distance you want to travel can be large. The term p_gain*distance, can be large and saturate your pwm output signal.

Usually in mobile robots, you control the velocity of the motors using PID control and then have another control loop/logic to tell the robot to stop when it has traveled the desired distance.

It is possible to directly run a position control loop. Here is an example of such an implementation.
http://www.barello.net/Papers/Motion_Control/index.htm

brijesh

[szhang]

You should always clamp your PID outputs, set a maximum PWM duty cycle. For linear distance you'll need to check the velocity of the proportional error to know when to stop.

[Ro-Bot-X]

Thanks guys, I think I'll be able to pull it out. If not, you'll know...

Edit: I just hoped that someone has done it before and knows EXACTLY what should be done. In theory I have the ideea, in practice... I lack programming skills.

[Rebelgium]

Would you be so kind to post your solution (code) when you figure it out ro-bot-x ?

[michaelsane]

Really you just need a bunch of condition checking with while/for  loops (use an interrupt to increment the variable being checked) and possibly some If/Switch statements.