PID problem on arduino

letxdudes · December 7, 2022, 12:45pm

Hi, i'm currently doing a PID controller for 3 motors, when I applied seperately, the result was quite acceptable, however, when combining them to one code, the noise oscillated really fast (I don't know how to say it right).
This is the result of one motor at a time (motor 2):

And this is the result of the combining (result of motor 2 only):

Is it possible if to much code slow down the process, I only use TimerOne to make the attachInterrupt for the PID every 0.01s, and the PWM pins are not 11, 12 which are related to TimerOne.
Here is the code I use:

#include <TimerOne.h>

#include <SoftwareSerial.h>
SoftwareSerial bluetooth(50,52);

// Động cơ 1 - thay đổi theo bảng xanh
#define ENA  5
#define IN11  31
#define IN12  33
#define ENCA 20
#define ENCB 21

// Động cơ 2 
#define ENA2  7
#define IN21 28
#define IN22 29
#define ENCA2 18
#define ENCB2 19 

// Động cơ 3
#define ENA3  8
#define IN31 25
#define IN32 23
#define ENCA3 2  
#define ENCB3 3  

char docgiatri = 's';

#define PPR 480
#define vtt 130

double T;
double tocdo1 = 0;  double xung1 = 0; double tocdodat1;
double tocdo3 = 0;  double xung2 = 0; double tocdodat2;
double tocdo2 = 0;  double xung3 = 0; double tocdodat3;

// PID động cơ 1
double Output1= 0; double LastOutput1 = 0;
double E_1, E1_1, E2_1;
double a_1, b_1, g_1, kp_1, ki_1, kd_1;

// PID động cơ 2
double Output2= 0; double LastOutput2 = 0;
double E_2, E1_2, E2_2;
double a_2, b_2, g_2, kp_2, ki_2, kd_2;

// PID động cơ 3
double Output3= 0; double LastOutput3 = 0;
double E_3, E1_3, E2_3;
double a_3, b_3, g_3, kp_3, ki_3, kd_3;

void follow_angle(int goc)
{
  float goc_rad = goc*PI/180.0;
  tocdodat1 = int(((2.0/3) * cos(goc_rad)) * vtt);
}

void demxung1()
{
  if (digitalRead(ENCB) == LOW) xung1++;
  else xung1--;
}

void demxung2()
{
  if (digitalRead(ENCB2) == LOW) xung2++;
  else xung2--;
}

void demxung3()
{
  if (digitalRead(ENCB3) == LOW) xung3++;
  else xung3--;
}

void PID_controller()
{ 
//  double present = millis();
  a_1 = 2*T*kp_1 + T*T*ki_1 + 2*kd_1;
  b_1 = T*T*ki_1 - 4*kd_1 - 2*T*kp_1;
  g_1 = 2*kd_1;

  a_2 = 2*T*kp_2 + T*T*ki_2 + 2*kd_2;
  b_2 = T*T*ki_2 - 4*kd_2 - 2*T*kp_2;
  g_2 = 2*kd_2;

  a_3 = 2*T*kp_3 + T*T*ki_3 + 2*kd_3;
  b_3 = T*T*ki_3 - 4*kd_3 - 2*T*kp_3;
  g_3 = 2*kd_3;
  
  tocdo1 =  ( abs((xung1/480.0) * (1/T)* 60));
  tocdo2 =  ( abs((xung2/480.0) * (1/T)* 60));
  tocdo3 =  ( abs((xung3/480.0) * (1/T)* 60));
  xung1 = 0;  xung2 = 0;  xung3 = 0;
  E_1 = tocdodat1 - tocdo1;
  E_2 = tocdodat2 - tocdo2;
  E_3 = tocdodat3 - tocdo3;
  Output1 = (a_1*E_1 + b_1*E1_1 + g_1*E2_1 + 2*T*LastOutput1)/(2.0*T);
  Output2 = (a_2*E_2 + b_2*E1_2 + g_2*E2_2 + 2*T*LastOutput2)/(2.0*T);
  Output3 = (a_3*E_3 + b_3*E1_3 + g_3*E2_3 + 2*T*LastOutput3)/(2.0*T);
  if (Output1 > 255) Output1 = 255;
  else if (Output1 < 0) Output1 = 0;
  if (Output2 > 255) Output2 = 255;
  else if (Output2 < 0) Output2 = 0;
  if (Output3 > 255) Output3 = 255;
  else if (Output3 < 0) Output3 = 0;
  LastOutput1 = Output1;
  E2_1 = E1_1;
  E1_1 = E_1;
  LastOutput2 = Output2;
  E2_2 = E1_2;
  E1_2 = E_2;
  LastOutput3 = Output3;
  E2_3 = E1_3;
  E1_3 = E_3;
}

void setup() {
  // put your setup code here, to run once:
  pinMode(ENCA, INPUT_PULLUP);
  pinMode(ENCB, INPUT_PULLUP);
  pinMode(ENA,OUTPUT);
  pinMode(IN11,OUTPUT);
  pinMode(IN12,OUTPUT);
  pinMode(ENCA2, INPUT_PULLUP);
  pinMode(ENCB2, INPUT_PULLUP);
  pinMode(ENA2,OUTPUT);
  pinMode(IN21,OUTPUT);
  pinMode(IN22,OUTPUT);
  pinMode(ENCA3, INPUT_PULLUP);
  pinMode(ENCB3, INPUT_PULLUP);
  pinMode(ENA3,OUTPUT);
  pinMode(IN31,OUTPUT);
  pinMode(IN32,OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA), demxung1, FALLING);
  attachInterrupt(digitalPinToInterrupt(ENCA2), demxung2, FALLING);
  attachInterrupt(digitalPinToInterrupt(ENCA3), demxung3, FALLING);
  TCCR1B = TCCR1B & 0b11111000 | 1;  
  T = 0.01;
//  kp_1 = 2.42; ki_1 = 2; kd_1 = 0;   
  kp_1 = 1.51; ki_1 = 0; kd_1 = 0;

//  kp_2 = 2.01; ki_2 = 1.8; kd_2 = 0;     
  kp_2 = 4; ki_2 = 6.47; kd_2 = 0;
  
//  kp_3 = 1.8; ki_3 = 1.8; kd_3 = 0;     
  kp_3 = 1.51; ki_3 = 0; kd_3 = 0;
  Serial.begin(9600);
  bluetooth.begin(9600);
  Timer1.initialize(5000);
  Timer1.attachInterrupt(PID_controller);
}

void loop() {
   if (bluetooth.available() > 0) docgiatri = bluetooth.read();
   
   Serial.print(tocdodat2); 
      
   Serial.print("\t"); Serial.print (tocdo2); Serial.print("\t"); Serial.println(Output2);
   switch (docgiatri)
   {
      case 's':
        tocdodat1 = 0;  tocdodat2 = 0;  tocdodat3 = 0;
        Output1 = 0;  LastOutput1 = 0;
        Output2 = 0;  LastOutput2 = 0;
        Output3 = 0;  LastOutput3 = 0;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
        digitalWrite(IN11, 0);  digitalWrite(IN12, 0);
        digitalWrite(IN21, 0);  digitalWrite(IN22, 0);
        digitalWrite(IN31, 0);  digitalWrite(IN32, 0);
//        pause = millis();
        break;
      case 'd':
        tocdodat1 = 100;  tocdodat2 = 50; tocdodat3 = 50;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
        digitalWrite(IN11, 1);  digitalWrite(IN12, 0);
        digitalWrite(IN21, 1);  digitalWrite(IN22, 0);  // Do bị ngược dây
        digitalWrite(IN31, 1);  digitalWrite(IN32, 0);  // Do bị ngược dây
        break;
      case 'a':
        tocdodat1 = 100;  tocdodat2 = 50; tocdodat3 = 50;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
//        analogWrite(ENA, 88);  analogWrite(ENA2, 100); analogWrite(ENA3, 100);
        digitalWrite(IN11, 0);  digitalWrite(IN12, 1);
        digitalWrite(IN21, 0);  digitalWrite(IN22, 1);  // Do bị ngược dây
        digitalWrite(IN31, 0);  digitalWrite(IN32, 1);  // Do bị ngược dây
        break;
      case 'w':
        tocdodat1 = 0;  tocdodat2 = 100; tocdodat3 = 100;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
        digitalWrite(IN11, 0);  digitalWrite(IN12, 0);
        digitalWrite(IN21, 1);  digitalWrite(IN22, 0);
        digitalWrite(IN31, 0);  digitalWrite(IN32, 1);
        break;
      case 'x':
        tocdodat1 = 0;  tocdodat2 = 100; tocdodat3 = 100;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
        digitalWrite(IN11, 0);  digitalWrite(IN12, 0);
        digitalWrite(IN21, 0);  digitalWrite(IN22, 1);
        digitalWrite(IN31, 1);  digitalWrite(IN32, 0);
        break;
      
//      case '3':
//        tocdodat1 = 50;
//        analogWrite(ENA, Output1);
//        digitalWrite(IN11, 0);  digitalWrite(IN12, 1);
//        break;
//      case '4':
//        tocdodat1 = 50;
//        analogWrite(ENA, (Output1));
//        digitalWrite(IN11, 1);  digitalWrite(IN12, 0);
//        break;
//      case '0':
//        tocdodat1 = 100;
//        analogWrite(ENA, (Output1));
//        digitalWrite(IN11, 0);  digitalWrite(IN12, 1);
//        break;
      case '9': // 
        tocdodat1 = 100;  tocdodat2 = 50; tocdodat3 = 50;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
        digitalWrite(IN11, 0);  digitalWrite(IN12, 1);
        digitalWrite(IN21, 0);  digitalWrite(IN22, 1); 
        digitalWrite(IN31, 0);  digitalWrite(IN32, 1);
        break;
      case '0': // 
        tocdodat1 = 100;  tocdodat2 = 50; tocdodat3 = 50;
        analogWrite(ENA, Output1);  analogWrite(ENA2, Output2); analogWrite(ENA3, Output3);
        digitalWrite(IN11, 1);  digitalWrite(IN12, 0);
        digitalWrite(IN21, 1);  digitalWrite(IN22, 0); 
        digitalWrite(IN31, 1);  digitalWrite(IN32, 0);
        break;
   }  
}

Thank you very much!!!

pylon · December 7, 2022, 3:44pm

Of course, code needs time to execute, the more code you have to more time it needs (this is simplified, it depends on how you write the code too).

You do the majority of calculations inside an interrupt. During these calculations the interrupts for the data are disabled. I strongly suggest to remove the timer interrupt and move these calculations to the loop, controlling the execution start by millis().
You do about 70 floating point multiplications. Such a calculation needs quite some time. You seem to use an Arduino Mega2560, there double is equal to float, otherwise using float would save you some time.
One float multiplication needs about 10 µs, so your interrupt handler blocks the complete processor for almost 1ms, as task you do every 10ms!

build_1971 · December 7, 2022, 4:23pm

Howmany of these floating point calcs have been optimized away?
Perhaps you could register micros() through 10 loops and then print the result...

Maybe OP could move to integer maths...

Why read the status of the enable pins from the registers? Isn't that info already available in a variable?
Serial.print() is also time consuming...

pylon · December 7, 2022, 6:38pm

Yes, but isn't done in interrupt context so it shouldn't influence the other ISRs.

build_1971 · December 7, 2022, 7:28pm

It us in loop and so it may retard the writing to the motor controllers. Or am I overlooking something?

letxdudes · December 7, 2022, 11:40pm

Th
Hi, after you said it, I cut off the a_1, b_1, g_1, ... part inside the interrupt to take it out to the main loop since it is unchanged after time, but the Output1, maybe it shouldn't be anywhere outside the interrupt don't you think.

build_1971 · December 8, 2022, 4:17am

Please send your new code in a new post...

pylon · December 8, 2022, 4:31pm

Remove the complete time1 interrupt! Move all the calculations to the loop() and use millis() to time that.

build_1971 · December 8, 2022, 4:43pm

Why is xung a double? I think it should be integer...

pylon · December 8, 2022, 5:02pm

Maybe to save the time for the integer to float conversion.

system · June 6, 2023, 5:02pm

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

Topic		Replies	Views
PID control of DC motor Motors, Mechanics, Power and CNC	27	1308	July 9, 2025
PID not behaving correctly for motor speed control? Programming	40	744	January 27, 2025
Implementing PID control using Timerone library Programming	44	1280	May 10, 2024
Commande de moteur par pid Français	41	504	July 8, 2025
PID Controller Starts Stutering with 2 Motors Motors, Mechanics, Power and CNC	12	1146	December 28, 2022

PID problem on arduino

Related topics