Controlling an animatronics bust

Some promise in the NXP PCA9506. 40 bits, true OE, I2C interface. Downside is 56pin TSSOP

http://cache.freescale.com/files/analog/doc/data_sheet/MC33996.pdf has some promise. No OE pin, but has a dedicated PWM pin.

74HC595s is going to be the winner. I even think I have some!

So it is established that there are many advantages to using the 74HC595 for my project. However, thinking this through using them with the analog PWM on the OE line would result in the same duty cycle going to all motors enabled at the same time. While trying to wrap my head around it, I see that what I need to do is have a timer going on 8ms ticks that triggers the 74HC595 routine. Then every 1ms, I need to update the 74HC595 with the pins that will be enabled for that 1ms. So, really I am back at software PWM, right? Or.... do I need to make my PWM 1Khz (every 1ms) and adjust my individual pins every 1ms?

Either way, I can see that I will need to pulse the OE line every 1ms and update my pins in between. I can't see any other way. The reason is because one motor may be running for 800ms before it reaches it's postion, while in that time the smaller movements will require other duty cycles as they are reaching their own positions. I've got the general idea in my head, so I'll get it eventually, just wondered if someone already knows the solution.

While waiting for my shift registers to arrive, I have started working on getting just one motor working reliably using the PID library and PWM. What I have learned is that the H-Bridges just do not respond well to PWM at all. They will not move at all until you reach a PWM of around 60. But this is not linear. Once they are moving, you can decrease the PWM down to about 38 before they stop running again. The speed changes very rapidly above 60, but then very very slowly towards 255.

I think this is why I cannot seem to get the PID library to work. It cannot control the speed as it approaches zero error. However, it seems to just quit trying and leaves the motor position far off the mark. (the motors will whine if a PWM above 0 is commanded to it) The motors do not whine. They act as if they have reached their position.

Unfortunately, I think I am going to have to devise a completely custom system. I will continue to focus on one motor to develop a solution that works before worrying about integrating all 10 motors at the same time.

You may have to try to work with the voltage somehow. At the same PWM the motor with go faster or slower if you increase or decrease the voltage. I'm not even really sure how you would get that to work electronically but it's an idea.

Edit: This may or may not be correct, probably not. Motors aren't my specialty.

I could add an offset to the error output to scale it up to a minimum, but not sure how I will deal with zero then. I am going to play around with gains some more tonight. I read some more today and I think that playing with the integral gains might help. However, the fact that it is not linear (meaning once it is moving, it can take a lower PWM than if it is still) is going to pose some issues, I think.

I think the original application got around this by using a slower PWM frequency. 1ms (12.5% duty cycle) is the minimum pulse width used. At 9V, that would mean the motor would see an average voltage of 1.125V. I doubt the motors really move at that low of a voltage... but it only uses that voltage as it approaches zero error, so it was moving to being with. From the signals I looked at, it seems to always start out at full speed and only adjusts as it is reaching the setpoint.

A 12.5% duty cycle with the analogWrite values is 31. The motor does not move at an analogWrite value of 31. I imagine that has to do with the responsiveness of the H-Bridge. Using the default PWM frequency (490hz on pins 3,9,10,11), the motor output is high only for 250us at 12.5%. At 50% duty cycle, I get 1ms which is the lowest the original PWM uses. I assume that this is why it begins to turn on around that point. The highest it would ever be is 2ms for 100% duty cycle. The original application goes up to 8ms.

You can change the PWM frequency, but it will affect the timers used by other functions. BTW, pins 5 and 6 are twice as fast at 980hz.

In the end, I think I am going to just have to come up with a software PWM method.

You may be able to send a higher PWM when you start and then immediately send a lower PWM frequency. This could be done using state programming. If just starting to move or reversing direction send a higher PWM, short delay, check PID, then send the lower required PWM.

The effects you're describing sound a lot like mechanical stiction (it will take less torque to keep the motor moving than to start it moving) and also the inherent variations in the current/torque relationship of the motor as the motor moves. (Try turning a motor by hand - the motor has to overcome that lumpiness before it will move, and if you maintain constant current and then turn the spindle, you can feel how much the torque fluctuates.)

The Integral term of the PID should prevent the system from settling with a non-zero error value.

You should be able to tune the Differential term so that the system slows down and stops at the target position without any overshoots or undershoots, as long as your mechanical system is linear. (If you have non-linear movements then it might be difficult to tune the PID correctly across all positions.)

I've now got it "sorta" working. I switched over to the lower frequency PWM pins (10 and 11), removed the integral (caused WILD oscillations) and I have been trying to follow along with various tutorials on PID tuning and it still oscillates. I've got it oscillating less... but still crazy amounts.

The problem, I think is that the potentiometer changes value very quickly. I am dealing with a very small range of motion here. Just kicking the motor a little can way overshoot. I am so close to giving up on PD control (remember, I zeroed the Integral).

Raising the integral even to 1 caused the motor to go absolutely nuts. A P of 2 and a D of 1 seems to be working the best.

This is what I think I understand:

P = How much force to move towards zero position
D = Pre-act (predicting what the output will be over time)

I am just not positive that this is right for me. What I am getting out of right now is no different than my simple If currpos > pos, then move left etc... routine I was using. In fact, even that actually got it right ocassionally!

This code mostly works. It is modified from the DIY-Servo code (unfortunately I forget the author):

int PWMPin = 5;
int PWMPin2 = 6;
int potPin = A0;

float KP = 2; // PID: position multiplier (gain) was 2 Adjust to get more aggressive or conservative control
float KI = 0; // PID: Intergral multiplier (gain) was .05 set about the same as your manual response time Seconds/4
float KD = 1; // PID: Derivative multiplier (gain) http://www.expertune.com/tutor.html

int lastError = 0;
int sumError = 0;

//int iMax = 1;
//int iMin = 0;

int setPos = 0;
int dutyMax = 254;

void setup() {

pinMode(PWMPin, OUTPUT);
pinMode(PWMPin2, OUTPUT);
setPwmFrequency(PWMPin, 255);
Serial.begin(9600);

}

void loop() {

int val = analogRead(potPin);
val = map(val,0,1023,0,255);

checkInput();

int error = val - setPos;

int ms = KP * error + KD * (error - lastError) + KI * (sumError);

lastError = error;
sumError += error;

// if(sumError > iMax){
// sumError = iMax;
// }
// else if(sumError < iMin){
// sumError = iMin;
// }

// BACKWARD DIRECTION
if(ms > 0){
int motorSpeed = ms;
if(motorSpeed > dutyMax){
motorSpeed = dutyMax;
}
PWMPin = 5;
PWMPin2 = 6;

analogWrite(PWMPin2, 0);
analogWrite(PWMPin, motorSpeed);
}

// FORWARD DIRECTION
if(ms < 0){
ms = -1 * ms;
int motorSpeed = ms;
if(motorSpeed > dutyMax){
motorSpeed = dutyMax;
}

PWMPin = 6;
PWMPin2 = 5;

analogWrite(PWMPin2, 0);
analogWrite(PWMPin, motorSpeed);
}

}

void checkInput(){
if(Serial.available()){
byte ch = Serial.read();

if (ch == '+'){
setPos = setPos+5;
}
if (ch == '-'){
setPos = setPos-5;
}
if(ch == 's'){
analogWrite(PWMPin,0);

}

}

setPos=constrain(setPos,0,255);

}

void setPwmFrequency(int pin, int divisor) {
byte mode;
if(pin == 5 || pin == 6 || pin == 9 || pin == 10) { // Timer0 or Timer1
switch(divisor) {
case 1:
mode = 0x01;
break;
case 8:
mode = 0x02;
break;
case 64:
mode = 0x03;
break;
case 256:
mode = 0x04;
break;
case 1024:
mode = 0x05;
break;
default:
return;
}
}
else if(pin == 3 || pin == 11) {
switch(divisor) {
case 1:
mode = 0x01;
break;
case 8:
mode = 0x02;
break;
case 32:
mode = 0x03;
break;
case 64:
mode = 0x04;
break;
case 128:
mode = 0x05;
break;
case 256:
mode = 0x06;
break;
case 1024:
mode = 0x7;
break;
default:
return;
}
TCCR2B = TCCR2B & 0b11111000 | mode; // Timer2
}
}

But it makes a ton of noise, even at idle. And the difference between setpos and currpos needs to be very large before the motor will even move. However, it will fairly accurately move to that position and "stop." Well, I say stop, but more like it sits there and whines. It does undershoot a little, but I think that is because it is setting the pwm so low that the motor stops turning and can't make it that last bit.

I just don't think this is going to work unless I figure out to control it exactly the same way it was controlled in the original.

I can't really visualise the mechanical side of your setup. What is it you're moving? What range of angles are you moving through and how precisely are you trying to position it?

Taking account of whatever gearing you have, how precisely are you trying to position it in terms of motor revolutions?

Is there any significant mechanical inertia or friction in the system?

Once it's achieved the target position, does it need any sustained output from the motor to stay in position?

PeterH:
I can't really visualise the mechanical side of your setup.

What is it you're moving?
What range of angles are you moving through and how precisely are you trying to position it?

Taking account of whatever gearing you have, how precisely are you trying to position it in terms of motor revolutions?

Is there any significant mechanical inertia or friction in the system?

Once it's achieved the target position, does it need any sustained output from the motor to stay in position?

I attached a picture of the two different styles of motors that represent what I am working with. Currently I am experimenting with the smaller one which uses a potentiometer for position feedback. The pot is a continuous turn type. I have it connected to the A0 pin of the arduino and I am scaling that down to 0-255 for position.

The motor is being controlled through it's original H-Bridge, which is a 6 transistor H-Bridge (Mark Tilden Style). It has two inputs, FORWARD and REVERSE. A high on either will move it in that direction. The supply for the motors are 9v. There are resistors limiting the current that can go through the H-bridges.

As for precision, I would be happy with just 32 positions, in multiples of 8. That would match the original application. It does not have to land exactly on the setpoint, though future applications of the same code may need more precision. We aren't doing CNC style stuff, just moving facial parts.

I do not know the gear ratio. I'm sure it will be different for the different motors. Range of motion for this one appears to be around 90 degrees before the pot goes out of limit.

I don't know if I would consider it significant. I think the H-Bridge is really the limitation here.

No, it does not need to hold it's position once it reaches it. Not at all. The mechanisms have enough leverage that they do not move once they are in position. Two of the motors do have spring returns, so moving those would move them from a center position and they would spring back once you remove power. They were designed that way.

The main problem seems to be that the PWM cannot accurately control the H-Bridges. I really don't need "PWM" proper. Just something that will slow the motors down as it reaches position so it can stop semi-accurately.

In early experiments I used proportional control. I used delays to move the move the motors, paused, checked position and compared, and continued to move if they didn't match. I actually had decent results with that, but the delays are a problem and the motors had a stepper feel to them. It was not smooth as you can imagine.

What I really think I need is a control system that determines the magnitude and sign of the error, determines how long to just run the motor before it checks it again, and starts throttling it down as it reaches the position. But not so much that the motor just whines instead of going the last little bit. And something that when it reaches zero (or a +- range of error) that it simply stops and moves on.

In real application, all of the motors will be refreshed about once every 20ms, so if they didn't make it to their position yet, they will just continue on the next refresh or if a new position was given in that time, it will just move towards that. I don't need something constantly adjusting the motors to hold them. That is why PID in it's normal form doesn't seem like what I need. But I cannot find anything about other types of control schemes at all.

Some motors may take up to 800ms to complete a movement from one extreme to the other. The majority will be very small movements that only take a few ms to complete.

I attached a picture of what I am trying to control.

I just think I am trying too hard to make something fit that isn't going to fit and isn't what I really need in the first place.

Have you tried this method for PID tuning?

I'm going to assume that the incremental movements you need correspond to multiple turns of the motor and you aren't trying to do any clever very slow speed movements.

From the symptoms I suspect that you haven't given the motors enough control authority. I suggest you find out what range of PWM values gives you a reliable movement of about the right sort of speed range, and map the control output from the PID algorithm to a PWM duty cycle using a fairly aggressive map so that the motors come up to 100% duty cycle for relatively small control demands. This will mean that the proportional element of the PID will do the bulk of the work and will only come below 100% dc when the position is approaching the target. At that point the derivative element will start damping things down as the position gets closer and closer to the target. If the motor can simply stop and switch off when it reaches the target then you don't need an Integral component. On the servos with springs that need to be resisted, you'll need the Integral component high enough to hold the position. When you increase the Integral component you need to be careful that it doesn't get too much authority relative to the Differential component, otherwise you can get oscillations. So, if it overshoots then increase the Derivative component.

Thanks to both of you. I will take another crack at it tomorrow and read that article.

With the little bit of understanding I have, I was trying high values for P thinking it would do exactly as you describe compared to D. Usually a ratio of 1/4 to 1/8th. Even at 255P, it still oscillated like crazy.

Am I right in thinking this way:

P is the amount of force applied to the motor in proportion to the error
I is the a function of time (not 100% sure beyond that)
D is rate change/time which helps determine when it is getting closer to closing the position and allows me to stop

If I can truly wrap my head around these concepts, I know I can come up with a custom PID that uses these components and gives me exactly what I want. It doesn't help that the majority of articles I am reading have to do with chemical processes and don't seem analogous to what I am trying to do. Also, it doesn't seem that I have true linear control over the motors. I am really surprised... this is something I figured would be so common that I could find all kinds of examples and information.

Trust me, I have been reading hundreds and hundreds of pages. I am an electrical engineer so the math is not above my head. It just seems like they are all explaining it to someone that already knows it trying to be too academic.

Arrrgh!!

Most algorithms involve increasing P until it oscillates. Once you have that set P to half that. Then increase I and D to get better speed or control. Read that quick wiki page and you should get a good idea of what you want.

Alright, I finally have some code that works. It isn't perfect, but it does what I need it to do. I got rid of the I term, and am doing PD only control.

This is just with one motor at the moment. I am still going to have the issue of dealing with multiple motors (but I think I have that figured out on paper) and with needing a psuedo PWM routine because I will not have enough PWM pins. I got the shift-registers, but I haven't played with them yet. Here is the the code:

#define fPin 5 //Causes increasing values of position
#define rPin 6 //Causes decreasing values of position
#define pot A0

volatile byte currpin;

float P, I, D, output;

float Kp = .8;
float Kd = 2.4;

int setpoint = 0;
int currpos = 0;
int lastpos = 0;

int val = 0;

void setup(){
  
  Serial.begin(9600);
  pinMode(fPin, OUTPUT);
  pinMode(rPin, OUTPUT);
  
}

void loop(){
  
  char c = 0;
  int speed;
  
  if(Serial.available()){
    c = Serial.read();
    
    if(c == 't' || c == 'T'){ //Set target position

      setpoint = Serial.parseInt();
      if(setpoint >255){
        setpoint=255;
      }
      if(setpoint < 0){
        setpoint = 0;
      }
      
    }
    
    if(c == 's' || c == 'S'){ //Stop if it is going crazy

      analogWrite(fPin,0);
      analogWrite(rPin,0);
    }
  }
  
  computePID(setpoint);

  
  if(output < 0){
    currpin = fPin;
    speed = -1 * output;  
    speed = speed + 40; //Add an offset because motor will not move at PWM less than 38
    if(speed < 50) speed = 0;
    speed = constrain(speed, 0, 255);
    analogWrite(currpin,speed);
  
  } else{
      currpin = rPin;
      speed = output;
  
      speed = speed + 40;
      if(speed < 50) speed = 0;
      speed = constrain(speed, 0,255);
      analogWrite(currpin,speed);
    }

}



float computePID(int point){
  int error;
  
  currpos = analogRead(0);
  currpos = map(currpos, 0, 1023, 0, 255);
  
  error = point - currpos;
  P = error * Kp;
  D = Kd * (currpos - lastpos);
  
  output = (P - D);
  output = constrain(output, -255, 255);
  
  lastpos = currpos;
  
  return output;
  
}

Technically, with P set so low, I am not positive I am even using P. Anything higher than 1 causes oscillation. Any D values lower, causes the motor to hum. The position that it lands on is +-2 my setpoint