Well, I'm trying to set up my first PID controller to get my robot going at a constant rate, but seem to run into a hitch. While the PID previously worked (although untuned) it started sending back an odd value. The output is now equal to the set point multipled by Kp. So, when Kp is >1 the motors don't run as they think they're going faster than setpoint. But, when Kp is < 1 (including 0, which gives PID = 0), the motors go full speed as the program thinks they're going slower. There's also an unknown multiplication factor with Ki and I didn't test what happens if a put a value besides 0 at Kd.
The input is encoder counts per 0.5 seconds (L/Rrot), which is working fine, it's just when I try to calculate the PID (PIDL/Rrot), I get set point (30) X Kp. Here is the code for the PID setup
// Original Encoder test taken from https://brainy-bits.com/tutorials/speed-sensor-with-arduino/
// see this webpage for instruction on how to run test
#include <PID_v1.h>
#include <TimerOne.h> // activate timer library
#include <AFMotor.h> // activate motor sheild library
AF_DCMotor Rmotor(3, MOTOR12_64KHZ); // defines motor 3 as Rmotor, 64KHz
AF_DCMotor Lmotor(4, MOTOR12_64KHZ); // defines motor 4 as Lmotor, 64KHz
volatile int Lcount;
volatile int Rcount;
//long Lrot;
//long Rrot;
int Lspeed, PIDLspeed;
int Rspeed, PIDRspeed;
//int Toprot = 60;
double Kp = 0, Ki = 0, Kd = 0;
double Lrot, PIDLrot, Rrot, PIDRrot, Setpoint = 30;
PID leftPID(&Lrot, &PIDLrot, &Setpoint, Kp,Ki,Kd, DIRECT);
PID rightPID(&Rrot, &PIDRrot, &Setpoint, Kp,Ki,Kd, DIRECT);
void countL() // counts from the Left speed sensor
{
Lcount++; // increase Lcount by one
}
void countR() // counts from the Right speed sensor
{
Rcount++; // increase Rcount by oune
}
void timerIsr() // counts how many slots went by the timer in 0.5 seconds
{
Timer1.detachInterrupt(); //reset the timer
// Serial.print("Motor Speed: ");
Lrot = (Lcount); // divide by number of holes in Disc to get RPS
Rrot = (Rcount); // or just use counts per 0.25sec
//Serial.print(rotation,DEC);
//Serial.println(" Rotation per seconds");
//Serial.println(potvalue);
Rcount = 0; // reset Rcount to zero
Lcount = 0; // reset Lcount to zero
Timer1.attachInterrupt( timerIsr ); //enable the timer
}
void setup()
{
Serial.begin(9600);
Timer1.initialize(500000); // set timer for 0.5 sec (500000 us)
attachInterrupt(0, countL, RISING); // use countL int when pin 2 goes high
// when speed sensor pin goes High
attachInterrupt(1, countR, RISING); // use countR int when pin 3 goes high
Timer1.attachInterrupt( timerIsr ); // enable the timer
rightPID.SetMode(AUTOMATIC);
leftPID.SetMode(AUTOMATIC);
}
void loop()
{
leftPID.Compute(); //Computes value of PIDLrot
rightPID.Compute(); //Computes value of PIDRrot
// This checks the PID corrected wheel speed. If either wheel shows less or
// more than 60 counts per sec, L/Rspeed is
// increased or decreased by 1, up to a maximum of 255 and minimum of zero)
if (PIDRrot < Setpoint) {
Rspeed ++;}
if (PIDLrot < Setpoint) {
Lspeed ++;}
if (PIDRrot > Setpoint){
Rspeed --;}
if (PIDLrot > Setpoint){
Lspeed --;}
if (Rspeed > 255) {
Rspeed = 255;}
if (Lspeed > 255) {
Lspeed = 255;}
if (Rspeed < 0){
Rspeed = 0;}
if (Lspeed < 0){
Lspeed = 0;}
if (PIDLrot < PIDRrot){
Lspeed --;}
if (PIDLrot < PIDRrot){
Lspeed ++;}
Rmotor.setSpeed(Rspeed); // set Rmotor speed to Rspeed value
Lmotor.setSpeed(Lspeed); // set Lmotor speed to Lspeed value
Rmotor.run(FORWARD);
Lmotor.run(FORWARD); // run both motors forward
//Serial.print("Lrot-LSPeed: ");
Serial.print(PIDLrot);
Serial.print(" - ");
Serial.println(Lrot);
//Serial.print(" , ");
Serial.println(Lspeed);
//Serial.print(" Rrot-RSPeed: ");
//Serial.print(PIDRrot);
//Serial.print(" - ");
//Serial.print(Rrot);
//Serial.print(" - ");
//Serial.println(Rspeed);
}
This is incredibly obscure and convoluted code. If I may be harsh, I suggest to simply toss it out and start over.
Start with the simplest example and get it to work with just one wheel before moving on to both.
The PID computes an output from the inputs that, when applied to the motor, should eventually make the measurement equal to the setpoint. Why are you comparing the setpoint and changing the speed elsewhere in the program? This is just working against the PID.
There is no need to repeatedly detach and reattach the interrupt.
The overall control loop looks something like this in pseudocode:
Actually, I don't mind at all. It was a mishmash of code taken from a few different sources, some of which I didn't completely understand. So, I re-wrote it from scratch and replaced Timer1 with MSTimer2, which is far simpler to use. I think it's much better now. Also, thanks for the explanation of PID. There really aren't a lot of good explanations out there for non-engineers. I incorrectly assumed I was using PID to correct my original algorithm to stay on speed. If I'm correct, then in order to control a wheel to stay at 6 clicks/0.1 seconds using a throttle that goes from 0-255, I'm going to need a high Kp. I also increased my sampling rate.
Here's my code that can now (thanks to you) control the right wheel, albeit poorly due to being badly tuned. But, one step at the time.
#include <PID_v1.h> // activates PID_v1 library
#include <MsTimer2.h> // activates MSTimer2 library
#include <AFMotor.h> // activate AdaFruit motor sheild library
AF_DCMotor Rmotor(3, MOTOR12_64KHZ); // defines motor 3 as Rmotor, 64KHz
double Kp = 20, Ki = 25, Kd = 0;
double R_Speed, PID_R_Speed, Setpoint = 6; //setpoint = encoder clicks per time period
volatile int R_Count; // where encoder "clicks" will be stored
PID RightPID(&R_Speed, &PID_R_Speed, &Setpoint, Kp,Ki,Kd, DIRECT);
void sample() // record encoder counts at regular interval, then reset to 0
{
R_Speed = R_Count; // set wheel counts to PID input variable (R_Speed)
Serial.print(R_Speed); // prints out R_Speed for debugging
Serial.print(" - ");
Serial.println(PID_R_Speed); //prints out PDIR_R_Speed for debugging
R_Count = 0; // reset count to 0
}
void R_Encoder(){
R_Count++; // increase Rcount by 1 every time right encoder (Rencoder) fires
}
void setup() {
Serial.begin(9600); // for debugging
attachInterrupt(1, R_Encoder, RISING); // use countR int when pin 3 (int1) goes high
MsTimer2::set(100, sample); // set 250 ms sampling rate
MsTimer2::start(); // activate MsTimer2
RightPID.SetMode(AUTOMATIC); // Turn
}
void loop() {
RightPID.Compute(); //Computes PID_R_Speed
Rmotor.setSpeed(PID_R_Speed); // Set Rmoter to PID corrected speed
Rmotor.run(FORWARD); // start motor running in forward direction
}
Basically you start by adjusting Kp, with Kd and Ki equal to zero. Increase Kp until the speed starts to oscillate about the setpoint, then make adjustments to the other two.