I am working on a project that requires a motor be kept at a set RPM. To do this, I am using an RS775 DC motor and an encoder to measure its RPM.
Reading RPM works no problem, it essentially counts the time between encoder ticks and converts time to tick to revolutions per minute.
Controlling the motor alone works fine, I am using a Sabertooth 2x25 to control the motor itself, and I can control the speed from 0-127. Put in the final program, the PID loop should calculate the
So finally, we have the code that puts the above two together and adds the PID loop. When I run this, I still get motor rotation and an RPM value of about 2200 RPM, regardless of what is set. (this is verified by the serial log and tach) I also had to map the RPM values to the motor inputs, so the maximum RPM of the motor as measured when running at full speed was 6291. So I mapped 0-6291 to 0-127. Then the PID loop should output a value, which is then set as the motor speed to maintain the setPoint RPM, but it isn't quite working as planned. How can I better maintain the RPM value I am reading using a PID loop?
//~~~~~~~~~~~~~~~~~PID VARIABLES~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//helpful PID loop guide: https://www.youtube.com/watch?reload=9&v=crw0Hcc67RY
#include <PID_v1.h> //PID library by Brett Beauregard
#include <SabertoothSimplified.h>
SabertoothSimplified ST; //names the SaberTooth object ST
double setPoint; //desired value
double input; //sensor (RPM in my case)
double output; //action to be taken (Altering the PWM frequency in my case)
double kP = .5, kI = 0, kD = 0; //PID tuners
//~~~~~~~~~~~~~~~~ENCODER READINGS~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//Source for positional readings: https://github.com/BenTommyE/BenRotaryEncoder/blob/master/BenRotaryEncoder.ino
volatile float temp, counter = 0; //This variable will increase or decrease depending on the rotation of encoder
int PPR = 400; //Equal to the number of ticks per revolution of your encoder
int pulseDifferential = 24; //number of ticks per mSec for desired RPM
float pulsemSec = 0; //the amount of ticks the encoder is reading every millisecond
float RPM = 0; //RPM of the motor
float timeToRev = 0; //time in uSec per revolution of encoder
float timeSample1 = 1;
float timeSample2 = 0;
PID rpmPID(&input, &output, &setPoint, kP, kI, kD, DIRECT);
//~~~~~~~~~~~~~~~~PWM MOTOR VARIABLES~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void setup() {
Serial.begin(9600);
SabertoothTXPinSerial.begin(9600); // This is the baud rate you chose with the DIP switches.
ST.motor(2,50); // Sets motor to stop (or a certain speed) at the beginning of program
setPoint = 500; //THIS VALUE is where you need to set your RPM
rpmPID.SetMode(AUTOMATIC); //Turns on PID loop
rpmPID.SetTunings(kP, kI, kD); //Defines which variables above are tuning variables
pinMode(2, INPUT_PULLUP); //sets pin mode for pin 2
pinMode(3, INPUT_PULLUP); //sets pin mode for pin 3
//Setting up interrupt
//A rising pulse from encoder activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on most Arduinos.
attachInterrupt(0, ai0, RISING);
//B rising pulse from encoder activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on most Arduinos.
attachInterrupt(1, ai1, RISING);
//analogWrite(motorPin, 30);
RPM = 1000;
delay(5000);
RPM = 1000;
}
void loop() {
// Send the value of counter
if ( counter != temp ) { //if change is detected in the encoder, print the positional data values
//Serial.println ("Positional Data: ");
//Serial.println (counter);
temp = counter;
}
if ( counter >= PPR or counter <= -PPR) { //This if statement resets the counter every time the encoder does a full revolution, protecting from reset after memory becomes filled
counter = 0;
timeSample2 = micros();
timeToRev = timeSample2 - timeSample1;
RPM = (1/timeToRev) * 1000 * 1000 * 60; //conversion for uSec/rev to RPMs.
Serial.println ("timeToRev: ");
Serial.println (timeToRev);
Serial.println ("RPM: ");
Serial.println (RPM);
Serial.println ("input: ");
Serial.println (input);
Serial.println ("output: ");
Serial.println (output);
timeSample1 = timeSample2;
}
input = map(RPM, 0, 6191, 0, 127); //changes scale of 0-max RPM (encoder only accurate to 5000 RPM) to a PWM scale of 0-127. This requires tuning and reading max rpm at 127 output
rpmPID.Compute(); //self explanatory
ST.motor(2, output); //Tells the motor to spin at the speed calcuated by PID loop based on encoder readings and RPM
//analogWrite(motorPin, output);
}
void ai0() {
// ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
// Check pin 3 to determine the direction
if (digitalRead(3) == LOW) {
counter++;
} else {
counter--;
}
}
void ai1() {
// ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
// Check with pin 2 to determine the direction
if (digitalRead(2) == LOW) {
counter--;
} else {
counter++;
}
}
