Speed control using PID controller

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

// Pin definitions
const int motorPin1 = 7;   // Motor driver input pin 1
const int motorPin2 = 8;   // Motor driver input pin 2
const int enablePin = 9;   // Motor driver enable pin (PWM control)
const int encoderPinA = 2; // Encoder signal A pin
const int encoderPinB = 3; // Encoder signal B pin

// Constants and Variables
const int numReadings = 5;  // Number of readings for averaging
unsigned long readings[numReadings]; // Array to store pulse durations
int readIndex = 0;
unsigned long total = 0;
int average = 0;
volatile unsigned long lastTime = 0;
volatile unsigned long pulseDuration = 0;
const int PPR = 240; // Pulses Per Revolution (Adjust based on your encoder)

// PID constants
float Kp = 1.0;
float Ki = 0.5;
float Kd = 0.1;
float integral = 0;
float previousError = 0;
const long interval = 100; // Interval in milliseconds for PID calculations

// Desired and current speed
int desiredSpeed = 100; // 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);

  // Start Serial Monitor
  Serial.begin(9600);
  
  // Initialize lastTime
  lastTime = micros();
}

void loop() {
  noInterrupts();
  // Update readings
  total = total - readings[readIndex];
  readings[readIndex] = pulseDuration;
  total = total + readings[readIndex];
  readIndex = readIndex + 1;
  if (readIndex >= numReadings) {
    readIndex = 0;
  }
  interrupts();
  
  // Calculate average pulse duration
  average = total / numReadings;

  // Calculate current speed (RPM)
  if (average > 0) {
    currentSpeed = (60.0 * 1000000.0) / (average * (float)PPR);
  } else {
    currentSpeed = 0; // Avoid division by zero
  }

  Serial.print("Average Pulse Duration: ");
  Serial.print(average);
  Serial.println(" microseconds");
  Serial.print("Current Speed: ");
  Serial.print(currentSpeed);
  Serial.println(" RPM");

  // PID control
  int pwmValue = pidControl(desiredSpeed, currentSpeed);
  
  // Set motor speed
  analogWrite(enablePin, pwmValue);

  // Print PID output and PWM value for debugging
  Serial.print("PID Output: ");
  Serial.print(pwmValue);
  Serial.println(" PWM");

  delay(100); // Delay to prevent overwhelming the serial monitor
}

// Encoder ISR
void encoderISR() {
  unsigned long currentTime = micros();
  pulseDuration = currentTime - lastTime;
  lastTime = currentTime;
}

// PID control function
int pidControl(int setPoint, float actual) {
  float error = setPoint - actual;
  integral += error * interval / 1000.0;
  float derivative = (error - previousError) / (interval / 1000.0);
  int output = Kp * error + Ki * integral + Kd * derivative;
  previousError = error;

  // Constrain output to PWM range
  return constrain(output, 0, 255);
}

will code work for speed control of a dc motor. I am using l293d motor driver, CH-N20-3 encoded dc motor and arduino uno

2

Much depend on the wiring, the schematics and facts told by datasheets.

3

All depends on your calibration constants:

You have to provide the best constants by trial and error on the final construction.

4

I did the wirings and tried this code , but the motor stays standstill. I test the driver and the motor using some basic code , it was fine. What might be wrong.

5

Ohh, I couldn't find proper datasheet for CH-N20-3( dc motor with encoder). Any sources?

6

Put some Serial.print(...) in your code to moitor what the PID puts out. On the datasheet, id you ask google for it you get as the first link https://www.pololu.com/file/0J1487/pololu-micro-metal-gearmotors_rev-5-1.pdf

7

Seller, manufactorer.

8 
digitalWrite(motorPin1,HIGH);
digitalWrite(motorPin2,LOW);
analogWrite(enablePin,255);

It started working after I set the motor at full speed in the set up. Why it didn't work at first , it should work anyway. By the way it is slow in adjusting the speed. Any advice. THANKS FOR YOUR HELP.

9 
#include <RunningAverage.h> 

void encoderISR();
int pidControl(int setPoint, float actual);

const int motorPin1 = 7;   
const int motorPin2 = 8;  
const int enablePin = 9;  
const int encoderPinA = 2; 
const int encoderPinB = 3; 

const int numReadings = 5;  
RunningAverage myRA(numReadings);
int average = 0;
volatile unsigned long lastTime = 0;
volatile unsigned long pulseDuration = 0;
const int PPR = 240; 
volatile bool newPulse = false;

float Kp = 0.78;
float Ki = 0.58;
float Kd = 1.35;
float integral = 0;
float previousError = 0;
const long interval = 100; 
float integralMin = 0;
float integralMax = 300;

int desiredSpeed = 150; 
float currentSpeed = 0;

void setup() {
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(enablePin, OUTPUT);
 
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  myRA.clear();
  attachInterrupt(digitalPinToInterrupt(encoderPinA), encoderISR, FALLING);
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);
  analogWrite(enablePin, 128);
  Serial.begin(9600);
  
  lastTime = micros();
}

void loop() {
  noInterrupts();
  myRA.addValue(pulseDuration);
  average = myRA.getAverage();
  interrupts();

  if (average > 0) {
    currentSpeed = (60.0 * 1000000.0) / (average * (float)PPR); 
  } else {
    currentSpeed = 0; 
  }

  Serial.print("Currentspeed:");
  Serial.print(currentSpeed);
  Serial.println("");

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

  int pwmValue = pidControl(desiredSpeed, currentSpeed);
  
  analogWrite(enablePin, pwmValue);

  delay(100); 
}

void encoderISR() {
  unsigned long currentTime = micros();
  pulseDuration = currentTime - lastTime;
  lastTime = currentTime;
}

int pidControl(int setPoint, float actual) {
  float error = setPoint - actual;
  integral = integral + error * interval / 1000.0;
  integral = constrain(integral, integralMin, integralMax);
  float derivative = (error - previousError) / (interval / 1000.0);
  int output = Kp * error + Ki * integral + Kd * derivative;
  previousError = error;

  return constrain(output, 0, 255);
}

will this work now?

10

Try it ...

11

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