PID
Be sure to download and install the superSerial library here before using this code.
Yoshio's PID
#include <superSerial.h>
// Boolean values
#define TRUE 1
#define FALSE 0
// Hardware Pin Information
#define POT_PIN 3
#define M1_PWM 3
#define M1_DIR 12
// Output Bounds
#define OUT_MAX 255
#define OUT_MIN -255
// Timer Variables
unsigned long startTime, stopTime, now, lastTime, duration;
// PID Variables
double Setpoint, Input, Output, sampleTime, potVal, Last;
double P, I, D;
double Error, Integral;
double timeChange;
double Kp, Ki, Kd;
// Program Setup
void setup() {
// Set serial communication baud rate
Serial.begin(115200);
// Initialize the Motor pins as output and turn them off
pinMode (M1_PWM,OUTPUT); digitalWrite (M1_PWM,LOW);
pinMode (M1_DIR,OUTPUT); digitalWrite (M1_DIR,LOW);
analogWrite (M1_PWM, 0);
// Initialize and set default time length for the code
duration = 2000; // How long to PID for in milli-seconds
}
// Main portion of the code
void loop() {
// Ask for the Set Point value
Serial.println();
Serial.println("What would you like the Set Point to be?");
Setpoint = sSerial.readFloat();
Serial.print("Setpoint: ");
Serial.println(Setpoint,0);
Serial.println();
// Ask for the PID gain values
Serial.print("Kp? ");
Kp = sSerial.readFloat();
Serial.println(Kp,3);
Serial.print("Ki? ");
Ki = sSerial.readFloat();
Serial.println(Ki,3);
Serial.print("Kd? ");
Kd = sSerial.readFloat();
Serial.println(Kd,3);
Serial.println();
// Report back the PID value
Serial.print("Kp: ");
Serial.print(Kp,3);
Serial.print(" Ki: ");
Serial.print(Ki,3);
Serial.print(" Kd: ");
Serial.println(Kd,3);
Serial.println();
// Start of the PID code
startTime = millis();
lastTime = micros();
stopTime = startTime;
timeChange = 0;
Integral = 0;
Last = analogRead (POT_PIN);
// Runs the PID code for a set duration
while ( (stopTime-startTime) < duration) {
Input = analogRead (POT_PIN);
Output = ComputePID();
/*
// Since output can be between -255 to 255, set the motor direction.
// You may need to change LOW <-> HIGH depending on your pot/motor setup.
// Do so if you see the turntable running the wrong direction.
*/
if (Output > 0) {
digitalWrite(M1_DIR,HIGH);
}
else {
digitalWrite(M1_DIR,LOW);
}
analogWrite(M1_PWM,abs(Output));
Serial.print("Pot Value: ");
Serial.print(Input,3);
Serial.print(" Drive: ");
Serial.print(Output,3);
Serial.print(" dt: ");
Serial.println((timeChange)/1000,3);
stopTime = millis();
}
analogWrite(M1_PWM,0);
}
// Meat and Bone of the PID alogorithm computation - Look at Lecture notes
double ComputePID() {
now = micros();
timeChange = now-lastTime;
Error = Setpoint-Input;
Integral = Integral + Error;
P = Error * Kp;
I = Integral * Ki * (timeChange/1000000);
D = (Error-Last) * Kd / (timeChange/1000000);
Output = P + I + D;
if (Output > OUT_MAX) Output = OUT_MAX;
else if (Output < OUT_MIN) Output = OUT_MIN;
lastTime = now;
Last = Error;
return Output;
}
Here's another PID coding example. You will need the superSerial library here also.
Steve's PID
/************* PID program SWR 11 Jan 2012 **************
NOTES:
* uses an error count (Near) to check if setpoint has been met.
* program tries to meet the setpoint for 500 (nloops) control loops. Stops if not met
* Each run of the program asks for a setpoint and a proportional gain coefficient.
*/
#include <superSerial.h> // include Adams/Tsuruta serial input
long int Actual,Error,Last,Integral,n;
long int P,I,D,kP=500,kI=5,kD=0,Drive; // control parameters
int ScaleFactor=1000,SetPt=500,IntThresh=50,Near=0;
int Position = A3; // position pot analog pin
int Motor = 3; // PWM pin
int Direction = 12; // Direction pin
int Elapsed;
const int nloops = 500, nMax = 30,SmErr=3; // PID stops after n loops
// or achieving nMax SetPts
void setup() {
Serial.begin(115200);
Serial.println("n, Set, Pos, Err, P, I, D, Drive, Elapsed");
}
void loop() {
n++; // increment loop counter
Elapsed=micros(); // get starting microsecond count
PID(); // do one PID loop
Elapsed = micros() - Elapsed; // get elapsed microseconds
if (abs(Error)<SmErr) {Near++;} // accum # loops near the SetPt
if (Near>nMax) { // near setpt for > nMax loops?
PrintData();
Near=0; // zero the small error count
goto done;} // done with the current PID run
PrintData(); // show the data, takes ~ 4500 uS
if (n>nloops) { // stop if SetPt not met
done:
analogWrite (Motor,0); // stop the motor
n=0; // zero the loop count
Serial.print("Enter new set point: "); // send a message
SetPt=NewVal(); // get new set point, continue pgm
Serial.print("Enter new P gain: "); // send a message
kP=NewVal();
delay(1000);
}
}
//========================= Functions =========================================
int NewVal() { // gets a new value
int val;
val=sSerial.readInt(); // get the new set point
Serial.println(val);
return val;
}
void PrintData() {
Serial.print(n); Serial.print(", "); // show some output
Serial.print(SetPt); Serial.print(", "); // copy/paste to analyse
Serial.print(Actual); Serial.print(", ");
Serial.print(Error); Serial.print(", ");
Serial.print(P); Serial.print(", ");
Serial.print(I); Serial.print(", ");
Serial.print(D); Serial.print(", ");
Serial.print(Drive); Serial.print(", ");
Serial.print(Elapsed);
Serial.println();
}
void PID() { // does one PID loop
Actual = analogRead(Position); // read current wheel position
Error = SetPt - Actual; // calc the error
if (abs(Error) < IntThresh){ // prevent integral 'windup'
Integral = Integral + Error;} // accumulate the error integral
else {Integral=0;} // zero it if out of bounds
P=Error*kP;
I=Integral*kI; // calc the terms
D=(Last-Actual)*kD;
Drive=P+I+D;
if (Drive < 0){ // check which direction to go.
digitalWrite (Direction,LOW);} // change direction as needed
else { // depends on sign of Drive
digitalWrite (Direction,HIGH);}
Drive = Drive/ScaleFactor; // scale Drive to 0-255
Drive = abs(Drive); // make Drive pos
if (Drive > 255) {Drive = 255;} // check for out of bounds
analogWrite (Motor,Drive); // send PWM command to motor board
Last = Actual; // save current value for next time
}