PID position control problem

I am trying to control position using L298 motor driver and quadrature encoder but the motor just won’t stop at my setpoint np matter what PID parameters I use.
Any ideas?

#include <PID_v1.h>
double Setpoint = 0 ; // will be the desired value
double Input = 0; 
double Output = 0 ; 
//PID parameters
double Kp = 3.8, Ki = 1.7, Kd = 0.15;

//create PID instance
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);


int in_A = 2;
int in_B = 3;
// Initialize the counter

// The number of pulses per revolution
// depends on the index disc
unsigned int pulsesperturn = 100;

// The total number of revolutions
unsigned int revolutions = 0;

// Initialize the counter
double pulses = 0;
double angle = 0;

//motor para
int Enable = 10;
int Direc1 = 7;
int Direc2 = 8;


//int motor = 10;

int flag = 1; //indicate the direction of motor, forward(counter clockwise) = 1, backward(clockwise) = 0
int position_index = 0;


void count_pulse() {
 // This function is called by the interrupt
 // If in_b is HIGH increment the counter
 // otherwise decrement it
 if (digitalRead(in_B)) {
   pulses++;
 }
 else {
   pulses--;
 }
}

void setup() {
 pinMode(in_A, INPUT_PULLUP);
 pinMode(in_B, INPUT_PULLUP);
 attachInterrupt(0, count_pulse, RISING);
 Serial.begin(9600);

 pinMode(Enable, OUTPUT);
 pinMode(Direc1, OUTPUT);
 pinMode(Direc2, OUTPUT);

 
 myPID.SetTunings(Kp, Ki, Kd);
 TCCR1B = TCCR1B & 0b11111000 | 1;                   // set 31KHz PWM to prevent motor noise
 myPID.SetMode(AUTOMATIC);
 myPID.SetSampleTime(1);
 myPID.SetOutputLimits(-255, 255);


 
}

void loop() {
 revolutions = pulses / pulsesperturn;
 Setpoint = 180;
 /*if ( position_index == 0){
   Setpoint = 0;
 }*/
 

 
 angle = pulses * 3.6;
 //motor_speed = 150;
 /*
 if ( flag == 1 && angle < 30 ) {
   forward();
 }
 if ( flag == 1 && angle > 30 ) {
   flag = 0;
   hardstop();
 }
 if ( flag == 0 && angle > 0 ) {
   backward();
 }
 if ( flag == 0 && angle < 0) {
   flag = 1;
   hardstop();
 }
 */
 //Serial.print("angle = ");
 Serial.println(angle);
 //Serial.println(pulses);
 Serial.println(revolutions);
 
 //Input = angle;
 Input = pulses;
 myPID.Compute();
 pwmOut(Output);
 

}

void pwmOut(int out) {                                // to H-Bridge board
 if (out > 0) {
   forward( out );                             // drive motor CW
 }
 else {
   backward( out );                       // drive motor CCW
 }
}




void forward( int motor_speed ) {
 analogWrite(Enable, motor_speed);
 digitalWrite(Direc1, HIGH);
 digitalWrite(Direc2, LOW);
}

void backward( int motor_speed ) {
 analogWrite(Enable, motor_speed);
 digitalWrite(Direc1, LOW);
 digitalWrite(Direc2, HIGH);
}

void hardstop() {
 digitalWrite(Enable, HIGH);
 digitalWrite(Direc1, HIGH);
 digitalWrite(Direc2, HIGH);
}

PID_control.ino (2.61 KB)

Please edit your post and add code tags (as per the sticky on how to use this forum - which of course you read, just like no other first posters, right?)

Variable type double is the exact same as float in the AVR 8-bit world (i.e. most Arduinos).

Why is pulses a double, not an unsigned int or unsigned long?

And what are you even trying to accomplish? If I read your code correctly you want to move your motor by 180 pulses, then stop there. So a speed of 0 in the end. That's not a normal application for a PID.

wvmarle:
So a speed of 0 in the end. That’s not a normal application for a PID.

I don’t see any problem using PID for position control. If there is a position error apply power to correct it.

Without being able to view the code easily I can’t see what the OP is trying to do.

@tonysun1002, to make it easy for people to help you please modify your post and use the code button </> so your code looks like this and is easy to copy to a text editor. See How to use the Forum

…R

Hi,
Welcome to the forum.

Please read the first post in any forum entitled how to use this forum.
http://forum.arduino.cc/index.php/topic,148850.0.html then look down to item #7 about how to post your code.
It will be formatted in a scrolling window that makes it easier to read.

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:

wvmarle:
Please edit your post and add code tags (as per the sticky on how to use this forum - which of course you read, just like no other first posters, right?)

Variable type double is the exact same as float in the AVR 8-bit world (i.e. most Arduinos).

Why is pulses a double, not an unsigned int or unsigned long?

And what are you even trying to accomplish? If I read your code correctly you want to move your motor by 180 pulses, then stop there. So a speed of 0 in the end. That's not a normal application for a PID.

Thanks for the heads up.
I am trying to make the motor stop at exactly where I want, and I am building a robot arm to control the end effector in the end. For now, I am stuck at this position control. Am I on the right track?

Robin2:
I don’t see any problem using PID for position control. If there is a position error apply power to correct it.

Without being able to view the code easily I can’t see what the OP is trying to do.

@tonysun1002, to make it easy for people to help you please modify your post and use the code button </>

so your code looks like this

and is easy to copy to a text editor. See How to use the Forum

…R

I am trying to make the motor stop at exactly where I want, and I am building a robot arm to control the end effector in the end. For now, I am stuck at this position control. Am I on the right track?
Thanks!

Here’s a code for more movement like stop at 90, 180, 270 and 360 at a two second interval.

#include <PID_v1.h>
double Setpoint = 0 ; // will be the desired value
double Input = 0; 
double Output = 0 ; 
//PID parameters
double Kp = 5, Ki = 1, Kd = 0.1;// 5 1 0.1

//create PID instance
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
unsigned long time;

int in_A = 2;
int in_B = 3;
// Initialize the counter

// The number of pulses per revolution
// depends on the index disc
unsigned int pulsesperturn = 100;

// The total number of revolutions
unsigned int revolutions = 0;

// Initialize the counter
double pulses = 0;
double angle = 0;

//motor para
int Enable = 10;
int Direc1 = 7;
int Direc2 = 8;
int motor_speed = 255;

//int motor = 10;

int flag = 1; //indicate the direction of motor, forward(counter clockwise) = 1, backward(clockwise) = 0
int position_index = 0;


void count_pulse() {
  // This function is called by the interrupt
  // If in_b is HIGH increment the counter
  // otherwise decrement it
  if (digitalRead(in_B)) {
    pulses++;
  }
  else {
    pulses--;
  }
}

void setup() {
  pinMode(in_A, INPUT_PULLUP);
  pinMode(in_B, INPUT_PULLUP);
  attachInterrupt(0, count_pulse, RISING);
  Serial.begin(9600);

  pinMode(Enable, OUTPUT);
  pinMode(Direc1, OUTPUT);
  pinMode(Direc2, OUTPUT);

  
  //myPID.SetTunings(Kp, Ki, Kd);
  TCCR1B = TCCR1B & 0b11111000 | 1;                   // set 31KHz PWM to prevent motor noise
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);


  
}

void loop() {
  //revolutions = pulses / pulsesperturn;
  //Setpoint = 90;
  time = millis();
  Serial.println(time);
  if (time < 2000){
    Setpoint = 90;
  }
  if (time > 2000 && time < 4000){
    Setpoint = 180;
  }
  if (time > 4000 && time < 6000){
    Setpoint = 270;
  }
  if ( time > 6000){
    Setpoint = 360;
  }
  

  
  angle = pulses * 3.6;
  //motor_speed = 150;
  /*
  if ( flag == 1 && angle < 30 ) {
    forward();
  }
  if ( flag == 1 && angle > 30 ) {
    flag = 0;
    hardstop();
  }
  if ( flag == 0 && angle > 0 ) {
    backward();
  }
  if ( flag == 0 && angle < 0) {
    flag = 1;
    hardstop();
  }
  */
  Serial.print("angle = ");
  Serial.println(angle);
  //Serial.println(pulses);
  //Serial.println(revolutions);
  
  Input = angle;
  myPID.Compute();
  pwmOut(Output);
  
 
}

void pwmOut(int out) {                                // to H-Bridge board
  if (out > 0) {
    forward( out );                             // drive motor CW
  }
  else {
    backward( out );                       // drive motor CCW
  }
}




void forward( int motor_speed ) {
  analogWrite(Enable, motor_speed);
  digitalWrite(Direc1, HIGH);
  digitalWrite(Direc2, LOW);
}

void backward( int motor_speed ) {
  analogWrite(Enable, motor_speed);
  digitalWrite(Direc1, LOW);
  digitalWrite(Direc2, HIGH);
}

void hardstop() {
  digitalWrite(Enable, HIGH);
  digitalWrite(Direc1, HIGH);
  digitalWrite(Direc2, HIGH);
}

Have you verified that your encoder interrupt routine is actually working?

Incrementing a float variable in an interrupt routine is a bad idea. Use an int or long int. In addition, global variables accessed by interrupts MUST be declared volatile.

double pulses = 0;

...

  if (digitalRead(in_B)) {
    pulses++;

Hi,

  • How have you got your motor and arm and encoder physically fitted?
  • What is the motor spec/data and what gearbox do you have fitted?
  • What is the model spec/data of the encoder?
  • Have you just written code to drive JUST the motor to make sure when you want the motor to STOP, it does STOP and not overshoot?
  • Have you written code to read JUST the encoder to make sure it is calibrated and you can read it at the speed needed?
  • Can you post a picture of your proiject so we can see your component and arm layout?
  • 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:

jremington:
Have you verified that your encoder interrupt routine is actually working?

Incrementing a float variable in an interrupt routine is a bad idea. Use an int or long int. In addition, global variables accessed by interrupts MUST be declared volatile.

double pulses = 0;

...

if (digitalRead(in_B)) {
    pulses++;

My encoder definitely works. I can get angle from serial monitor when testing.

Remove the many Serial.write() from loop(), they will slow down execution and prevent proper PID operation.

TomGeorge:
Hi,

  • How have you got your motor and arm and encoder physically fitted?
  • What is the motor spec/data and what gearbox do you have fitted?
  • What is the model spec/data of the encoder?
  • Have you just written code to drive JUST the motor to make sure when you want the motor to STOP, it does STOP and not overshoot?
  • Have you written code to read JUST the encoder to make sure it is calibrated and you can read it at the speed needed?
  • Can you post a picture of your proiject so we can see your component and arm layout?
  • 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:

Image of layout is in attached.
Please check

Hi,
OPs image;
4ef9f667b6de7209d821e0ee7d32c9533f97b6c1.png
Is that motor direct driving the spindle with the encoder wheel on it or is there a gearbox.
What are the specs/data on the motor, to do what you need to do you may need more control than just a L298.

If you are trying to get 30deg or 180deg turns you are pushing up hill, how fast do you want the system to respond to your setpoint.

It looks like you are trying to do a servo type control.

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

Tom… :slight_smile:
PS. If you are going to mount the motor horizontal and going to try and hold a spindle position against the force of gravity, can you motor and driver (L298?) withstand the constant stall current that will need to run through the motor to keep position?

tonysun1002:
I am trying to make the motor stop at exactly where I want, and I am building a robot arm to control the end effector in the end. For now, I am stuck at this position control. Am I on the right track?
Thanks!

Here's a code for more movement like stop at 90, 180, 270 and 360 at a two second interval.

You have posted your code but you have not told us what it actually does when you try it.

Making a DC motor stop at a precise location is not trivial. Your encoder system can only identify the fact that the motor has moved off the location which will happen when the forces are not in balance. That probably means that it will move even further off the desired location before a correction can take effect.

If there is a large gear reduction between the motor and the arm and if the encoder is attached to the motor an error of a few encoder positions will be of much less significance.

If the mechanical system can hold position without motor power (for example with a non-reversible worm drive) that would also make position keeping easier. But I can see that a non-reversible mechanism would not be suitable in some situations.

...R