PID speed controller

Other Hardware Motors, Mechanics, Power and CNC
1 
void encoderISR();
int pidControl(int setPoint, float actual);

// Pin definitions
const int motorPin1 = 7;   
const int motorPin2 = 8;  
const int enablePin = 9;  
const int encoderPinA = 2; 
const int encoderPinB = 3; 
int output = 0;
int min = 0;
int max = 1500;

volatile unsigned long lastTime = 0;
volatile unsigned long pulseDuration = 0;
const int PPR = 240; // Pulses Per Revolution (Adjust based on your encoder)
volatile bool newPulse = false;
float speed1Prev = 0;
float speed1 = 0;

// PID constants
int antiWind = 1;
int kwd = 1;
float Kp = 0.85;
float Ki = 0.5;
float Kd = 0.5;
float integral = 0;
float previousError = 0;
long prevT = 0;

//const long interval = 100; // Interval in milliseconds for PID calculations
float integralMin = -1000;
float integralMax = 1000;

// Desired and current speed
int desiredSpeed = 120; // Desired speed in RPM
float currentSpeed = 0;

void setup() {
  // Set motor pins as outputs
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(enablePin, OUTPUT);
 
  // Set encoder pins as inputs
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);

  // Attach interrupt to encoder pin A
  attachInterrupt(digitalPinToInterrupt(encoderPinA), encoderISR, FALLING);

  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);
  analogWrite(enablePin, 0);

  // Start Serial Monitor
  Serial.begin(9600);
}

void loop() {
  noInterrupts();
  float speed = speed1;
  interrupts();

  long currT = micros();
  float delta = ((float)(currT - prevT)) / 1.0e6;
  prevT = currT;

  currentSpeed = 0.854 * currentSpeed + 0.0728 * speed + 0.0728 * speed1Prev;
  speed1Prev = speed;

  Serial.print("Currentspeed:"); // serial plotter depends on this, don't give space and extra terms, units like rpm
  Serial.print(currentSpeed);
  Serial.println("");

  Serial.print("desiredspeed:");
  Serial.print(desiredSpeed);
  Serial.println("");

  Serial.print("int:");
  Serial.print(output);
  Serial.println("");

  // PID control
  int setPoint = desiredSpeed;
  float actual = currentSpeed;
  float error = setPoint - actual;

  integral = integral + (error * delta) * kwd;
  integral = constrain(integral, integralMin, integralMax);

  float derivative = (error - previousError) / delta;
  float u = Kp * error + Ki * integral + Kd * derivative;
  previousError = error;

   output = (int)fabs(u);
  int dir = 1;
  if (u < 0) {
    dir = -1;
  }

  int pwmValue = output;
  if (pwmValue > max && pwmValue * error > 0) {
    kwd = 0;
  } else {
    kwd = 1;
  }

  if (pwmValue > 255) {
    pwmValue = 255;
  }

  setMotor(dir, pwmValue, enablePin, motorPin1, motorPin2);

  // Delay to match the interval
  delay(100);
}

void setMotor(int dir, int pwmValue, int enablePin, int in1, int in2) {
  analogWrite(enablePin, pwmValue);  // Motor speed
  if (dir == 1) {
    // Turn one way
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
  } else if (dir == -1) {
    // Turn the other way
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
  } else {
    // Or don't turn
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
  }
}

// Encoder ISR
void encoderISR() {
  int b = digitalRead(encoderPinB);
  int increment = 0;
  if (b > 0) {
    increment = 1;
  } else {
    increment = -1;
  }

  unsigned long currentTime = micros();
  pulseDuration = currentTime - lastTime; // this can't give neg I think
  lastTime = currentTime;

  speed1 = (60.0 * 1000000.0) / (pulseDuration * (float)PPR);
}

will this code be able to control the speed of the motor properly?

2

What is the outcome running it?

3

currentSpeed was 280 something oscillating.

4

Tune your PID parameters.

2 Likes
5

There are for sure tutorials telling how to adjust the PID factors. Google is a good friend.

1 Like
6

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