Arduino PID Controller

Timer1
l298n-dual-motor-controller-module-2a-and-arduino
Discrete PID

By refererring to the above documentations, I managed to write a PID controlled Arduino Car with 2 speed controllers powered by an L298N Arduino Motor Shield. The Arduino used is an Arduino Nano with CH340 USB to Serial converter.

In Timers-Arduino wiki:

PWM and timers:

There is fixed relation between the timers and the PWM capable outputs. When you look in the data sheet or the pinout of the processor these PWM capable pins have names like OCRxA, OCRxB or OCRxC (where x means the timer number 0..5). The PWM functionality is often shared with other pin functionality.

The Arduino has 3Timers and 6 PWM output pins. The relation between timers and PWM outputs is:

Pins 5 and 6: controlled by timer0
Pins 9 and 10: controlled by timer1
Pins 11 and 3: controlled by timer2

In speed-sensor-with-arduino tutorial:
since the L298N demo uses PWM, and we need the use of timer1 for accurate timing of intervals for the speed wheel demo, we cannot use pins 9 and 10. We need to use Pin 11 for the second PWM which is used only by timer2.

For PID code, we use the algorithm in Discrete PID to adjust the power to be output to the PWM pins based on the difference between the desired speed and the measured speed, done by the speed wheel sensor of the Arduino car.

However it is not easy to tune. I have not managed to get lowest speed possible. It is better to just set the minimum power. The reason is due to the huge non-linearity of the motor and gear of the Arduino car.

Standard Arduino car, L298N, Nano and LM393 speed sensor.

The code is:
 #include <TimerOne.h>

//set the desired speed here
unsigned int desired_speed=1;
unsigned int desired_speed2=1;

//variables for the PID controller
unsigned int measured_speed=0;
unsigned int measured_speed2=0;
unsigned int total_pulses=0;
unsigned int total_pulses2=0;

unsigned int set_power=0;
unsigned int set_power2=0;
unsigned int maximum_power=255;
unsigned int maximum_power2=255;
unsigned int minimum_power=20;
unsigned int minimum_power2=20;

//tune these PID coefficients
float Kp=16;
float Ki=0.125;
float Kd=0.125;

float Kp2=16;
float Ki2=0.125;
float Kd2=0.125;

int error=0;
int prev_error=0;
int integral=0;
int diff=0;

int error2=0;
int prev_error2=0;
int integral2=0;
int diff2=0;

// connect motor controller pins to Arduino digital pins
// motor one
int enA = 11;
int in1 = 9;
int in2 = 8;
// motor two
int enB = 5;
int in3 = 7;
int in4 = 6;

void docount()  // counts from the speed sensor
{
  measured_speed++;  // increase +1 the measured_speed value
  total_pulses += measured_speed;
}
void docount2()  // counts from the speed sensor
{
  measured_speed2++;  // increase +1 the measured_speed value
  total_pulses2 += measured_speed2;
} 
void timerIsr()
{
  Timer1.detachInterrupt();  //stop the timer
 
  error = measured_speed - desired_speed;
  integral += (error + prev_error)* Ki;
  diff = (error - prev_error)* Kd;

  if (set_power < maximum_power) set_power -= ((error * Kp) + integral + diff);
  else {set_power =minimum_power; integral =0;};

  error2 = measured_speed2 - desired_speed2;
  integral2 += (error2 + prev_error2)* Ki;
  diff2 = (error2 - prev_error2)* Kd;

  if (set_power2 < maximum_power2) set_power2 -= ((error2 * Kp) + integral2 + diff);
  else {set_power2 = minimum_power2; integral2 = 0;};

  prev_error = error;
  prev_error2 = error2;

  int temp = (measured_speed);
  Serial.print(temp,DEC); 
 
  temp = (measured_speed2);
  Serial.print(temp,DEC);
 
  Serial.println();

/*

  Serial.print(" measured_speed:");
  int temp = (measured_speed);
  Serial.print(temp,DEC); 
 
  Serial.print(" measured_speed2:");
  temp = (measured_speed2);
  Serial.print(temp,DEC); 
 
  Serial.print(" set_power:");
  temp = (set_power);
  Serial.print(temp,DEC); 
 
  Serial.print(" set_power2:");
  temp = (set_power2);
  Serial.print(temp,DEC); 

  Serial.print(" total_pulses:");
  temp = (total_pulses);
  Serial.print(temp,DEC); 
 
  Serial.print(" total_pulses2:");
  temp = (total_pulses2);
  Serial.print(temp,DEC); 

  Serial.print(" error:");
  temp = (error);
  Serial.print(temp,DEC); 
 
  Serial.print(" error2:");
  temp = (error2);
  Serial.print(temp,DEC);

  Serial.print(" integral:");
  temp = (integral);
  Serial.print(temp,DEC); 
 
  Serial.print(" integral2:");
  temp = (integral2);
  Serial.print(temp,DEC); 

  Serial.print(" diff:");
  temp = (diff);
  Serial.print(temp,DEC); 
 
  Serial.print(" diff2:");
  temp = (diff2);
  Serial.print(temp,DEC); 
 
  Serial.println(" End");
 
*/
 
  measured_speed=0;  //  reset measured_speed to zero
  measured_speed2=0;
  Timer1.attachInterrupt( timerIsr );  //enable the timer
}

void setup() {
  // put your setup code here, to run once:
// set all the motor control pins to outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
 
  //Serial.begin(9600);
  Serial.begin(9600);
  enable_interrupt(); 
}
void enable_interrupt()
{
  Timer1.initialize(100000); // set timer for 0.1 sec
  attachInterrupt(digitalPinToInterrupt(2), docount, RISING);  // increase measured_speed when speed sensor pin goes High
  attachInterrupt(digitalPinToInterrupt(3), docount2, RISING);  // increase measured_speed when speed sensor pin goes High
  Timer1.attachInterrupt( timerIsr ); // enable the timer
}

void disable_interrupt()
{
  detachInterrupt(digitalPinToInterrupt(2));  // increase measured_speed when speed sensor pin goes High
  detachInterrupt(digitalPinToInterrupt(3));  // increase measured_speed when speed sensor pin goes High
  Timer1.detachInterrupt(); // enable the timer
}


void demoStraightPID()
{
// this function will run the motors in a straight line at a fixed speed

//set the desired speed here
  desired_speed=1;
  desired_speed2=1;

  unsigned int lpower = (set_power);
  unsigned int lpower2 = (set_power2);
  analogWrite(enA, lpower);
  analogWrite(enB, lpower2);
 
// turn on motor A
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
// turn on motor B: faulty, cannot reverse
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}

void demoStop()
{
 // now turn off motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

void demoOne()
{
// this function will run the motors in a rectangle at a fixed speed
// set speed to 200 out of possible range 0~255
    analogWrite(enA, 70);
    analogWrite(enB, 70);

  // run straight
  // turn on motor A
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
  // turn on motor B: faulty, cannot reverse
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  delay(1000);
  // turn left
   // turn on motor A
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
  // turn on motor B: faulty, cannot reverse
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  delay(500); 
 // now turn off motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW); 
  delay(2000);
}
void demoTwo()
{
  // this function will run the motors across the range of possible speeds
  // note that maximum speed is determined by the motor itself and the operating voltage
  // the PWM values sent by analogWrite() are fractions of the maximum speed possible
  // by your hardware
  // turn on motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  // accelerate from zero to maximum speed
  for (int i = 0; i < 256; i++)
  {
    analogWrite(enA, i);
    analogWrite(enB, i);
    delay(20);
  }
  // decelerate from maximum speed to zero
  for (int i = 255; i >= 0; --i)
  {
    analogWrite(enA, i);
    analogWrite(enB, i);
    delay(20);
  }
  // now turn off motors
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW); 
}

void loop() {
  // put your main code here, to run repeatedly:
  //disable_interrupt();
  //demoOne();
//  demoTwo();
  demoStraightPID();
 // delay(5000);

}