Smoothing Out a PID controlled motor

Other Hardware Motors, Mechanics, Power and CNC
1

I'm having a load of issues with what I though would be quite a simple little component of a larger project.

Pretty simple task - make a position controlled drum which can rotate infinitely in either direction to an angular position which is sent to the micro-controller. (Most likely with I2C eventually but im just going to use serial for now).

Im using an Uno, a standard little 6v DC hobby motor, an L298N, to measure the position im using an AMS-AS5048b magnetic position sensor.

The standard way to do this seems to be a stepper and a optical encoder but due to the small package size I'm going to need to fit this in to eventually I went for DC motors and the little hall effect sensor instead, and I'll replace the L298N with a TB6612FNG.

Here's some photos of the test rig I made. The orange drum has the magnet recessed in the end.

960×1280 164 KB

960×1280 203 KB

My issue is the PID is being extremely temperamental to tune. Most of the time it rocks consistently back and forth passed the set point at maximum speed. Here's a little video and the output graph.

912×642 23.3 KB

I've managed to tune it to the point where it steadies out but as you can see in the graphs it's still very unstable during the deceleration. I'm also getting about 5 degrees of steady-state error from this when I do a 360 to -360 sweep.

912×635 20.5 KB

So I'm a little baffled really. I'm not sure if it's an error with the PID, with the hardware, or with the Code.

Any help appreciated.

One other strange thing I noticed is uploading the exact same code using the default arduino IDE and using VS code platformio results in different level of stability, with the VScode version being much more unstable. Any ideas?

#include <Arduino.h>
#include <PID_v1.h>
#include <ams_as5048b.h> //https://github.com/sosandroid/AMS_AS5048B

//Constants required for position sensor
#define U_RAW 1
#define U_TRN 2
#define U_DEG 3
#define U_RAD 4
#define U_GRAD 5
#define U_MOA 6
#define U_SOA 7
#define U_MILNATO 8
#define U_MILSE 9
#define U_MILRU 10

//Postion Sensor Device
AMS_AS5048B sensor1(0x40);

//Motor Pins and Constants
#define MO1 9       //Motor Enable(Speed) Pin
#define CW1 10      //Clockwise Pin
#define ACW1 11     //Anti-clockwise Pin
#define MAX_PWM 255 //Motor Max PWM value
#define MIN_PWM 140 //Motor Min PWM value

//Control Variables
double Input = 0, Setpoint = 0, Output = 0; //For PID controller
int motor_speed, rots = 0;                  //For Calculating True Position
double sens_ang, prev_ang = 0;

#define window 30
#define MAX_OUTPUT 100
#define MIN_ERROR 2
bool new_data = false; //New data receive flag

//Controller Constants
double Kp = 5, Ki = 1.4, Kd = 0.0125;

//Set up PID
PID pid1(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

//Print for Monitor or Plotter
bool graph = false;

/////////SUBROUTINES//////////

void get_true_position()
{
  sens_ang = sensor1.angleR(U_DEG, true); //Get Position from 0-360 from Sensor

  if (prev_ang > (360 - window) && prev_ang <= 360)
  {
    if (sens_ang >= 0 && sens_ang < window)
    {
      rots++; //If has moved clockwise add 1 to rotation
    }
  }

  if (prev_ang >= 0 && prev_ang < window)
  {
    if (sens_ang <= 360 && sens_ang > (360 - window))
    {
      rots--; //If moved acw take one away.
    }
  }

  prev_ang = sens_ang;
  Input = sens_ang + (360 * rots); //Calculate true position
}

void clockwise()
{
  digitalWrite(CW1, HIGH);
  digitalWrite(ACW1, LOW);
}

void anticlockwise()
{
  digitalWrite(CW1, LOW);
  digitalWrite(ACW1, HIGH);
}

void motor_off()
{
  digitalWrite(CW1, LOW);
  digitalWrite(ACW1, LOW);
}

///////////////////////////////

void setup()
{
  //Serial For Debug
  Serial.begin(9600);
  Serial.setTimeout(500); //Lower Timeout to speed up loop when new data received.

  //Motor Pins
  pinMode(MO1, OUTPUT);
  pinMode(CW1, OUTPUT);
  pinMode(ACW1, OUTPUT);

  //Start sensor and set current position as zero.
  sensor1.begin();
  sensor1.setZeroReg();
  sensor1.setClockWise(true);

  //Set up PID
  pid1.SetMode(AUTOMATIC);
  pid1.SetSampleTime(1);
  pid1.SetOutputLimits(-MAX_OUTPUT, MAX_OUTPUT);

//Change base PWM fq. to reduce motor noise
  TCCR1B = TCCR1B & 0b11111000 | 1;
}

void loop()
{
  //Get new Setpoint from Serial if new data is available.
  new_data = Serial.available();
  if (new_data == true)
  {
    Setpoint = Serial.parseInt();
  }

  //reset flag
  new_data = false;

  //Get True Position
  get_true_position();

  //Calculate PID
  pid1.Compute();

  //Output is value from -100 to 100 which must be converted to PWM to send motor between -255 and 255.
  //Positive is Clockwise. However, motor will not turn if output is below MIN_PWM.
  //Additionally, Only let motor turn if output is above a threshold to prevent shuddering
  if (Output > MIN_ERROR)
  {
    clockwise();
    motor_speed = map(abs(Output), 0, MAX_OUTPUT, MIN_PWM, MAX_PWM);
  }
  else if (Output < -MIN_ERROR)
  {
    anticlockwise();
    motor_speed = map(abs(Output), 0, MAX_OUTPUT, MIN_PWM, MAX_PWM);
  }
  else
  {
    motor_speed = 0;
    motor_off();
  }

  motor_speed = round(motor_speed);
  analogWrite(MO1, motor_speed);

  //Print Info to Serial
  if (graph == true)
  {
    Serial.print("Target_Angle: ");
    Serial.print("Measured_Angle: ");
    Serial.println(Setpoint);
    Serial.print("\t");
    Serial.println(Input);
    Serial.print("\t");
  }
  else
  {
    Serial.print("Time:");
    Serial.print("\t");
    Serial.print(millis());
    Serial.print("\t");
    Serial.print("Target Angle:");
    Serial.print("\t");
    Serial.print(Setpoint);
    Serial.print("\t");
    Serial.print("Measured Angle:");
    Serial.print("\t");
    Serial.print(Input);
    Serial.print("\t");
    Serial.print("PID Output:");
    Serial.print("\t");
    Serial.print(Output);
    Serial.print("\t");
    Serial.print("Motor Speed:");
    Serial.print("\t");
    Serial.println(motor_speed);
  }
}
2

Nice test rig!

PID tuning can be very difficult, but there are several systematic approaches that allow informed choices to be made. Your favorite search engine will find a number of excellent, in depth tutorials.

In your particular case, geartrain backlash will be an additional complication, possibly quite serious. Have you measured the backlash?

3

I'm not convinced that you need PID. You might want to write a controller that understands your system rather than PID, which doesn't.

I'd start such a thing just using proportional control, which happpily, you can immediately do by dropping the I and D components.

4

You’ll find your PID update rate is nowhere near 1 ms with all the serial print statements at 9600 baud. Increase serial baud rate to 115,200 or even better, comment out/reduce character count/remove the serial output stuff until you can get the loop stable.

You’ll only need P and I, leaving D at zero, it will only make things worse.

5

Your basic problem is that you have backlash in the gear train between the motor and the encoder.

PID loops don't like backlash as this will cause overshoot and oscillation. You'll need to turn down the
gain, try using PI or P only.

6

Hi Guys,
Thanks for all your ideas. I upped the baud rate as suggested and did some testing with a combination of P, PI, PD and PID controllers.

From the results it looks like a PD, or just straight P controller would be the best option as any integral component causes some pretty nasty steady-state error.

1932×1040 178 KB

I'll have a look into measuring the gearbox backlash and see if there's anything I can do to fix that.

Thanks again for all the help, It's looking much better now.

Code used for this testing:

#include <Arduino.h>
#include <PID_v1.h>
#include <ams_as5048b.h>

//Constants required for position sensor
#define U_RAW 1
#define U_TRN 2
#define U_DEG 3
#define U_RAD 4
#define U_GRAD 5
#define U_MOA 6
#define U_SOA 7
#define U_MILNATO 8
#define U_MILSE 9
#define U_MILRU 10

//Postion Sensor Device
AMS_AS5048B sensor1(0x40);

//Motor Pins and Constants
#define MO1 9       //Motor Enable(Speed) Pin
#define CW1 10      //Clockwise Pin
#define ACW1 11     //Anti-clockwise Pin
#define MAX_PWM 255 //Motor Max PWM value
#define MIN_PWM 140 //Motor Min PWM value

//Control Variables
double Input = 0, Setpoint = 0, Output = 0;        //For PID controller
int motor_speed, rots = 0; //For Calculating True Position
double  sens_ang, prev_ang = 0;
int t;



#define window 30
#define MAX_OUTPUT 100
#define MIN_ERROR 0

//Controller Constants
double Kp = 5, Ki = 0.0, Kd = 0.1;

//Set up PID
PID pid1(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

/////////SUBROUTINES//////////

void get_true_position()
{
  sens_ang = sensor1.angleR(U_DEG, true); //Get Position from 0-360 from Sensor

  if (prev_ang > (360 - window) && prev_ang <= 360)
  {
    if (sens_ang >= 0 && sens_ang < window)
    {
      rots++; //If has moved clockwise add 1 to rotation
    }
  }

  if (prev_ang >= 0 && prev_ang < window)
  {
    if (sens_ang <= 360 && sens_ang > (360 - window))
    {
      rots--; //If moved acw take one away.
    }
  }

  prev_ang = sens_ang;
  Input = sens_ang + (360 * rots); //Calculate true position
}

void clockwise()
{
  digitalWrite(CW1, HIGH);
  digitalWrite(ACW1, LOW);
}

void anticlockwise()
{
  digitalWrite(CW1, LOW);
  digitalWrite(ACW1, HIGH);
}

void motor_off()
{
  digitalWrite(CW1, LOW);
  digitalWrite(ACW1, LOW);
}

///////////////////////////////

void setup()
{
  
  //Serial For Debug
  Serial.begin(115200);
  Serial.setTimeout(100); //Lower Timeout to speed up loop when new data received.

  //Motor Pins
  pinMode(MO1, OUTPUT);
  pinMode(CW1, OUTPUT);
  pinMode(ACW1, OUTPUT);
  TCCR1B = TCCR1B & 0b11111000 | 1;
  
  //Start sensor and set current position as zero.
  sensor1.begin();
  sensor1.setZeroReg();
  sensor1.setClockWise(true);

  //Set up PID
  pid1.SetMode(AUTOMATIC);
  pid1.SetSampleTime(1);
  pid1.SetOutputLimits(-MAX_OUTPUT, MAX_OUTPUT);

}

void loop()
{
  //Input for this test will be a 0 to 360 to -360 to 0 sweep allowing 2.5 seconds between each change.
  t = millis();

  if (t >= 2500 && t < 5000){
    Setpoint = 360; 
  }
  else if (t >=5000 && t < 10000){
    Setpoint = -360;
  }
  else{
    Setpoint = 0;
  }

  //Calculte True Position
  get_true_position();

  //Calculate PID
  pid1.Compute();

  //Output is PWM value to send motor between -255 and 255. Positive is Clockwise.
  //However, motor will not turn if output is below MIN_PWM.
  //Additionally, Only let motor turn if output is above a threshold to prevent shuddering
  if (Output > MIN_ERROR)
  {
    clockwise();
    motor_speed = map(abs(Output), 0, MAX_OUTPUT, MIN_PWM, MAX_PWM);
  }
  else if (Output < -MIN_ERROR)
  {
    anticlockwise();
    motor_speed = map(abs(Output), 0, MAX_OUTPUT, MIN_PWM, MAX_PWM);
  }
  else
  {
    motor_speed = 0;
    motor_off();
  }

  motor_speed = round(motor_speed);
  analogWrite(MO1, motor_speed);

  //Print Info to Serial
  //Serial.print("Time:"); Serial.print("\t"); 
  Serial.print(t); Serial.print("\t");
  //Serial.print("Target Angle:"); Serial.print("\t"); 
  Serial.print(Setpoint); Serial.print("\t");
  //Serial.print("Measured Angle:"); Serial.print("\t"); 
  Serial.print(Input); Serial.println("\t");

}
7

One approach is to permanently apply a load in one direction to the gear box, so its always pressing the
same direction on the teeth - this will increase power consumption and wear, and requires another motor
to apply the constant torque.

Another is to use zero-backlash gears, which are spring loaded.

Having encoder on the motor shaft fixes the control loop behaviour, leaving just the backlash to
be compensated - sometimes this can be done by feedforward control if the amount of backlash
is constant and the load is dominated by static friction after the gears. Basically you always move a little
bit further after any change in direction to make up for the slack.