Working with ArduPID for Motor Speed Control

Hello All,

I have been working on my project a lot the last couple of weeks. I have a decent speed measurement but when letting the bot drive around I noticed that the speeds of mymotors were not equal so I implemented a PID control. This improved all a bit. But today I'm doing some tests and noticed something weird. 

I send a speed setting in m/s to my bot. And than the PID controller should do its work to create the correct output in order to get the same speed for both motors. But when I place some extra friction on one motor I wont see the output of the PID changing? I'm wondering if I made a mistake with coding or I did not understand the way it works.

Below a recording of one motor

You can see the speed drops so I would expect that the output signal tries to increase in order to compensate for the error but it maintains the same ouput.

I have added my code whcih start to get a bit long. In the Loop() the PID is asked to compute the signals. in the function Cmd_DriveForwards() it receives the setpoint.  

Again I think when a motor is asked for more power the speed reduces and than the output signal should increase if I understand it correct.

Maybe someone has an idea why this is not happening?

//************************************************************************************************************************************//
//                                                                                                                                    //
//  Project: Rover                                                                                                                    //
//  Version: V0.2                                                                                                                     //
//  Date: Feb-2023                                                                                                                    //
//  Current version capabilities:                                                                                                     //
//        - Drive forward option                                                                                                      //
//        - Speed sensing of Left and Right motors                                                                                    //
//        - Interface with MegunoLink for speed monitoring and speed control                                                          //
//        V0.1:                                                                                                                       //
//        - Adjusted speed control hardware with Logic inverter port, so two pins less will be used                                   //
//        - Added WiFI communication with Megunolink                                                                                  //
//        - Clean up Loop() by adding new functions                                                                                   //
//        - Added controls for driving Forward, Backwards, Left, Right and Stop                                                       //
//        V0.2:                                                                                                                       //
//        - Changed variable names for more logical names                                                                             //
//        -                                                                                                                           //
//************************************************************************************************************************************//

// Include SSID and password from a library file.
#if defined(ARDUINO_ARCH_ESP32)
#include "WiFi.h"
#include <ESPmDNS.h>
#elif defined(ARDUINO_ARCH_ESP8266)
#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#else
#error Not compatible with the selected board. 
#endif

#include <MegunoLink.h>
#include "CircularBuffer.h"
#include "ESPTCPCommandHandler.h"
#include "CommandProcessor.h"
#include "ArduinoTimer.h"
#include "ArduPID.h"


#define USEWIFICONFIGFILE
#ifdef USEWIFICONFIGFILE

// Include SSID and password from a library file. 
#include "WiFiConfig.h"
#else
// Option 2
const char *SSID = "Your SSID";
const char *WiFiPassword = "Your Password";
#endif

//Variables used for WiFi Server
const uint8_t ServerPort = 23;
WiFiServer Server(ServerPort);
ArduinoTimer SendTimer;
uint32_t PlottingPeriod = 200;
const int MaxConnections = 2;
TcpCommandHandler<MaxConnections> Cmds(Server);
CommandProcessor<> SerialCmds(Cmds);

String MakeMine(const char *NameTemplate);

//Global declarations
#define PWM_A 14                                                                                              //PWM Channel for left motor
#define PWM_A_Chan 0
#define AI_1 12                                                                                               //Enable Channel left motor, logic "0" is forward, inverter IC takes care of inversing signal for AI_2
#define PWM_B 25                                                                                              //PWM Channel for right motor
#define PWM_B_Chan 1
#define BI_1 26                                                                                               //Enable Channel right motor, logic "0" is forward, inverter IC takes care of inversing signal for BI_2
#define PWM_Res 8
#define PWM_Freq 15000
#define buffer_size 7                                                                                         //Buffer size for computing average of speed sensing signal
#define PI 3.14159265359

const byte slots = 20; 

// timing variables rightside motor speed sensor                                                              //Total slots on motor disk
volatile unsigned long usRight;
volatile unsigned long prevPulseUsRight; 
volatile unsigned long pulseUsRight;
volatile unsigned long prevPulseUsCopyRight; 
volatile unsigned long pulseUsCopyRight;
unsigned long pulsePeriodRight;
unsigned long AvgPulseTimeRight;

// timing variables leftside motor speed sensor
volatile unsigned long usLeft;
volatile unsigned long prevPulseUsLeft; 
volatile unsigned long pulseUsLeft;
volatile unsigned long prevPulseUsCopyLeft; 
volatile unsigned long pulseUsCopyLeft;
unsigned long pulsePeriodLeft;
unsigned long AvgPulseTimeLeft;

unsigned long prevMs;
unsigned long now;

// Variables used for calculation of Rotations Per Sec and AVG speed
float rpsRight = 0;
float rpsLeft = 0;
float AvgVelocityRight;
float AvgVelocityLeft;

float pwm;
int PWM_DutyCycle;

const unsigned long slotUs = 1000000 / slots;
const int RightMotorSpeedSens = 19;                                                       //Right motor Interrupt pin18 for speed sensing 
const int LeftMotorSpeedSens = 18;                                                        //Left motor Interrupt pin19 for speed sensing 
const float WheelDiameter = 0.0664;                                                       //Wheel diameter
unsigned int RightSpeedCount = 0; 
unsigned int LeftSpeedCount = 0;            
unsigned long tempbufferRight[buffer_size];
unsigned long tempbufferLeft[buffer_size];


//Define MegunoLink GUI
InterfacePanel MyPanel; 
typedef CircularBuffer<unsigned long, buffer_size> PulsePeriodBuffer;                                
PulsePeriodBuffer PulseTimesRight;
PulsePeriodBuffer PulseTimesLeft;


//PID Control variables
ArduPID LeftSpeedController;
ArduPID RightSpeedController;

double LeftInput;
double LeftOutput;
double RightInput;
double RightOutput;

double SpeedSetpoint;
double LeftKp = 2.2;
double LeftKi = 6;
double LeftKd = 2;
double RightKp = 2.5;
double RightKi = 9;
double RightKd = 2;

//Interrupt for right speed sensor 
void IRAM_ATTR isrRight()
{
  usRight = micros();
  if ((usRight - pulseUsRight) > 7500)                                                               // debounce interval, also determines max rpm
  {  
    prevPulseUsRight = pulseUsRight;
    pulseUsRight = usRight;
  }
}

//Interrupt for left speed sensor
void IRAM_ATTR isrLeft()
{
  usLeft = micros();
  if ((usLeft - pulseUsLeft) > 7500)                                                               // debounce interval, also determines max rpm
  {  
    prevPulseUsLeft = pulseUsLeft;
    pulseUsLeft = usLeft;
  }
}

bool timeOut(unsigned long ms) 
{
  now = millis();
  if ((now - prevMs) >= ms) 
  {
    prevMs = now;
    return true;
  }
  return false;
}

//WiFi connect function, checks SSID and Password shows if a connection is made and displays the IP address on the serial port
void ConnectToWiFi()
{
  WiFi.mode(WIFI_STA);
  WiFi.begin(SSID, WiFiPassword);
  Serial.print("Connecting to "); Serial.println(SSID);

  uint8_t i = 0;
  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print('.');
    delay(500);

    if ((++i % 16) == 0)
    {
      Serial.println(F(" still trying to connect"));
    }
  }

  Serial.print(F("Connected. My IP address is: "));
  Serial.println(WiFi.localIP());
}

//mDNS function to show device name, printed on the serial port
void AdvertiseServices()
{
  String MyName = MakeMine("MyDevice");
  if (MDNS.begin(MyName.c_str()))
  {
    Serial.println(F("mDNS responder started"));
    Serial.print(F("My name is: "));
    Serial.println(MyName.c_str());

    // Add service to MDNS-SD
    MDNS.addService("n8i-mlp", "tcp", ServerPort);
  }
  else
  {
    Serial.println(F("Error setting up MDNS responder"));
  }
}

/* Returns a semi-unique id for the device. The id is based
*  on part of a MAC address or chip ID so it won't be 
*  globally unique. */
uint16_t GetDeviceId()
{
#if defined(ARDUINO_ARCH_ESP32)
  return ESP.getEfuseMac();
#else
  return ESP.getChipId();
#endif
}

/* Append a semi-unique id to the name template */
String MakeMine(const char *NameTemplate)
{
  uint16_t uChipId = GetDeviceId();
  String Result = String(NameTemplate) + String(uChipId, HEX);
  return Result;
}

void Cmd_ListAll(CommandParameter &Parameters)
{
  Parameters.GetSource().print(F("PlottingPeriod [ms]="));
  Parameters.GetSource().println(PlottingPeriod);
}

 

void Cmd_SetPlottingPeriod(CommandParameter &Parameters)
{
  PlottingPeriod = Parameters.NextParameterAsInteger(PlottingPeriod);
}

 

void Cmd_Unknown()
{
  Serial.println(F("I don't understand"));
}


void setup() {
  //Setup Serial connection
  Serial.begin(115200);
  Serial.println(F("......Program starts....."));

  //Setup WiFI connection
  ConnectToWiFi();
  AdvertiseServices();
  // Start the TCP server
  Server.begin();
  Server.setNoDelay(true);
  

  // Setup the serial commands to MegunoLink
  Cmds.AddCommand(F("PlottingPeriod"), Cmd_SetPlottingPeriod);
  Cmds.AddCommand(F("ListAll"), Cmd_ListAll);
  Cmds.SetDefaultHandler(Cmd_Unknown);
  Cmds.AddCommand(F("MotorSpeed"), Cmd_DriveForwards);                //Command to communicate with Megunolink
  Cmds.AddCommand(F("btnDriveForward"), Cmd_DriveForwards);
  Cmds.AddCommand(F("btnDriveBackwards"), Cmd_DriveBackwards);
  Cmds.AddCommand(F("btnDriveLeft"), Cmd_DriveLeft);
  Cmds.AddCommand(F("btnDriveRight"), Cmd_DriveRight);
  Cmds.AddCommand(F("btnEmergStop"), Cmd_Stop);    


  pinMode(AI_1, OUTPUT);                                              //A motor setup output channel, Left motor
  pinMode(BI_1, OUTPUT);                                              //B motor setup output channel, Right motor
  pinMode(RightMotorSpeedSens, INPUT_PULLUP);                         //Setup input channels for speed sensing
  pinMode(LeftMotorSpeedSens, INPUT_PULLUP);

  ledcAttachPin(PWM_A, PWM_A_Chan);                                   //Setup A motor PWM channel
  ledcAttachPin(PWM_B, PWM_B_Chan);                                   //Setup B motor PWM channel
  ledcSetup(PWM_A_Chan, PWM_Freq, PWM_Res);
  ledcSetup(PWM_B_Chan, PWM_Freq, PWM_Res);

  //Setup interrupt ISR for speedsensing
  attachInterrupt(digitalPinToInterrupt(RightMotorSpeedSens), isrRight, RISING);
  attachInterrupt(digitalPinToInterrupt(LeftMotorSpeedSens), isrLeft, RISING);

  //Setup PID controllers for motor control
  LeftSpeedController.begin(&LeftInput, &LeftOutput, &SpeedSetpoint, LeftKp, LeftKi, LeftKd);
  RightSpeedController.begin(&RightInput, &RightOutput, &SpeedSetpoint, RightKp, RightKi, RightKd);

  LeftSpeedController.setOutputLimits(0,255);                                         //Output limits for controller
  RightSpeedController.setOutputLimits(0,255);
  LeftSpeedController.setBias(0);                                                     //Create a Bias
  RightSpeedController.setBias(0);
  LeftSpeedController.setWindUpLimits(-10,10);                                        //Bounds for the integral term to prevent integral wind-up
  RightSpeedController.setWindUpLimits(-10,10);
  LeftSpeedController.start();
  RightSpeedController.start();
  
  Serial.println("Setup Ready...");

}

void loop() {
    #if defined(ARDUINO_ARCH_ESP8266)
      MDNS.update();
    #endif
  
  SerialCmds.Process();                                                                                     //Monitor serial commands
  Cmds.Process();                                                                                           //Monitor WiFi commands
    
  SpeedSensing();

  InterfacePanel MyPanel("", Cmds); 
                                                     
  MyPanel.SetNumber(F("RightSpeedGauge"), AvgVelocityRight);
  MyPanel.SetNumber(F("LeftSpeedGauge"), AvgVelocityLeft);                                                  // Set control value

  

  if (SendTimer.TimePassed_Milliseconds(PlottingPeriod))
  {
      Serial.println("~");
      TimePlot MyPlot("", Cmds);                                                                          //Needs to use Cmds to access the connections
      //Send Data To MegunoLink Pro
      MyPlot.SendData(F("Left Speed"), AvgVelocityLeft); 
      MyPlot.SendData(F("Left Output"), LeftOutput/100); 
      MyPlot.SendData(F("Right Speed"), AvgVelocityRight); 
      MyPlot.SendData(F("Right Output"), RightOutput/100); 
      MyPlot.SendData("Setpoint", pwm/100);                                                               //PWM signal divided by 100 to scale equal with speed                                       
  }

  LeftInput = AvgVelocityLeft;
  RightInput = AvgVelocityRight;  
  LeftSpeedController.compute();
  RightSpeedController.compute();
  ledcWrite(PWM_A_Chan, LeftOutput);
  ledcWrite(PWM_B_Chan, RightOutput);
}


void Cmd_DriveForwards(CommandParameter& p)                                                           //Function for driving forward
{
  int SP = p.NextParameterAsInteger();
  SpeedSetpoint = (double) SP;
  digitalWrite(AI_1, HIGH);
  digitalWrite(BI_1, LOW);
  pwm = SpeedSetpoint;   
}

void Cmd_DriveBackwards(CommandParameter& p)                                                           //Function for driving Backwards
{
  int PWR = p.NextParameterAsInteger();  
  digitalWrite(AI_1, HIGH);
  digitalWrite(BI_1, LOW);
  ledcWrite(PWM_A_Chan, PWR);
  ledcWrite(PWM_B_Chan, PWR);
  pwm = PWR;   
}

void Cmd_DriveLeft(CommandParameter& p)                                                               //Function for driving Left
{
  unsigned long end = millis() + 200;
  int PWR = 65; 

  while (millis() < end)
  {
     
    digitalWrite(AI_1, HIGH);
    digitalWrite(BI_1, HIGH);
    ledcWrite(PWM_A_Chan, PWR);
    ledcWrite(PWM_B_Chan, PWR);
    pwm = PWR; 

  }
  PWR = 0;
  ledcWrite(PWM_A_Chan, PWR);
  ledcWrite(PWM_B_Chan, PWR);
}

void Cmd_DriveRight(CommandParameter& p)                                                              //Function for driving Right
{
  unsigned long end = millis() + 200;
  int PWR = 65;   
  
  
  digitalWrite(AI_1, LOW);
  digitalWrite(BI_1, LOW);
  ledcWrite(PWM_A_Chan, PWR);
  ledcWrite(PWM_B_Chan, PWR);
  pwm = PWR;   
}

void Cmd_Stop(CommandParameter& p)                                                           //Function for stop driving
{
  int PWR = 0;  
  digitalWrite(AI_1, LOW);
  digitalWrite(BI_1, HIGH);
  ledcWrite(PWM_A_Chan, PWR);
  ledcWrite(PWM_B_Chan, PWR);
  pwm = PWR;   
}

void averagebuffer()
{
  AvgPulseTimeRight = 0;
  AvgPulseTimeLeft = 0;
  for(int a=0;a<buffer_size;a++)
  {
    AvgPulseTimeRight += tempbufferRight[a];                                              //Sum up buffer values
    AvgPulseTimeLeft += tempbufferLeft[a];    
  }

  AvgPulseTimeRight = AvgPulseTimeRight/buffer_size;                                      //Compute average of buffer values by dividing by buffer length
  AvgPulseTimeLeft = AvgPulseTimeLeft/buffer_size;
}

void MoveAverageSpeed()
//This function computes the avverage for Left and Right speed values 
{
    for(PulsePeriodBuffer::ForwardIterator Iterator(PulseTimesRight); Iterator.AtEnd() == false; Iterator.Next())
    {
    tempbufferRight[Iterator.ItemNumber()]=Iterator.CurrentValue();
    }
    
    for(PulsePeriodBuffer::ForwardIterator Iterator(PulseTimesLeft); Iterator.AtEnd() == false; Iterator.Next())
    {
    tempbufferLeft[Iterator.ItemNumber()]=Iterator.CurrentValue();
    }
    averagebuffer();
}

void SpeedSensing()
//This functions determines the current speed of Left and Right wheels
{
  if (timeOut(250)) 
  {                                                                                       // 4 Hz update frequency
    cli();                                                                                //stop interrupts
    prevPulseUsCopyRight = prevPulseUsRight;
    pulseUsCopyRight = pulseUsRight;
    prevPulseUsCopyLeft = prevPulseUsLeft;
    pulseUsCopyLeft = pulseUsLeft;
    sei();                                                                                //allow interrupts
    pulsePeriodRight = pulseUsCopyRight - prevPulseUsCopyRight;                           //Length of one pulse period
    pulsePeriodLeft = pulseUsCopyLeft - prevPulseUsCopyLeft;

    PulseTimesRight.Add(pulsePeriodRight);
    PulseTimesLeft.Add(pulsePeriodLeft);
    MoveAverageSpeed();
    if(pulsePeriodRight != 0 || pulsePeriodLeft != 0) 
    {
    rpsRight = (float)slotUs / (float)AvgPulseTimeRight ;                                   //Compute RPS value
    AvgVelocityRight = (PI * WheelDiameter) * rpsRight;                                     //Compute measured speed in m/s right wheel
    rpsLeft = (float)slotUs / (float)AvgPulseTimeLeft ;                                     //Compute RPS value
    AvgVelocityLeft= (PI * WheelDiameter) * rpsLeft;                                        //Compute measured speed in m/s left wheel
    }
  }
  
  if(pwm == 0)
  {
    AvgVelocityLeft = 0;
    AvgVelocityRight = 0;
  }
}