PID Controller Starts Stutering with 2 Motors

newbieoftheday · July 1, 2022, 1:36am

I have a PID controller code that controls position. I have two motors to use that code. To use them at the same time, I wrote another code that does the same thing as Left Motor and Right Motor (L and R). However, in the second code(that is made for two motors) motors starts to stutter. Everything was fine in the first code and they were nailing the target position with the same PID constants. I don't know what is wrong. Everything should work. I am in hurry, I appreciate faster answers. Thank you...

Note: I used the word stuttering to mean "keeps changing directions very fast", CW,CCW,CW,CCW in miliseconds.

Code 1:

// This alternate version of the code does not require
// atomic.h. Instead, interrupts() and noInterrupts() 
// are used. Please use this code if your 
// platform does not support ATOMIC_BLOCK.

#define ENCA 2 // YELLOW
#define ENCB 4 // WHITE
#define PWM 10
#define IN2 8
#define IN1 9

volatile int posi = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
float eprev = 0;
float eintegral = 0;

void setup() {
  Serial.begin(9600);
  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
  
  pinMode(PWM,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  
  Serial.println("target pos");
}

void loop() {

  // set target position
  //int target = 1200;
  int target = 5000;

  // PID constants
  float kp = 30;
  float kd = 0.48;
  float ki = 0.05;

  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;

  // Read the position
  int pos = 0; 
  noInterrupts(); // disable interrupts temporarily while reading
  pos = posi;
  interrupts(); // turn interrupts back on
  
  // error
  int e = pos - target;

  // derivative
  float dedt = (e-eprev)/(deltaT);

  // integral
  eintegral = eintegral + e*deltaT;

  // control signal
  float u = kp*e + kd*dedt + ki*eintegral;

  // motor power
  float pwr = fabs(u);
  if( pwr > 255 ){
    pwr = 255;
  }

  // motor direction
  int dir = 1;
  if(u<0){
    dir = -1;
  }

  // signal the motor
  setMotor(dir,pwr,PWM,IN1,IN2);


  // store previous error
  eprev = e;

  Serial.print(target);
  Serial.print(" ");
  Serial.print(pos);
  Serial.println();
}

void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}

void readEncoder(){
  int b = digitalRead(ENCB);
  if(b > 0){
    posi++;
  }
  else{
    posi--;
  }
}

Code 2:

#include <SoftwareSerial.h>

#define L_ENCA 2 // YELLOW
#define L_ENCB 4// GREEN
#define L_PWM 10
#define L_IN2 8
#define L_IN1 9

#define R_ENCA 3 // YELLOW
#define R_ENCB 5// GREEN
#define R_PWM 13
#define R_IN2 11
#define R_IN1 12

#define Enable_Wifi 6
#define Transmit_On_Off 7

const int pinRX = 0;
const int pinTX = 1;

volatile int posiL = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
volatile int posiR = 0;
long prevT = 0;
float eprevL = 0;
float eprevR = 0;
float eintegralL = 0;
float eintegralR = 0;
char state;
char idle = '0';
char left = '1';
char right = '2';
char forward = '3';
char backward = '4';

SoftwareSerial apc220(pinRX, pinTX); // Crt softserial port and bind tx / rx to appropriate PINS
void setup() {
  Serial.begin(9600);
  pinMode(L_ENCA,INPUT);
  pinMode(L_ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(L_ENCA),readEncoderL,RISING);
  
  pinMode(L_PWM,OUTPUT);
  pinMode(L_IN1,OUTPUT);
  pinMode(L_IN2,OUTPUT);
  
  Serial.println("targetL targetR posL posR");

  pinMode(R_ENCA,INPUT);
  pinMode(R_ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(R_ENCA),readEncoderR,RISING);
  
  pinMode(R_PWM,OUTPUT);
  pinMode(R_IN1,OUTPUT);
  pinMode(R_IN2,OUTPUT);

  pinMode (Enable_Wifi, OUTPUT);
  digitalWrite (Enable_Wifi,HIGH);
  pinMode (Transmit_On_Off, OUTPUT);
  digitalWrite (Transmit_On_Off,LOW);
  apc220.begin(9600);  
}

void loop() {
  if (apc220.available())
  {
    state  = char(apc220.read());
  }
  long int target_L = -5000; //Target_L needs to be negative for to be forward
  long int target_R = 1000; //
  if(state == idle){ //0
    long int target_L = 0;
    long int target_R = 0;
    Serial.println("default");
  }
  else if(state == forward){ //3
    long int target_L = -5000;
    long int target_R = 5000;
    Serial.println("forward");
  }
  else if(state == backward){ //4
    long int target_L = 5000;
    long int target_R = -5000;
    Serial.println("backward");
  }
  else if(state == left){ //1
    long int target_L = -1000;
    long int target_R = 5000;
    Serial.println("left");
  }
  else if(state == right){ //2
    long int target_L = -5000;
    long int target_R = 1000;
    Serial.println("right");   
  }
  // PID constants
  float kp_R = 30;
  float kd_R = 0.48;
  float ki_R = 0.05;
  
  float kp_L = 30;
  float kd_L = 0.48;
  float ki_L = 0.05;
  
  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;

  // Read the position
  int posL = 0; 
  int posR = 0;
  noInterrupts(); // disable interrupts temporarily while reading
  posL = posiL;
  interrupts(); // turn interrupts back on
  noInterrupts(); // disable interrupts temporarily while reading
  posR = posiR;
  interrupts(); // turn interrupts back on
  
  // error
  int eL = posL - target_L;
  int eR = posR - target_R;

  // derivative
  float dedtL = (eL-eprevL)/(deltaT);
  float dedtR = (eR-eprevR)/(deltaT);

  // integral
  eintegralL = eintegralL + eL*deltaT;
  eintegralR = eintegralR + eR*deltaT;

  // control signal
  float uL = kp_L*eL + kd_L*dedtL + ki_L*eintegralL;
  float uR = kp_R*eR + kd_R*dedtR + ki_R*eintegralR;

  // motor power
  float pwrL = fabs(uL);
  if( pwrL > 255 ){
    pwrL = 255;
  }
  float pwrR = fabs(uR);
  if( pwrR > 255 ){
    pwrR = 255;
  }
  // motor direction
  int dirL = 1;
  if(uL<0){
    dirL = -1;
  }
  int dirR = 1;
  if(uR<0){
    dirR = -1;
  }

  // signal the motor
  setMotor(dirL,pwrL,L_PWM,L_IN1,L_IN2);
  setMotor(dirR,pwrR,R_PWM,R_IN1,R_IN2);


  // store previous error
  eprevL = eL;
  eprevR = eR;

  Serial.print(target_L);
  Serial.print(" ");
  Serial.print(target_R);
  Serial.print(" ");
  Serial.print(posL);
  Serial.print(" ");
  Serial.print(posR);
  Serial.println();
}

void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}

void readEncoderL(){
  int bL = digitalRead(L_ENCB);
  if(bL > 0){
    posiL++;
  }
  else{
    posiL--;
  }
}
void readEncoderR(){
  int bR = digitalRead(R_ENCB);
  if(bR > 0){
    posiR++;
  }
  else{
    posiR--;
  }
}

Image 1:(First Code)

Image 2:(Second Code, Check especially TargetL and posL)

cncnotes · July 1, 2022, 1:56am

Looks like you have done more than make a new code with 2 motors. You have also added WiFi.

Could you please tell us the exact boards you are using?
Have you tested only one motor with WiFi?

jremington · July 1, 2022, 2:15am

You made the classic mistake of adding too many functions to your code, without testing each one and making sure it works.

Start over with the one motor case and make sure it works.

Add the second motor and make sure it works. AVOID printing!

Adding the radio will randomly change the timing of the PID loops, which can be fatal (PID assumes a constant loop time). So does printing a lot of nonsense at the glacial rate of 9600 Baud -- increase that to 115200.

So, add that stuff in and let us know if anything still works.

newbieoftheday · July 1, 2022, 2:37am

Yeah, I forgot to mention that. But for now, Wi-Fi doesn't work. only target they are getting is the first one.

newbieoftheday · July 1, 2022, 2:41am

Wait a sec. Your last sentence is very important. Rn, radio is not working. It gets the target variable outside the if statements. Which prints are you talking about, exactly? There is already print on the first code. Does doubling the print variables affect that much?

TomGeorge · July 1, 2022, 2:42am

Hi,
What are you using for power supplies?

Can we please have a circuit diagram?
An image of a hand draw schematic will be fine, include ALL power supplies, component names and pin labels.

What hardware are you using?

Thanks.. Tom...

newbieoftheday · July 1, 2022, 2:45am

There is two power supplies. 5V(Arduino) and 8V(Battery). I am using Arduino Uno. I am not sure if I can share the circuit diagram for privacy reasons.

newbieoftheday · July 1, 2022, 2:50am

Baud rate solved everything. How can I work with radio? Will that be a problem with 115200 baud rate? If it will, can you help me on that topic, too? You are a life saver. Thank You.

TomGeorge · July 1, 2022, 3:08am

What do you mean. What has the radio got to do with the com port, do you mean the WiFi?
Is this a commercial project?
What is it?

Tom....

jremington · July 1, 2022, 3:38am

That is exactly the problem. Each iteration of loop() is also an iteration of the PID loop, and the loop timing is erratic because of print statements.

Variable loop timing leads directly to instability in PID loops.

At 9600 Baud, it takes one millisecond to print each and every character, both to the radio, and to the serial monitor. It also takes one millisecond to receive each character from the radio.

jremington · July 1, 2022, 3:43am

The project uses a 9600 Baud wireless serial connection (APC220 radio).

johnwasser · July 1, 2022, 4:44pm

This block of code is creating its own local variables that disappear a soon a the block ends. If you want to change the values of the existing variables of the same name:

  if(state == idle){ //0
    target_L = 0;
    target_R = 0;
    Serial.println("default");
  }

This mistake is repeated in five places.

system · December 28, 2022, 4:45pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
2 pid controllers on same arduino Motors, Mechanics, Power and CNC	10	5395	May 6, 2021
Help with PID controller! Programming	3	1177	May 5, 2021
PID control with two motors Programming	3	2429	October 3, 2021
Changing A Motor Code To Work For Two Motors Programming	7	477	November 28, 2022
Motor speed control doesn't work for 2 motors at the same time Programming	4	700	May 5, 2021

PID Controller Starts Stutering with 2 Motors

Related topics