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 } |
Embedded Programming > Examples >