Controlling an animatronics bust

In your third schematic, the "my controller" could probably be replaced with an arduino/mega controller and a number of h-bridges smilar to below to drive your motors.

http://www.ebay.com/itm/L298N-DC-Stepper-Motor-Dual-H-Bridge-Drive-Controller-Board-Module-for-Arduino-/380520063503?pt=LH_DefaultDomain_0&hash=item5898c3620f

The "my controller" IS my arduino and associated hardware. I already have the h-bridges, the pullups, proper caps and filtering, the power supply, etc... The hardware is all done (the easy part for me.)

I am working on the software and that is where I need help.

ok, I think I have found a better way to approach this... since I am currently trying to tie all the pieces together and am really asking for birds-eye view help, it makes sense to start with a flow chart. That is really what I need help with, generically. Individually I will need some help with some of the pieces, but for now it is the "big picture" part of it.

I have attached the beginning of my flow chart. If someone would be willing to look it over and help me get it right, I would appreciate that. I have attached both a picture of it and the actual flowchart file drawn in the freeware program "Diagram Designer" Diagram Designer

Elvis_flow.jpg

elvis_flow.ddd (1.12 KB)

Just refreshing my code as I have added a bunch more and am starting to link things up better. I may have bungled up the PID routine as it was a bit confusing which variables I needed to track externally between calls. If anyone sees glaring errors, please let me know. It's not completed code, so I am aware that it won't just "run" as it is.

Elvis.c (8.83 KB)

Just of the top of my head I would initialize the motors in a loop. I'm pretty sure there is a nicer way to do it, but a loop would definitely simplify it a lot.

Edit: you might also want to look at the PID library
http://playground.arduino.cc/Code/PIDLibrary

Thanks. I have actually looked at the PID library and it seemed too complex for what I am trying to do, especially with trying to tie in a bunch of motors. It could be that I am just not understanding it fully. if someone is willing to help me figure out how to incorporate it, I would be extremely grateful.

As for the loop suggestion. That is the idea. New, incoming serial commands get loaded into the array. A loop pulls the commands out of the array and updates the motors. If no new commands were loaded, it just refreshes the motors to the last position they were in the array (in case they had moved somehow.) That generically holds the position. Upon first initialization, I load all the default positions into the array for neutral. And the head will move into a nuetral state on the next update.

So, I am technically only commanding a single motor at a time but instead of directly moving the motor I load the command into an array, and a timer triggers the update routine and refreshes the motors.

The way I would approach this is to create an object for each servo that you want to control. Each servo object would use an input position sensing object to determine the position of the output, and contain a PID instance (or other feedback algorithm if you prefer) to control the power/direction output requirements.

If you need to support different types of position sensing (I think you implied that in a different post) then use a class hierarchy with a base class providing the interface and common functionality, and derived classes providing the alternative implementations.

Design your servo class with a method that can be called periodically to read the current position, compare that to the commanded position, perform the feedback calculation and generate and send the output control.

I'd represent the error input to the PID and the power and direction outputs using signed integer values.

In setup you would create and configure the required servo instances and store then in an array, and then in loop() you would call each servo's update method at regular intervals. If you want to be able to receive servo positioning commands too then you'd configure the interface for that and in loop() you'd also check for any incoming commands, parse them, and send them to the relevant servo. It doesn't sound particularly complicated, and the most critical part would be designing the class hierarchies for your servo and input position sensor so that you avoid code duplication and are able to treat the different implementation types the same once you have instantiated them.

PeterH,

That pretty much sounds like what I am aiming for in my attached code. The beginnings of it anyway. I still have to determine how to handle feedback structure. However, I am using structures instead of classes. I am really unfamiliar with the concept of classes just yet.

I like the way you are suggesting, just trying to figure out how to implement it. You say error input to my PID calculations, but isn't it the other way around? Doesn't the PID calculate the error as a result? Then I use the error to determine the direction to move and the speed (in another routine, or just do that in the PID routine itself?)

No, you calculate the error and feed it into a PID routine. Libraries may do the calculation for you, but the versatility of the PID is that it is irrespective of the inputs/outputs. It is really just a zero finding scheme on the error function.

laadams85:
No, you calculate the error and feed it into a PID routine. Libraries may do the calculation for you, but the versatility of the PID is that it is irrespective of the inputs/outputs. It is really just a zero finding scheme on the error function.

hmm... according to my notes from reseearch, this is the PID formula:

error = P * Kp + I * Ki + D * Kd);

P, I, and D

are calculated like this:

P = Current position - Set position
D = P - Last_P

So, perhaps we are defining error differently? As my notes have error as the entire PID formula.

Is error in your terms equal to currpos - setpos?

Thanks

Retroplayer:
Is error in your terms equal to currpos - setpos?

Yes, in the context of a closed loop feedback system the difference between the actual and required output is termed the error.

The notes you quoted are broadly along the right lines but don't account for time correctly, so don't put too much trust in them as an explanation of how PID works.

Retroplayer:
I am using structures instead of classes. I am really unfamiliar with the concept of classes just yet.

Structures let you hold the set of data associated with a servo but don't give you the ability to associate behaviour with it and don't (without a lot of work) enable you to deal with similar but not identical things.

Classes enable a much more comprehensive solution. You can define a simple common interface to a servo and then provide multiple different implementations that look the same from the outside but behave differently. So you could have some servos that use a potentiometer for feedback, and some that use an encoder to track movement, and some that are implemented using stepper motors, and some that are standard RC servos controlled by PWM, and fro the point of view of your control logic these would all behave the same.

It would be well worth your time researching the concepts of abstract typing, polymorphism and inheritance because they are key to using classes effectively, and will make your solution much, much simpler.

The typical definition of error is expected response - actual response. With servos its input position - actual position. The definition that you have is the output of the PID, with P being the error, I being the integral of the error and D being the derivative of the error.

Edit: As Peter said you are missing the dt term

Here is some pseudo code from wiki

previous_error = 0
integral = 0
start:
error = setpoint - measured_value
integral = integral + errordt
derivative = (error - previous_error)/dt
output = Kp
error + Kiintegral + Kdderivative
previous_error = error
wait(dt)
goto start

For the arduino instead of setting the dt value and the looping you would instead loop and then measure the dt.

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.