Controlling an animatronics bust

Is the arduino millis() function good source for determining dt? So something along the lines of this:

int PIDcalc(){

I = 0;
dt = millis() - prev_millis;

error = setPos - currPos;


I = I +error*dt;
D = (error-prev_error)/dt;

output = (error * Kp + I * Ki + D * Kd);

prev_error = error;

prev_millis = millis();
return output;

}

Or since I am calling this in a timed loop, would it be smarter to just make dt equal the amount of time that changed between calls?

The million dollar dumb question is: what do I do with output now to determine forward or reverse and speed? Should I just have that calculation done right in this routine?
Finally, what variables do I need to keep track of outside of this routine for handling the several motors?

Thanks!

Is the arduino millis() function good source for determining dt?

Its the best you will get without using an outside timer.
This method isn't exactly correct, but it is a good approximation. What you are trying to do is figure out what the error will be in the future based on the current slope of the error. So dt should technically be the time step between now and the next step. If your time steps are more or less uniform this isn't a problem. If they are all over the place then you are gonna have problems.

The million dollar dumb question is: what do I do with output now to determine forward or reverse and speed? Should I just have that calculation done right in this routine?

The PID routine takes care of direction. If you have overshot then the output will be in the opposite direction from before because your error will of the opposite sign from before. You set your output to the output variable of your function

Finally, what variables do I need to keep track of outside of this routine for handling the several motors?

You should make your PID a function in which you pass it the set_position, the current_position, the previous_milli, and the previous_error with previous_milli and previous_error being reference variables so that you can set them in the function.

You should really look at the code in the PID library, it will answer a lot of your questions. One thing I'm not so sure if is why they subtract the D term.

/* Compute() **********************************************************************
 *     This, as they say, is where the magic happens.  this function should be called
 *   every time "void loop()" executes.  the function will decide for itself whether a new
 *   pid Output needs to be computed.  returns true when the output is computed,
 *   false when nothing has been done.
 **********************************************************************************/ 
bool PID::Compute()
{
   if(!inAuto) return false;
   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
	  double input = *myInput;
      double error = *mySetpoint - input;
      ITerm+= (ki * error);
      if(ITerm > outMax) ITerm= outMax;
      else if(ITerm < outMin) ITerm= outMin;
      double dInput = (input - lastInput);
 
      /*Compute PID Output*/
      double output = kp * error + ITerm- kd * dInput;
      
	  if(output > outMax) output = outMax;
      else if(output < outMin) output = outMin;
	  *myOutput = output;
	  
      /*Remember some variables for next time*/
      lastInput = input;
      lastTime = now;
	  return true;
   }
   else return false;
}

Also followed the links on the library page an stumbled on this. Might be explanatory.
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

Thanks, given that I now know a bit more about this thanks to you guys, I will have another look at the PID library. I may come back with some questions, of course.

They definitely need better examples for the PID library! Grr!! I do understand it a little better now, though.

The library takes these values:

input
output
setpos
Kp
Ki
Kd
and mode? I think I want to use DIRECT

So, for each motor, I create an instance of the class (I'll call it just motorPID)

My inputs the current position from the feedback source
My output is just a variable that I use (not sure exactly how yet)
my setpos is the position I want my motor to equal when it is done moving
Kp
Ki
Kd

those are probably going to be different for each motor, aren't they?

The output is going to be a double? And my input is expected to be a double? I have to completely change this library if you know I am not going to be using doubles anywhere, unless I just don't care about the RAM it eats up (which I do.)

I can use the SetInputLimits to 0-255 which is what I will be using for position, except for two which will be 32 and 64

I can use the SetOutputLimits to umm... still not exactly sure what I am doing with the output...

Logically, I need direction which will control a different pin and speed.

Is someone willing to work through setting this up with me?

Once I get this all going and understand it, I think I might have to write up a much better example for the PID library since controlling motors like this is probably the number one use for it.

I'm not very good with the electronics side of this as I am more of a numerics nut, but I will help out at much as I can. How do you control the motors regularly without PID, what outputs do you send. How is the motor connected to the arduino. I know you mentioned this earlier but what are you using to measure position and what is the range of values you get from these.

Edit: I'll let you figure out the electronics side if it but, it basically comes down to tuning your K parameters. However you switch the direction of your motor depends on if the output is positive or negative. I have attached two screenshots showing the calculations with just the P(error) term. As you can see if I overshoot the set point my output is then negative, this is when you'd switch motor direction. So you just need to tune your system to give you the desired outputs for your motor.

From wiki

If the system must remain online, one tuning method is to first set KI and KD values to zero. Increase the KP until the output of the loop oscillates, then the Kp should be set to approximately half of that value for a "quarter amplitude decay" type response. Then increase KI until any offset is corrected in sufficient time for the process. However, too much KI will cause instability. Finally, increase KD, if required, until the loop is acceptably quick to reach its reference after a load disturbance. However, too much KD will cause excessive response and overshoot. A fast PID loop tuning usually overshoots slightly to reach the setpoint more quickly; however, some systems cannot accept overshoot, in which case an over-damped closed-loop system is required, which will require a KP setting significantly less than half that of the KP setting that was causing oscillation.

Too_High_KP.PNG

decent_KP.PNG

laadams85:
I'm not very good with the electronics side of this as I am more of a numerics nut, but I will help out at much as I can. How do you control the motors regularly without PID, what outputs do you send. How is the motor connected to the arduino. I know you mentioned this earlier but what are you using to measure position and what is the range of values you get from these.

Perfect, because I am an electrical engineer, so the electronics are always easy for me, but the code is the devil that haunts me! I am also a bit of a mechanical guru. I would happy to return my help with anything electronic you need!

The H-bridges have two inputs for each motor (all 10 are identical) there is a forward and a reverse pin. A logic high on either of these will move the mechanisms L/U or R/D respectively. 0 and 0 do nothing, 1 and 1 is bad, it will short out the power supply. I am trying to think of a clever way to always control these in a way that they are never high at the same time. If you look at the truth table:

0 0 = No movement
0 1 = Left or Up
1 0 = Right or Down
1 1 = Invalid

You can see that is an XOR, so if I have a way to always tie these two pins together within the code, I can use an XOR operator on them to ensure they are never high at the same time. But that is separate from what we are trying to accomplish at the moment.

As for position, 8 motors have potentiometers connected to them and 2 of them use an optical tach and limit switches.

The potentiometers are straightforward. The tachs and limits are slightly more complex. If you need to know the details of them, just ask, but I am using an interrupt from the tachs in my code already to count between 0 and 32 for one motor and 0 and 64 for another. These numbers align to 1mm of linear travel per step. That work is done.

So, pots will give a position value between 0 and 255 and the linear actuators will give values between 0 and 32 and 0 and 64. I could also just scale the linears to make them also 0 and 255. Not a really big deal.

See above edited comment^

I am trying to think of a clever way to always control these in a way that they are never high at the same time.

Somebody can tell me if I'm wrong but true/false should resolve as 1 and 0

//send to h-bridge
digitalWrite(pin1,output>1e-6);
digitalWrite(pin2,output<-1e-6);

if output is 0 then h-bridge gets 0 0
if output is >0 then h-bridge gets 1 0
if output is <0 then h-bridge gets 0 1
Notice I used 1e-6 and -1e-6. I don't like to use exactly zero to allow a little lee-way. Mileage may vary.

1e-6

e as in logarithmic e, engineering exponent or is that a variable of some sort?

I apologize if that seems like a dumb question.

Otherwise, I completely understand what you are doing there. Just converting the output into expressions that will evaluate true or false based on the sign of the error. Right?

1e-6

e as in logarithmic e, engineering exponent or is that a variable of some sort?

1e-6 as an exponential number 1x10^-6 or 0.00001. You can use any value slightly greater than and slightly less than 0. This makes sure that you don't continue to oscillate around zero due to machine precision.

C isn't my strong suit but I know programming well, regardless of the language.

Just converting the output into expressions that will evaluate true or false based on the sign of the error. Right?

That's correct.

How do you control the speed of the motor?

laadams85:
How do you control the speed of the motor?

That is what I expected that you meant. Exponent in C is a bit more complicated. I can figure that out, though.

Speed is another issue entirely. I am looking at a softPWM library right now, but I will have to heavily modify it, I think. Obviously I need 20 PWMs, so the hardware PWM isn't going to work. I am trying to get enough of the gist of it to try to write something very basic to do it.

I will be duplicating the stock way it is done (I am modifying existing hardware) right now, it uses a 125Hz frequency PWM. That's 8ms per period. The motor speed is controlled in 1ms increments (12.5% duty cycle steps.) So there are only 8 speeds + zero/off. It seems that I should be able to come up with something even simpler than the library to manage this. I am trying my best not to have to use external libraries (unless I make my own) as they tend to be bloated with lots of extra stuff that increases your code size, and sometimes hog resources I might need, for functions that I am not using.

The grand result is for me to package this up and make it portable enough that people could use it in their own similiar projects. That's not a priority, but I am trying to keep that in mind as I go.

come to think of it, I am really fine with an error of 1 or -1. The original application only supported 32 possible positions in the first place. My movements are quite small, so 255 steps wouldn't even be noticeable.

Be careful with that. You'll end up oscillating around your set point if you don't land on it exactly.

So is there a way to set a deadband (or we call it hystiresis in the electronics world) where it is fine as long as it is within a range of the desired position? Like I don't really care all that much if it stops at 5 or 9 when I commanded 7. The difference would not be noticeable in my application. It seems like that would prevent a lot of oscillation.

A good PID algorithm would have this included. In a thermostat for example, you wouldn't want it turning your heater on, then your air conditioner over and over trying to maintain an exact temp.

tolerance=2
digitalWrite(pin1,output>tolerance);
digitalWrite(pin2,output<-tolerance);

You can do something like this. Also thermostats have heat/cool settings for this reason. You don't try to cool your house you just turn off the heat. This only works because your house will gravitate towards the outdoor temperature.

What H-Bridge are you using? Shouldn't you be able to send an analog signal to your h-bridge to control the speed?

The H-Bridges are already built into the hardware. They are 6-transistor (Mark Tilden style) H-Bridges. I can put the PWM on either the forward or reverse lines depending one which direction I want to go, but no, there isn't a separate PWM input.

It "may" be possible to use gates to allow PWM.... hey wait a minute....

In the original controller, all of the motor lines are on latches. I beeeeeeeeet, they are providing PWM on the OE lines. That way they just set which pins they want to be HIGH, and pulse the OE line with the PWM signal.

I would have to add latches into my circuit, but that reduces my PWM needs down to the point that I can use the hardware PWM. Though I can't use the built in analogWrite because it is too fast and makes the motors go nuts and I don't see a way of changing that without changing the built in arduino code.

I was hoping to do this without adding additional circuitry, but now I am thinking that may be the best way.

I just thought of something. To make sure you don't fry your H-Bridge your gonna want to zero out your pins before sending the next command. Lets say your last command was 0 1. When you reverse the direction the first thing you do is set pin1 to 1. So then you have a 1 1 state because your 2nd pin hasn't been set yet. You don't have to worry about it when you go from 1 0 to 0 1 because your 1 pin is set first.

I'm not even sure it really matters and would depend on how much time passes between digital writes.

tolerance=2
digitalWrite(pin2,0);//make sure a 1 1 state isn't sent to h-bridge.

digitalWrite(pin1,output>tolerance);
digitalWrite(pin2,output<-tolerance);

Retroplayer:
I beeeeeeeeet, they are providing PWM on the OE lines. That way they just set which pins they want to be HIGH, and pulse the OE line with the PWM signal.

That's how my motor driver shield works and seems like the most sensible design to me.

It's possible to change the PWM frequency and I'm sure you'll find that Nick Gammon's forum has a tutorial covering that.