PID servo control - torque issue

background:

I'm trying to control some geared DC servo motors with PID rather than the built in electronics and PWM. I'm hoping to achieve smoother and more controllable motion from a cheap hobby servo. I intend to use these hacked servos in a miniature pick and place delta robot. anyways, i've removed the control PCB, and soldered leads onto the motor and potentiometer. i'm driving the motor with an L293d dual Hbridge IC, and controlling everything with an arduino uno. I've written some simple code using the PID library by Brett Beauregard to drive the motor back and forth, pausing in between. so far this is all working as it should.

The problem:

tuning the PID values seems to be a tricky process, and I don't think i've got it really right, but I can achieve relatively smooth movement, and a stable landing without too much oscillation. probably this will get better as i add in some tighter timing constraints. the problem i've encountered is one of torque. when the motor is just holding position it's very strong, but when it's moving, it's very weak. if I attempt to slow it down it gets even weaker. I'm not sure if this is a tuning issue or something to do with my code... these motors are usually pretty strong with the original control, so i know they can do better.

I have a feeling it's my code.. at this point i'm just incrementing the pid setpoint each cycle to try to get smooth controllable motion. I think it never sends full power to the motor because it's always so close to the setpoint.

here's my code (i'm sorry it's kindof sloppy.. there's a bunch of stuff in there still from when i was sending setpoints directly through the serial monitor):

#include <PID_v1.h>

float speedFactor = .5; //actually just steps per tick

//feedback pins coming from motors (pots)
const int feedbackPin[3] = {A0, A1, A2};

//PWM pins to motor drivers
const int torquePin[3] = {10,11,12}; 

//direction pins to motor drivers
const int forwardsPin[3] = {3,5,7}; 
const int backwardsPin[3] = {4,6,8}; 

//for serial input
const byte numChars = 5;
char receivedChars[numChars];
float integerFromPC = 500;
boolean newData = false;


//PID tunings
float Kp1 =12;
float Ki1 = .00;
float Kd1 = 0.07;
float Kp2 = 9;
float Ki2 = .1;
float Kd2 = .1;
float Kp3 = 9;
float Ki3 = .1;
float Kd3 = .1;

// these are all 3 dimensional arrays because i'm going to have 3 motors. only one is connected at this point. 

float setPoint[3] = {0,0,0};
float feedback[3] = {0,0,0};
float error[3] = {0,0,0};
float output[3] = {0,0,0};
float torque[3] = {0,0,0};
float currentPoint[3] = {0,0,0};
float nextPoint[3] = {0,0,0};
float currentGoal[3] = {50,50,50};
boolean whichWay[3] = {true,true,true};
boolean paused = false;
float pauseCount=0;
int i;

PID pid1(&error[0], &torque[0], 0, Kp1, Ki1 ,Kd1, REVERSE);
PID pid2(&error[1], &torque[1], 0, Kp2, Ki2,Kd2, REVERSE);
PID pid3(&error[2], &torque[2], 0, Kp3, Ki3,Kd3, REVERSE);

void setup() {
  
  // i've been using the serial port for debugging, but it makes things run slow
  // which affects the pid tuning, so i turn off the output function when testing. 
  Serial.begin(19200);
  Serial.println("Ready");
  
  get_feedback();

  for (i=0; i<=2; i++){
    setPoint[i]= feedback[i];
    currentPoint[i]=feedback[i];
  }

  calc_error();

  //set output pins
for (i=0; i<=2; i++){
  pinMode(forwardsPin[i], OUTPUT);
  pinMode(backwardsPin[i], OUTPUT);
  pinMode(torquePin[i], OUTPUT);
}

  pid1.SetSampleTime(2);
  pid1.SetMode(AUTOMATIC);
  pid2.SetSampleTime(2);
  pid2.SetMode(AUTOMATIC);  
  pid3.SetSampleTime(2);
  pid3.SetMode(AUTOMATIC);  
}

void loop() {
  
  get_feedback();  

  set_setPoint();
  //check_input();  //right now we're using the serial port to change the setpoint, but this will be changed later
  
 // display_data(); //comment out this line to improve performance once the motors are tuned
  
  calc_error();
  
  move_motors(); //this includes the PID calculations for each axis.. consider changing this to two separate functions to improve motor synchronization

  //delay(100);

}

void get_feedback(){
  for (i=0; i<=2; i++){
    feedback[i] = analogRead(feedbackPin[i]);
  }
}
void check_input() {
 recvWithStartEndMarkers();
   parseData() ;
   storeNewData() ;
 }


void calc_error(){
  for(i=0; i<=2; i++){
    error[i] = abs(setPoint[i] - feedback[i]);  
  }
}

void set_setPoint() {
  for (i=0; i<=0; i++){
    if (paused==false){
      
      if (currentPoint[i]>=950) {
        whichWay[i]=false;
        paused=true;
        pause();
      }
      if (currentPoint[i]<=50) {
        whichWay[i]=true;
        paused=true;
        pause();
      }
      if (whichWay[i]==true) {
        if ((abs(feedback[i]-setPoint[i]))<5){
        setPoint[i]=(currentPoint[i]=currentPoint[i]+speedFactor);
        }
      }
      else {
        if ((abs(feedback[i]-setPoint[i]))<5){
        setPoint[i]=(currentPoint[i]=currentPoint[i]-speedFactor);  
        }
      } 
    }
    else {
      pause(); 
    }
  }
}

void pause(){
  if (pauseCount==10000){
    paused=false;
    pauseCount=0;
  }
  else {
    pauseCount++;
  }
}
void recvWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;
 
 // if (Serial.available() > 0) {
    while (Serial.available() > 0 && newData == false) {
        rc = Serial.read();

        if (recvInProgress == true) {
            if (rc != endMarker) {
                receivedChars[ndx] = rc;
                ndx++;
                if (ndx >= numChars) {
                    ndx = numChars - 1;
                }
            }
            else {
                receivedChars[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }

        else if (rc == startMarker) {
            recvInProgress = true;
        }
    }
}

void storeNewData() {
    if (newData == true) {
      set_setPoint();
      newData = false;
    }
}
void parseData() {
     
    integerFromPC = atoi(receivedChars);     // convert this part to an integer
    
}

void display_data(){
  for (i=0;i<=0; i++){
    Serial.print(feedback[i]);
    Serial.print(" -- ");
    Serial.print(setPoint[i]);
    Serial.print(" -- ");
    Serial.print(whichWay[i]);
  }
  Serial.print("\n");
}

//trying to figure out how to make this more compact...
int move_motors(){
    if(pid1.Compute()){
    //Write enable pins
      if (setPoint[0] - feedback[0] > 0){
        digitalWrite(forwardsPin[0], LOW);
        digitalWrite(backwardsPin[0], HIGH);
      }
      else if (setPoint[0] - feedback[0] < 0){
        digitalWrite(forwardsPin[0], HIGH);
        digitalWrite(backwardsPin[0], LOW);
      } 
      analogWrite(torquePin[0], torque[0]);
    }
   
}

Have you read one or more tutorials about PID tuning?

One of your torque problems is the L293, which consumes too much voltage - get a modern MOSFET driver board instead or increase the servo driver voltage.

If you want smooth operation == slow motor rotation == low duty cycle == low power == low torque, you see that you have to increase the gear ratio first.

Thanks for the reply, DrDiettrich.

i've read a fair amount, and understand the basic concept. i've gotten it pretty close honestly, but the problem is that when I give it enough juice (P gain) to move with decent force, it jitters like crazy, and no amount of D gain will smooth it out. or at least I can't find the correct D value... maybe I'll hook up a pot so i can twiddle the numbers while it's running... I think i might have problems with latency then though if the loop has to do too many things at once.

as far as the driver goes, can you recommend a chip with a similar form factor / solderability / price point? I had the l293d chips on hand, and they seem to work pretty well. it's only a teeny motor and i'm powering with about 6.6v.

i'm not looking for SUPER smooth or powerful motion, just trying to get an improvement on the stock electronics in these micro steppers.

I've actually made some decent progress this afternoon. I switched to an arduino DUE for the 12 bit input, and improved my code using micros() for the speed control. I have a 16 bit pwm driver I can run over i2c as well for the output, but i'm not sure if it plays nice with the due without logic level shifting... anyways at this point it's quite a bit smoother, and has significantly more torque when running.

what i don't get is how it's rock solid when it's just holding position, but much weaker when it's in motion. I wouldn't think a DC servo would have more torque at zero rpm than at full speed! maybe it's a mechanical thing, like once the gears get moving they have some momentum, but it's harder to get them started. I don't have a ton of experience with gears. which is strange, because I literally make them for a living. :stuck_out_tongue_closed_eyes: :stuck_out_tongue_closed_eyes: :stuck_out_tongue_closed_eyes:

In the process control world, D is said to stand for disaster since it will turn any noise/short term instability in the system into jitter.

For servo speed control, P and maybe some I is all you need (and want). So, start with I and D to 0, increase P until it oscillates on a move and then back off. Add a small amount of I and increase until it overshoots when coming to a stop and then back off until there is no overshoot.

Too little I and it may undershoot, too much and it will overshoot.

hmm.. I must be mis-understanding the purpose of derivative gain. I thought that it was meant to decrease the output as the system approaches the setpoint (without it the output is simply a function of the distance from from setpoint and the proportional gain value, which means to avoid overshoot, you need a very low value for P, which means you lose torque), then the integral adds a little push if mechanical friction has stopped it slightly off the mark. with just P i can get it to move to location, oscillate briefly, then come to a stop. adding a small amount of D lessens the oscillation, then i increase it slowly until it stops oscillating. the I is the one that i end up leaving out, because it seems to make it wobble when you put a load on the motor. i've read that there's usually a setting for something like "integral level" or something like that, which defines the level of torque to apply, and the standard I gain is the time it takes to kick in..

that's the way i've understood it, but i've really only just started reading about PID this week LOL

anyways, I think i've got the performance to the point where i can continue with my project, and i've definitely improved the performance of these little steppers. the trick I think is the clock speed of the DUE and the 12 bit input. i'd like to do 12 bit output as well, but when I tried, everything went crazy. for now i'm moving on. :grin:

I’m sorry, I mistook you for someone that was looking for assistance. I see now you’re clearly and fully enlightened about PID, sorry to have wasted your time.

I wonder what PID can really do in this case, as you want to move to a certain point.

The D you definitely don't need here, that's for systems that are slow to react to a change of input. Your motor reacts instantly on a change of power supplied.

The I you may or may not need, depending on your overall system.

But what confuses me most is that you first talk about servos, where you remove the electronics, and then in #4 you say they're steppers. That's a totally different kind of motor, with totally different way of control. Indeed for your requirement of precision and high torque they may be the way to go.

Hi,
What servos have you hacked?
Can you post a link/data to some specs please?
What are you using to power the servos?

Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png?

Thanks.. Tom... :slight_smile:

gah, i've been busy all week and didn't check for responses.. when I said steppers i mis-typed. i'm using little 9g metal gear servos. "tower pro" is the brand, i believe.

WattsThat, I'm pretty sure i was clear that i'm new to this, and was just explaining what i've understood from my brief research, in hopes that someone will point out where/if i'm wrong. also there's definitely some distance between completely helpless and fully enlightened. i hope we can have a grownup conversation here.

wvmarle, I could be totally wrong, but i thought commercial servo drivers operate on (perhaps a more complicated and proprietary version of) PID algorithms. if not, what would be a better way to control them?

TomGeorge, here's a link to the servos i'm using:

LINK

I'm powering them with this 5v power supply (it's just what I had on hand) with the voltage adjusted up to max (5.8v) to help with the voltage drop over the l293d:

LINK

If i get a few minutes i'll draw up the circuit, but it's really simple. in fact i found this image that is exactly the same as what i did, but they're using a raspberry pi. also you have to pretend it's a servo, and that the potentiometer is connected to an analog input. here it is:

LINK

thanks again for any help guys.
cheers!

I took a video of the setup as it is now. it's HERE. i'm pretty happy with the torque and stability at this point, but i'm sure it's not tuned optimally. there's quite a bit of noise in the potentiometer, so some kind of filter might be in order. oh, and don't judge me for the RadioShack breadboard LOL, it's retro.

ergosum:
wvmarle, I could be totally wrong, but i thought commercial servo drivers operate on (perhaps a more complicated and proprietary version of) PID algorithms. if not, what would be a better way to control them?

PID is a very simple algorithm, really.
I don't think servos use PID, especially not cheap ones like those TowerPro servos - just simple proportional control will do just fine.

ergosum:
I'm powering them with this 5v power supply (it's just what I had on hand) with the voltage adjusted up to max (5.8v) to help with the voltage drop over the l293d:

Not using an ancient H-bridge is a better solution... E.g. the TB6612FNG is about as cheap as the L293D, without the voltage drop and related losses.

Hi,
Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png?
A fritzy picture is not a schematic. and yours is nowhere near what you describe.
Ops Fritzy..

Please a properly labeled schematic.

Thanks.. Tom.. :slight_smile:

TomGeorge:
Please a properly labeled schematic.

please forgive me if this is incorrect or doesn't give all the info you need, this is actually the first time i've made anything resembling an electrical schematic... hopefully it gives you the idea..

EDIT: ugh, they really went to town with the watermark on the version I attached, here's a link instead.

LINK

servo+hack.png

PID pid1(&error[0], &torque[0], 0, Kp1, Ki1 ,Kd1, REVERSE);
PID pid2(&error[1], &torque[1], 0, Kp2, Ki2,Kd2, REVERSE);
PID pid3(&error[2], &torque[2], 0, Kp3, Ki3,Kd3, REVERSE);

Really? It lets you pass ZERO as the address of the Setpoint variable?!?!

When I try to compile your code I get:

sketch_jul13a:49:59: error: no matching function for call to 'PID::PID(float*, float*, int, float&, float&, float&, int)'
 PID pid1(&error[0], &torque[0], 0, Kp1, Ki1 , Kd1, REVERSE);
                                                           ^

A PID algorithm as noted bove has virtually no relevance to an integrated proportional RC servo.
If you were using a driver & servo motor - without the control wrapper - that would make more sense.

johnwasser:
Really? It lets you pass ZERO as the address of the Setpoint variable?!?!

I apologize if my solution is not standard, i'm relatively new to coding, and have no formal training. the error you're getting is because I altered the library to use floats instead of the original doubles. I did this because I'm using an arduino DUE, and doubles are 64 bit which is unnecessary. the setpoint is zero because i'm calculating the error and sending it to the input, then the PID just has to drive it to zero. I'll be honest, I did it this way because I saw someone else's code, and that's what they did. now you've got me thinking though if it's maybe not the most efficient method. i realize now it's not the intended functionality.

lastchancename:
A PID algorithm as noted bove has virtually no relevance to an integrated proportional RC servo.
If you were using a driver & servo motor - without the control wrapper - that would make more sense.

I think maybe i didn't explain that part well enough.. I took a standard RC servo and de-soldered the original pcb from inside, then soldered new leads directly to the motor and potentiometer. my plan is to have better resolution, smoother motion, acceleration and deceleration, etc.

i've made quite a few changes to the code, so i'll re post it now. it's still just basic movement of the motor. next i'm going to add in kinematic functions for a delta style robot, and hack up a few more motors. I feel like i'm going to have to think more about timing with 3 pid calculations, as well as all the kinematics, and some sort of outside control mechanism.

#include <pid_v1_float.h>

uint32_t oldmillis;
uint32_t pausemillis;
uint32_t laststep;
float speedFactor = 100; //bigger the number the slower it goes. not a perfect solution, just testing. 

//feedback pins coming from motors (pots)
const int feedbackPin[3] = {A0, A1, A2};

//PWM pins to motor drivers
const int torquePin[3] = {10,11,12}; 

//direction pins to motor drivers
const int forwardsPin[3] = {3,9,7}; 
const int backwardsPin[3] = {5,6,8}; 

//for serial input
const byte numChars = 5;
char receivedChars[numChars];
float integerFromPC = 500;
boolean newData = false;



//PID tunings
float Kp1 =10;
float Ki1 = .00;
float Kd1 = .025;
float Kp2 = 9;
float Ki2 = .1;
float Kd2 = .1;
float Kp3 = 9;
float Ki3 = .1;
float Kd3 = .1;

// these are all 3 dimensional arrays because i'm going to have 3 motors. only one is connected at this point. 

float setPoint[3] = {0,0,0};
float feedback[3] = {0,0,0};
float error[3] = {0,0,0};
float output[3] = {0,0,0};
float torque[3] = {0,0,0};
float currentPoint[3] = {0,0,0};
float nextPoint[3] = {0,0,0};
boolean whichWay[3] = {true,true,true};
boolean paused = false;
int i;

PID pid1(&error[0], &torque[0], 0, Kp1, Ki1 ,Kd1, REVERSE);
PID pid2(&error[1], &torque[1], 0, Kp2, Ki2,Kd2, REVERSE);
PID pid3(&error[2], &torque[2], 0, Kp3, Ki3,Kd3, REVERSE);

void setup() {
  
  // i've been using the serial port for debugging, but it makes things run slow
  // which affects the pid tuning, so i turn off the output function when testing. 
  Serial.begin(19200);
  Serial.println("Ready");

  //set resolution to 12 bit
  analogReadResolution(12);
  
  get_feedback();

  for (i=0; i<=2; i++){
    setPoint[i]= feedback[i];
    currentPoint[i]=feedback[i];
  }

  calc_error();

  //set output pins
for (i=0; i<=2; i++){
  pinMode(forwardsPin[i], OUTPUT);
  pinMode(backwardsPin[i], OUTPUT);
  pinMode(torquePin[i], OUTPUT);
}

  pid1.SetSampleTime(1);
  // pid1.SetOutputLimits(0,4096);
    pid1.SetMode(AUTOMATIC);
  pid2.SetSampleTime(2);
  // pid2.SetOutputLimits(0,4096);
    pid2.SetMode(AUTOMATIC);  
  pid3.SetSampleTime(2);
  // pid3.SetOutputLimits(0,4096);
    pid3.SetMode(AUTOMATIC);  

  
}

void loop() {
  
  oldmillis = millis(); //reset timer
  
  get_feedback();  

  set_setPoint();
  //check_input();  //right now we're using the serial port to change the setpoint, but this will be changed later
  
  calc_error();
  
  move_motors(); //this includes the PID calculations for each axis.. consider changing this to two separate functions to improve motor synchronization
  
  //display_data(); //comment out this line to improve performance once the motors are tuned

}

void get_feedback(){
  for (i=0; i<=2; i++){
    feedback[i] = analogRead(feedbackPin[i]);
  }
}
void check_input() {
 recvWithStartEndMarkers();
   parseData() ;
   storeNewData() ;
 }


void calc_error(){
  for(i=0; i<=2; i++){
    error[i] = abs(setPoint[i] - feedback[i]);  
  }
}

void set_setPoint() {
  
  for (i=0; i<=2; i++){
    
    if (paused==false){
      
      if (micros()-laststep>=speedFactor){
        
        laststep=micros();
        
        if (currentPoint[i]>=4000) {
          whichWay[i]=false;
          paused=true;
          pausemillis=millis();
          pause();
        }
        if (currentPoint[i]<=100) {
          whichWay[i]=true;
          paused=true;                
          pausemillis=millis();
          pause();
        }
        if (whichWay[i]==true) {
          //if ((abs(feedback[i]-setPoint[i]))<50){ //I had these if statements in there to give the servo some time to catch up, 
          //but I removed them at some point, and it runs good now so i don't want to add them back in LOL
          setPoint[i]=(currentPoint[i]+1);
          currentPoint[i]=currentPoint[i]+1;
          //}
        }
        else {
          //if ((abs(feedback[i]-setPoint[i]))<50){
          setPoint[i]=(currentPoint[i]=currentPoint[i]-1);  
          //}
        }
      }
    } 
    else {
      pause(); 
    }
  }
}

void pause(){
  if (millis()-pausemillis>=1000){
    paused=false;
    pausemillis=0;
  }
}
void recvWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    char startMarker = '<';
    char endMarker = '>';
    char rc;
 
 // if (Serial.available() > 0) {
    while (Serial.available() > 0 && newData == false) {
        rc = Serial.read();

        if (recvInProgress == true) {
            if (rc != endMarker) {
                receivedChars[ndx] = rc;
                ndx++;
                if (ndx >= numChars) {
                    ndx = numChars - 1;
                }
            }
            else {
                receivedChars[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }

        else if (rc == startMarker) {
            recvInProgress = true;
        }
    }
}

void storeNewData() {
    if (newData == true) {
      set_setPoint();
      newData = false;
    }
}
void parseData() {
     
    integerFromPC = atoi(receivedChars);     // convert this part to an integer
    
}

void display_data(){
  Serial.print(millis()-oldmillis);
  Serial.print(" -- ");
  Serial.print(paused);
  Serial.print(" -- ");
  for (i=0;i<=0; i++){
    Serial.print(feedback[i]);
    Serial.print(" -- ");
    Serial.print(setPoint[i]);
    Serial.print(" -- ");
    Serial.print(whichWay[i]);
  }
  Serial.print("\n");
}

//i'm only running one motor right now, need to copy this with correct array values to add more motors. 
int move_motors(){
    if(pid1.Compute()){
    //Write enable pins
      if (setPoint[0] - feedback[0] > 0){
        digitalWrite(forwardsPin[0], LOW);
        digitalWrite(backwardsPin[0], HIGH);
      }
      else if (setPoint[0] - feedback[0] < 0){
        digitalWrite(forwardsPin[0], HIGH);
        digitalWrite(backwardsPin[0], LOW);
      } 
      analogWrite(torquePin[0], torque[0]);
    }
   
}

Hi,
Thanks for the circuit;

Tom.. :slight_smile: