#include <Servo.h> 
//Set up pins 

int motorSpeedPin_1 = 3;    // pin for motor 1  demand 
int motorDirPin_1 = 12;     // pin for motor 1 direction 
int motorBrakePin_1 = 9;    // pin for motor 1 brake 
int motorSpeedPin_2 = 11;   // pin for motor 2  demand 
int motorDirPin_2 = 13;     // pin for motor 2 direction 
int motorBrakePin_2 = 8;    // pin for motor 2 brake 
int encPhaseA_1 = 18;       // pin for encoder 1 phase A 
int encPhaseB_1 = 19;       // pin for encoder 1 phase B 
int encPhaseA_2 = 20;       // pin for encoder 2 phase A 
int encPhaseB_2 = 21;       // pin for encoder 2 phase B 
int blackButtonPin = A9;    // pin for black button 
int greenButtonPin = A10;   // pin for green button 
int potPin = A8;            // pin for rotary pot 
int linMotorPin = 6;          // pin for linear motor 

Servo linMotor; 

//Set up variables 

volatile int encCount_1 = 0;            // Counts made for encoder 1 
volatile int encCount_2 = 0;            // Counts made for encoder 2
float encCountPerSec_1 = 0;             // Encoder 1 angular velocity in counts/s 
float encCountPerSec_2 = 0;             // Encoder 2 angular velocity in counts/s 
float motorDemand_1 = 0;                // Motor 1 demand, -100% to +100% 
float motorDemand_2 = 0;                // Motor 2 demand, -100% to +100% 
int motorCommand_1 = 0;                 // Motor 1 signal, 0 to 255 
int motorCommand_2 = 0;                 // Motor 2 signal, 0 to 255 
volatile int linMotorPos = 0;           // Linear motor pulse length 
int potValue;                           // Potentiometer value. 
float maxPot = 270;                     // Used to calculate potentimeter value. 
float potBias;                          // Potentiometer bias -1 to +1 
unsigned long timePassed = 0;           // Time in msec since buttom press 
unsigned long timeStarted;              // Time is msec when button pressed. 
int buttonPressed = 0;                  // Which button Pressed, 0 = neither, 1 = black, 2 = green 


/// THE NEXT VARIABLES ARE DEFINED BY ME :) 

float enc_dist1 = 0; //calculating the distance in terms of revolutions 
float enc_dist2 = 0; // same thing as above for the second encoder 
boolean payload; 
float error_1 = 0; 
float error_2 =0; 
int setpoint = 0;
float Kp = 1; 
float Ki = 0; 
float error_prior; 
float integral_prior; 
unsigned long prev_time; 
unsigned long deltaT; 
float output; 
float integral; 

 
 

void setup() { 

  // Setup the pins and variables for the programme and define the interupts for the encoder. 
  // Setup Motor 1 
  pinMode(motorDirPin_1, OUTPUT);      // Initiates Motor Channel A pin 
  pinMode(motorBrakePin_1, OUTPUT);    // Initiates Brake Channel A pin 

  // Setup Motor 2 
  pinMode(motorDirPin_2 , OUTPUT);     // Initiates Motor Channel A pin 
  pinMode(motorBrakePin_2, OUTPUT);    // Initiates Brake Channel A pin 

  // Setup Encoder 1 
  pinMode(encPhaseA_1, INPUT);         // Initiates Encoder 1 Phase A 
  pinMode(encPhaseB_1, INPUT);         // Initiates Encoder 1 Phase B 

  // Setup Encoder 2 
  pinMode(encPhaseA_2, INPUT);         // Initiates Encoder 2 Phase A 
  pinMode(encPhaseB_2, INPUT);         // Initiates Encoder 2 Phase B 

   // Setup Linear Motor Control 
  linMotor.attach(6); 
  linMotor.writeMicroseconds(1000);    // Change to 2000 to initialise fully extended 
  delay(3000);                         // Initiates the linear motor control pin 

  // Setup Button box 

  pinMode(blackButtonPin, INPUT);      // Initiates Black Button 
  pinMode(greenButtonPin, INPUT);      // Initiates Green Button 
  pinMode(potPin, INPUT);              // Initiates Potentiometer 

 
 

  attachInterrupt(digitalPinToInterrupt(encPhaseA_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase A 
  attachInterrupt(digitalPinToInterrupt(encPhaseB_1),encInc_1,CHANGE); // Create interrupt for change in encoder 1 phase B 
  attachInterrupt(digitalPinToInterrupt(encPhaseA_2),encInc_2,CHANGE); // Create interrupt for change in encoder 2 phase A 
  attachInterrupt(digitalPinToInterrupt(encPhaseB_2),encInc_2,CHANGE); // Create interrupt for change in encoder 2 phase B 
  Serial.begin(9600);                 //  setup serial 

} 

 
 

// Main loop 

void loop(){                                  
 digitalWrite(motorBrakePin_1, HIGH);          //Engage the Brake for Channel A 
 digitalWrite(motorBrakePin_2, HIGH);          //Engage the Brake for Channel B 

 // Initial buttonPressed variable. 

 buttonPressed = 0;                         //Set button pressed to 0 
 payload = 0; 

 // Waits in this section until button pressed. 

  while (buttonPressed == 0) {                 //Stay in while loop until a button is pressed. 
    if (analogRead(blackButtonPin) < 2) {      //Check for the black button to be pressed. 
      buttonPressed = 1;                       //Set button pressed to 1 
    } 

    else if (analogRead(greenButtonPin) < 2) { //Check for the green button to be pressed. 
      buttonPressed = 2;                       //Set button pressed to 2 
    } 

    else {                                     //Else no button pressed. 
      buttonPressed = 0;                       //Set button pressed to 0 
    } 
      delay(50);                               //Wait 50ms before checking button status again. 
  } 

   

  // Code about to start running, intialise some variables ready for the running loop. 

   potValue = analogRead(potPin);              // Read potentiometer value 
   potBias = (potValue-260)/maxPot;            // Calculate potBias 
   encCount_1 = 0;                             // Initialise encoder 1 
   encCount_2 = 0;                             // Initialise encoder 2 
   timeStarted = millis();                     // Get the time in sec at which the button was pressed. 
   timePassed = millis() - timeStarted; 

 
 

 // This is the run loop, you set the condition for the while loop to keep runnin 

  while (enc_dist1 <= 11.5  && enc_dist2 <= 11.5 ) {     //Add your condition for the loop to continue running here, that is when should your controller stop? 
    while (enc_dist1 <=6.5 && enc_dist2 <=6.5) {  
      // Place your run code here: 
      timePassed = millis() - timeStarted; 
      setpoint = 6.5;
      error_1 = calc_error1(setpoint);
      error_2 = calc_error2(setpoint);
      motor_PID(error_1, error_2); 
      encVel_1();
      encVel_2();

      if ((6 <= abs(enc_dist1) && payload ==0) ||(abs(enc_dist2)>= 6 && payload ==0) ) { // if the distance calculated of either encoder is 6 and the payload is 0 ///SHOULD BE 6 + LENGTH OF BUGGY 
        analogWrite(motorSpeedPin_2, 0);
        analogWrite(motorSpeedPin_1, 0); 
        linMotor.writeMicroseconds(2000);  // Change to 1000 to retract 
        delay (1000); 
        linMotor.writeMicroseconds(1000);  // Change to 2000 to extend 
        delay (1000); 
        payload = 1;  
      } 
    } 

    while (enc_dist1<=11.5 && enc_dist2<=11.5)
    {
      timePassed = millis()- timeStarted;
      setpoint = 11.5;
      error_1 = calc_error1(setpoint);
      error_2 = calc_error2(setpoint);
      motor_PID (error_1, error_2);
      if(error_1== 0 && error_2==0)
      {
        motorDemand_1 =0;
        motorDemand_2= 0;
      }
    }
   
    // This code below is used to output the demand to the motors, it should not need to be changed. 
    motorCommand_1 = round(motorDemand_1*2.55);  //Coverts -100 to 100 to -255 to 255 
    motorCommand_2 = round(motorDemand_2*2.55);  //Coverts -100 to 100 to -255 to 255 
    motorCommand_1 = abs(motorCommand_1);         //Makes motorCommand_1 a positive value. 
    motorCommand_2 = abs(motorCommand_2);         //Makes motorCommand_2 a positive value. 
    if (motorDemand_1 > 0) {                      //Sets motor 1 direction. 
    digitalWrite(motorDirPin_1, HIGH);            //Establishes forward direction of Channel A 
    } 
    else { 
      digitalWrite(motorDirPin_1, LOW);           //Establishes reverse direction of Channel A 
    } 
    digitalWrite(motorBrakePin_1, LOW);           //Disengage the Brake for Channel A 
    analogWrite(motorSpeedPin_1, motorCommand_1); //Spins the motor on Channel A at motor demand 
    if (motorDemand_2 > 0) {                      //Sets motor 2 direction. 
    digitalWrite(motorDirPin_2, HIGH);            //Establishes forward direction of Channel B 
    } 
    else { 
      digitalWrite(motorDirPin_2, LOW);           //Establishes reverse direction of Channel B 
    } 
    digitalWrite(motorBrakePin_2, LOW);           //Disengage the Brake for Channel B 
    analogWrite(motorSpeedPin_2, motorCommand_2); //Spins the motor on Channel B at motor demand 
    delay(2); 
    // For debugging, remove once code is complete 
    Serial.print("Time passed: "); 
    Serial.print(timePassed); 
    Serial.print(" Encoder 1: "); 
    Serial.print(encCount_1); 
    Serial.print(" Encoder 2: "); 
    Serial.print(encCount_2); 
    Serial.print(" Encoder count per sec 1: "); 
    Serial.print(analogRead(blackButtonPin)); 
    Serial.print(" Encoder count per sec 2: "); 
    Serial.print(analogRead(greenButtonPin)); 
    Serial.print(" MotorDemand_1: "); 
    Serial.print(motorDemand_1); 
    Serial.print(" MotorDemand_2: "); 
    Serial.print(motorDemand_2); 
    Serial.println();     
    prev_time = timePassed; 
  } 

} 

// Function for calculating encoder velocity 1 
void encVel_1() { 
   int measurementWidth = 4;             // Number of counts that need to pass before determining velocity 
   static int timeElapsed = 0;           //  Time elasped since last velocity  given 
   static int refTime;                   // Current reference time in ms 
   static float encChange =0;            // Change in encoder count since last velocity measure 
   static int prevEncCount = 0;            // Encoder value at last velocity measure 
   static int encChangeAbs = 0;          // Absolute change in encoder count 
   static int storedTime = millis();     // Time at previous velocity measure in ms 
   refTime = millis();                   // Update reference time in ms 
   timeElapsed = refTime-storedTime;     // Calcualte the time that has elapsed since last velocity measure 
   encChange = encCount_1-prevEncCount;    // Calculate the change in counts since previous velocity measure 
   if (timeElapsed >= 400) {             // If time is over 400ms since last velocity measure, assume zero velocity 
    encCountPerSec_1 = 0; 
    prevEncCount = encCount_1;             // set previous encoder Count to the current encoder count and storedTime to current time in ms 
    storedTime = refTime; 
   } 

    else {                                               // If the time elapsed is short... 
      encChangeAbs = abs(encChange);                     // Get absolute encoder change. 
    if (encChangeAbs >= measurementWidth) {              // If absolute encoder change is greater or equal to measurement width... 
      encCountPerSec_1 = 1000*(encChange/timeElapsed);   // Calculate counts per second to give velocity measure  
      storedTime = refTime;                              // set previous encoder Count to the current encoder count and storedTime to current time in ms 
      prevEncCount = encCount_1; 
   } 
   } 
} 


// Function for calculating encoder velocity 2 

void encVel_2() { 
   int measurementWidth = 4;             // Number of counts that need to pass before determining velocity 
   static int timeElapsed = 0;           //  Time elasped since last velocity  given 
   static int refTime;                   // Current reference time in ms 
   static float encChange =0;            // Change in encoder count since last velocity measure 
   static int prevEncCount = 0;            // Encoder value at last velocity measure 
   static int encChangeAbs = 0;          // Absolute change in encoder count 
   static int storedTime = millis();     // Time at previous velocity measure in ms 
   refTime = millis();                   // Update reference time in ms 
   timeElapsed = refTime-storedTime;     // Calcualte the time that has elapsed since last velocity measure 
   encChange = encCount_2-prevEncCount;    // Calculate the change in counts since previous velocity measure 
   if (timeElapsed >= 400) {             // If time is over 400ms since last velocity measure, assume zero velocity 
    encCountPerSec_2 = 0; 
    prevEncCount = encCount_2;             // set previous encoder Count to the current encoder count and storedTime to current time in ms 
    storedTime = refTime; 
   } 

    else {                                               // If the time elapsed is short... 
      encChangeAbs = abs(encChange);                     // Get absolute encoder change. 
    if (encChangeAbs >= measurementWidth) {              // If absolute encoder change is greater or equal to measurement width... 
      encCountPerSec_2 = 1000*(encChange/timeElapsed);   // Calculate counts per second to give velocity measure  
      storedTime = refTime;                              // set previous encoder Count to the current encoder count and storedTime to current time in ms 
      prevEncCount = encCount_2; 
   } 
   } 
} 

 
 

// ISR for counting the encoder 1 value 

void encInc_1() { 
   static bool prevPhaseA; 
   static bool prevPhaseB; 
   bool currentPhaseA; 
   bool currentPhaseB; 
   currentPhaseA = digitalRead(encPhaseA_1);                                       // Get current value of phase A and phase B 
   currentPhaseB = digitalRead(encPhaseB_1);  
   if (currentPhaseA != prevPhaseA) {                                              // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B 
   if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) { 
    encCount_1 ++; 
   } 
   else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) { 
    encCount_1 --; 
   } 
   else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) { 
    encCount_1 --; 
   } 
   else { 
    encCount_1 ++; 
   } 
   } 

   else if (currentPhaseB != prevPhaseB) {                                         // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B 
    if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) { 
    encCount_1 --; 
   } 
   else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) { 
    encCount_1 ++; 
   } 
   else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) { 
    encCount_1 ++; 
   } 
   else { 
    encCount_1 --; 
   } 
   } 
   prevPhaseA = currentPhaseA;  
   prevPhaseB = currentPhaseB;   
} 

 
 

// ISR for counting the encoder 2 value 

void encInc_2() { 
   static bool prevPhaseA; 
   static bool prevPhaseB; 
   bool currentPhaseA; 
   bool currentPhaseB; 
   currentPhaseA = digitalRead(encPhaseA_2);                                       // Get current value of phase A and phase B 
   currentPhaseB = digitalRead(encPhaseB_2); 
   if (currentPhaseA != prevPhaseA) {                                              // Check for a change in Phase A then apply count logic depending on state of Phase A and Phase B 
   if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) { 
    encCount_2 ++; 
   } 
   else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) { 
    encCount_2 --; 
   } 
   else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) { 
    encCount_2 --; 
   } 
   else { 
    encCount_2 ++; 
   } 
   } 

   else if (currentPhaseB != prevPhaseB) {                                      // Check for a change in Phase B then apply count logic depending on state of Phase A and Phase B 
    if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) { 
    encCount_2 --; 
   } 
   else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) { 
    encCount_2 ++; 
   } 

   else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) { 
    encCount_2 ++; 
   } 
   else { 
    encCount_2 --; 
   } 
   } 

   prevPhaseA = currentPhaseA;  
   prevPhaseB = currentPhaseB;   
} 

 
 

void motor_PID(float A, float B){ 

  deltaT = timePassed - prev_time; 
  integral = integral_prior * deltaT; 
  motorDemand_1 = Kp*A + Ki*integral; 
  motorDemand_2 = Kp*B + Ki*integral; //REMOVED KD FOR NOW WILL LOOK LATER 
  integral_prior = integral; 

} 

float calc_error1 (int target)
{
  float enc_dist1, error_1;
  enc_dist1 = ((2*3.142*0.088)/512) * encCount_1; 
  error_1 = target - enc_dist1;
  return error_1;
}

float calc_error2(int target)
{
  float enc_dist2, error_2;
  enc_dist2 = ((2*3.142*0.088)/512) * encCount_2; 
  error_2 = target - enc_dist2;
  return error_2;
}