Hello All,
I'm trying to use the ArduPID library. I think I use it the worng way or maybe I do not understand it completely. I use the library for speed control. So I have a speedsensor which computes the speed in m/s. Than I give a setpoint e.g. 0.5 m/s and I beleive the controller should try to reach the given setpoint. The motors are driven by a PWM signal which goes from 0 to 255.
Now when I give a certain setpoint the controller gives and PWM output but the "weird" thing is that when I apply a bit of extra load to one of the wheels, so the speed decreases I dont see the PWM output increasing. I would expect that if the Error increases the output signal should increase as well in order to compensate for that error.
Probably I do something wrong. My full code is as follows:
//************************************************************************************************************************************//
// //
// Project: Rover //
// Version: V0.1 //
// 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 //
// - Added PID control for Left and Right motors //
// V0.3: //
// - Changed filter type of speed sens signals to a Exponential filter from MegunoLink Library //
// - //
//************************************************************************************************************************************//
// 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"
#include "Filter.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 PI 3.14159265359
const byte slots = 20;
// timing variables rightside motor speed sensor //Total slots on motor disk
long usRight;
long prevPulseUsRight;
long pulseUsRight;
long prevPulseUsCopyRight;
long pulseUsCopyRight;
long pulsePeriodRight;
long AvgPulseTimeRight;
// timing variables leftside motor speed sensor
long usLeft;
long prevPulseUsLeft;
long pulseUsLeft;
long prevPulseUsCopyLeft;
long pulseUsCopyLeft;
long pulsePeriodLeft;
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;
//Create Filter instances
ExponentialFilter<long> LeftSpeedFilter(20, 0);
ExponentialFilter<long> RightSpeedFilter(20, 0);
//Define MegunoLink GUI
InterfacePanel MyPanel;
//PID Control variables
ArduPID LeftSpeedController;
ArduPID RightSpeedController;
double LeftInput;
double LeftOutput;
double RightInput;
double RightOutput;
double SpeedSetpoint;
double LeftKp = 2.5;
double LeftKi = 0;
double LeftKd = 0;
double RightKp = 2.5;
double RightKi = 0;
double RightKd = 0;
//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 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;
LeftSpeedFilter.Filter(pulsePeriodLeft);
RightSpeedFilter.Filter(pulsePeriodRight);
if(pulsePeriodRight != 0 || pulsePeriodLeft != 0)
{
rpsRight = (float)slotUs / (float)RightSpeedFilter.Current() ; //Compute RPS value
AvgVelocityRight = (PI * WheelDiameter) * rpsRight; //Compute measured speed in m/s right wheel
rpsLeft = (float)slotUs / (float)LeftSpeedFilter.Current() ; //Compute RPS value
AvgVelocityLeft= (PI * WheelDiameter) * rpsLeft; //Compute measured speed in m/s left wheel
}
}
if(pwm == 0)
{
AvgVelocityLeft = 0;
AvgVelocityRight = 0;
}
}
This is a plot of the right motor.
I noticed that the table below doesn't make sense for readers, so a explanation.
Neglect the first two rows from the top. Than:
3rd value is the sensed speed unit is m/s
4th value is the output signal from the PID controller, unit is PWM (0-255). In order to scale it on the graph I have devided the value by 100. So real value is in the table 0.62 * 100 = 62
5th value is the setpoint unit if this value is m/s
Well I hope someone could point out where I go wrong, thanks in advance
