Embedded Programming‎ > ‎Examples‎ > ‎

PID with Timing and Stopping Criterion

Modified from 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, vError, Integral;
double timeChange, runTime;
double Kp, Ki, Kd;

// Setpoint achivement variables
#define P_ERR_MAX 10
#define V_ERR_MAX 2
int n, nReq = 30;

// 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 = 5000; // 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 = millis();
  stopTime = startTime;
  timeChange = 0;
  Integral = 0;
  n = 0;
  Last = analogRead (POT_PIN);
 
  // Runs the PID code for a set duration
  while ( n < nReq && (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);
  
    // Check the ending Criterion
    if ( abs(Error) < P_ERR_MAX && abs(vError) < V_ERR_MAX) { n++; }
   
    stopTime = millis();
  }
 
  runTime = (stopTime - startTime);
  runTime = runTime/1000;
 
  if ( n == nReq ) {
    Serial.print ("Setpoint has been REACHED!  Time (sec): ");
    Serial.println (runTime,3);
  }
  else {
    Serial.print ("Setpoint was NEVER reached! :(  Time (sec): ");
    Serial.println (runTime,3);
  }
 
  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;
  vError = Error-Last;
  Integral = Integral + Error;
  
  P = Error * Kp;
  I = Integral * Ki * (timeChange/1000000);
  D = vError * 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;
}

Comments