Workshop Code

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

}