I've tried to implement a PID controller using the PID library on the Arduino but without success. I'm using a simple 6 V DC motor to move a wheel, upon which I have attached a simple sha'ft encoder to measure RPM. I'm using the L293D H-bridge to control the DC motor.
My objective is to control the RPM to a desired value using PID. The code I'm using is the one below:
#include <PID_v1.h>
#define SpeedSetPoint 200
double Input, Output, Setpoint; //PID Variables
PID myPid(&Input, &Output, &Setpoint, 1, 0.0, 0.0, DIRECT);
//RPM Counter
unsigned long start;
const byte encoderPinA = 2;//A pin -> interrupt pin 0
const byte encoderPinB = 4;//B pin -> digital pin 4
volatile long pulse;
volatile bool pinB, pinA, dir;
const byte ppr = 25, upDatesPerSec = 2;
const int fin = 1000 / upDatesPerSec;
const float konstant = 60.0 * upDatesPerSec / (ppr * 2);
int rpm=0;
int outputValue=0;
//DC Motor Pins
int pwm = 5;
int motorA1 = 10;
int motorA2 = 9;
void setup(){
Serial.begin(9600);
Input = 25;
Setpoint = SpeedSetPoint;
myPid.SetMode(AUTOMATIC);
attachInterrupt(0, readEncoder, CHANGE);
pinMode(encoderPinA,INPUT_PULLUP);
pinMode(encoderPinB,INPUT_PULLUP);
pinMode(pwm, OUTPUT);
pinMode(motorA1, OUTPUT);
pinMode(motorA2, OUTPUT);
//Start DC Motor
digitalWrite(pwm, HIGH);
digitalWrite(motorA1, HIGH);
digitalWrite(motorA2, LOW);
}
void loop() {
if(millis() - start > fin)
{
start = millis();
rpm = pulse * konstant;
rpm = abs(rpm);
//Serial.println(rpm);
pulse = 0;
}//End RPM reading
Input = rpm;
myPid.Compute();
//Serial.println(Output);
outputValue = Output;
outputValue = map(outputValue, 0, 400, 180, 255);
analogWrite(pwm, outputValue);
Serial.print("Output = ");
Serial.print(outputValue);
Serial.print("\t");
Serial.print("RPM = ");
Serial.println(rpm);
}
void readEncoder()
{
pinA = bitRead(PIND,encoderPinA);
pinB = bitRead(PIND,encoderPinB);
dir = pinA ^ pinB; // if pinA & pinB are the same
dir ? --pulse : ++pulse; // dir is CW, else CCW
}
And the Serial Monitor shows the following:
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 206
Output = 180 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 172
Output = 185 RPM = 235
Output = 185 RPM = 235
Output = 185 RPM = 235
Output = 185 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 235
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 211
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 204
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Output = 180 RPM = 208
Note: I've adjusted the map function to map from 180 to 255 because, for PWM values below 180, the DC motor just does a high pitched noise and doesn't move at all.
Thing is: my output just stays like that (with just some small variations), so I'm not really controlling the RPM. I've just started using PID, so I'm really newbie with this.
Is my code correct? How should I do the tuning for the PID? I've just left Kp = 1 and all others equal to zero.