#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;
}