I am building an Inverted Pendulum on a cart using a Linear-axis with a stepper motor and a rotary encoder for the pendulum angle.
The PID control of the system works okay, but needs a little tuning.
The goal with this project is to implement a latency to the PID-control to simulate what happens to control systems with latency.
I implemented a circular buffer which saves all the output from the PID-conroller with a timestamp and releases them after a specified latency.
My Problem:
My code is to slow to get the stepper motor (400 steps/turn) to a decend enough speed.
(without the circular buffer it works fine).
Do you guys have any idea how I can speed up/optimize my code to achieve the goal?
Here is the Code with the circular buffer:
#include <AccelStepper.h>
// Pendulum encoder pins
#define OUTPUT_A 3 // PE5
#define OUTPUT_B 2 // PE4
volatile long encoderValue = 0L;
volatile long lastEncoded = 0L;
// Define variables for running the motor
const long max_speed = 5000;
// Define pins for motor
const int stepPin = 4;
const int dirPin = 5;
AccelStepper stepper(1, stepPin, dirPin); // (Typeof driver: with 2 pins, STEP, DIR)
/* Define Variables for PID Control */
float kp = 600; //300 bei 200 Schritten.
float ki = 0;
float kd = 0.001;
//Declare necessary global variables to be passed between calls of ComputPID()
long Setpoint, output, error;
unsigned long Told;
float ErrorSum, ErrorOld;
volatile int input;
/* Define Variables for PID_POS Control */
float kp_POS = 0.0;
float ki_POS = 0;
float kd_POS = 0.0;
//Declare necessary global variables to be passed between calls of ComputPID()
float Setpoint_POS = 0, Output_POS, error_POS;
unsigned long Told_POS;
float ErrorSum_POS, ErrorOld_POS;
volatile int input_POS;
// Circular buffer for outgoing data
const int bufferSize = 500;
struct DataPoint {
unsigned long timestamp;
long value;
};
DataPoint outgoingData[bufferSize];
int writeIndex = 0;
int readIndex = 0;
long delayedOutput;
// Latency parameters
unsigned long latency = 100; // Latency in milliseconds
void encoderHandler();
void CompudePID_POS();
void ComputePID();
void sendDelayedData();
void setup() {
// Motor Parameters and pins
stepper.setMaxSpeed(max_speed);
pinMode(OUTPUT_A, INPUT_PULLUP);
pinMode(OUTPUT_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(OUTPUT_A), encoderHandler, CHANGE);
attachInterrupt(digitalPinToInterrupt(OUTPUT_B), encoderHandler, CHANGE);
// Serial monitor for debugging
//Serial.begin(115200);
}
void loop() {
input_POS = stepper.currentPosition(); //Get current cart position
ComputePID_POS(); //Compute the Output_POS
Setpoint = 1203 - Output_POS; //Set the Setpoint of the ComputePID() with the Output_POS from ComputePID_POS
input = encoderValue; //Read the Encoder value (1200 is upright)
if (input <= 1400 && input >= 1000) { //Check if the pendulum is in the conrollable range (+- 200 around the Setpoint)
ComputePID();
sendDelayedData();//Compute the output for the motor speed
// move the motor
stepper.setSpeed(delayedOutput); //Set Motor speed to the output of ComputePID()
stepper.runSpeed(); //Run that speed
}
}
/* Reads the Pendulum-Encoder */
void encoderHandler() {
int MSB = digitalRead(3); //MSB = most significant bit
int LSB = digitalRead(2); //LSB = least significant bit
int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
__disable_irq(); // Disable interrupts while updating shared variables
// Update encoder value based on the current and previous states
if ((lastEncoded == 0b00 && encoded == 0b01) || (lastEncoded == 0b11 && encoded == 0b10)) {
encoderValue++; // CW
} else if ((lastEncoded == 0b01 && encoded == 0b00) || (lastEncoded == 0b10 && encoded == 0b11)) {
encoderValue--; // CCW
}
lastEncoded = encoded; //store this value for next time
__enable_irq();
}
/* Computes the output (speed of Motor) with the Pendulum angle as input */
void ComputePID() {
// PID Control
error = Setpoint - input; // Calculate error between Setpoint and Input from Encoder
unsigned long Tnew = micros(); // Store current time (or use micros() for better accuracy)
float dT = (Tnew - Told) / 1000.0; // Convert milliseconds to seconds for proper time difference
if (dT == 0) {
return; // Avoid division by zero
}
ErrorSum += (error * dT); // Approximate the integral by summing the total error
float dError = (error - ErrorOld) / dT; // Approximate the derivative by dividing the change in error by the time change
output = kp * error + ki * abs(ErrorSum) + kd * dError; // Compute the PID controller output
// Store output in the circular buffer with timestamp
outgoingData[writeIndex].timestamp = Tnew + latency; // Add latency to the current time
outgoingData[writeIndex].value = output;
writeIndex = (writeIndex + 1) % bufferSize;
ErrorOld = error;
Told = Tnew; // Update variables for use in next iteration of ComputePID()
}
/* Computes the Output_POS with the current cart position (Output_POS will be the new Setpoint for the ComputePID() function */
void ComputePID_POS() {
error_POS = Setpoint_POS - input_POS; // Calculate error between Setpoint and Input from Encoder
unsigned long Tnew_POS = micros(); // Store current time (oder verwende micros() für bessere Genauigkeit)
float dT_POS = (Tnew_POS - Told_POS) / 1000.0; // Convert milliseconds to seconds for proper time difference
if (dT_POS == 0) {
return; // Avoid division by zero
}
ErrorSum_POS += (error_POS * dT_POS); // Approximate the integral by summing the total error
float dError_POS = (error_POS - ErrorOld_POS) / dT_POS; // Approximate the derivative by dividing the change in error by the time change
Output_POS = kp_POS * error_POS + ki_POS * ErrorSum_POS + kd_POS * dError_POS; // Compute the PID controller output
ErrorOld_POS = error_POS;
Told_POS = Tnew_POS; // Update variables for use in next iteration of ComputePID_POS()
}
void sendDelayedData() {
unsigned long currentTime = micros();
// Check if there is data in the buffer and if its timestamp is in the past
if (readIndex != writeIndex && outgoingData[readIndex].timestamp <= currentTime) {
// Send the delayed data without changing its content
delayedOutput = outgoingData[readIndex].value;
// Move readIndex to next element
readIndex = (readIndex + 1) % bufferSize;
}
else {
delayedOutput = 0;
}
}