Embedded Programming‎ > ‎Examples‎ > ‎

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
      } 
Comments