#include <superSerial.h> // Boolean values #define TRUE 1 #define FALSE 0 // Hardware Pin Information #define POT_PIN 3 #define PHOTO_PIN 1 #define M1_PWM 3 #define M1_DIR 12 #define BUTTON 5 #define SYNC 2 #define LISTEN 4 // 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; int choice; int threshold = 250; // 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); pinMode (SYNC,OUTPUT); pinMode (LISTEN,INPUT); analogWrite (M1_PWM, 0); // Initialize and set default time length for the code duration = 5000; // How long to PID for in milli-seconds } void loop(){ menu(); choice = sSerial.readInt(); switch(choice){ case 1: digitalWrite(SYNC, HIGH); PID(); while(digitalRead(LISTEN)!=HIGH){} break; case 2: Serial.println(readPot()); break; case 3: while(analogRead(BUTTON)!=0){ Serial.println(readPot()); } break; case 4: Serial.println(readPhoto()); break; case 5: while(analogRead(BUTTON)!=0){ Serial.println(readPhoto()); } break; case 6: digitalWrite(M1_DIR,HIGH); analogWrite(M1_PWM,150); while(readPhoto()<=threshold){} Serial.println(readPot()); Setpoint = readPot(); //setpoint is global PID(); break; default: break; } } void menu(){ Serial.println("What would you like to do?"); Serial.println(" "); Serial.println("1. Run PID"); Serial.println("2. Read Pot (Instantaneous)"); Serial.println("3. Read Pot (Continuous)"); Serial.println("4. Read Photo (Instantaneous)"); Serial.println("5. Read Photo (Continuous)"); Serial.println("6. Run until readPhoto reaches threshold then PID to pot position"); //skeleton must have set PID values and remove questions at beginning } int readPot(){ return analogRead(POT_PIN); } int readPhoto(){ return analogRead(PHOTO_PIN); } // Main portion of the code void PID() { // 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; } |
Embedded Programming > Examples >