#include <Servo.h>
// defining constants:
//Set up pins
const int wheel_diameter = 137;
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 black_button_pin = A9;    // pin for black button
int green_button_pin = A10;   // pin for green button
int potPin = A8;            // pin for rotary pot
int linMotorPin = 6;          // pin for linear motor
Servo linMotor;

// these are just random values:
// have halved the values for testing --------------------------
const int drop_off_distnace = 3000; //(+potentially the length of the buggy)
const int final_distance = 5500;// (+potentially length of buggy/ circumfrance of bumps)

//defining global 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
float time_passed = 0;           // Time in msec since buttom press
unsigned long timeStarted1;
unsigned long timeStarted2;                // Time is msec when button pressed.
int buttonPressed = 0;                  // Which button Pressed, 0 = neither, 1 = black, 2 = green
float distance_travelled_1 = 0;
float distance_travelled_2 = 0;
float angular_velocity_1 = 0;
float angular_velocity_2 = 0;
float translational_velocity_1 = 0;
float translational_velocity_2 = 0;

//These are variables which have been added since editting/updating the code:
bool actuator_activated = false;
unsigned long start_time;
int button_pressed = 0;
unsigned long current_time;
bool second_timer_started = false;
bool destination_reached = false;
bool code_activated = false;
float now = 0;
float dt = 0;
float previous_time = 0;
float error = 0;
float proportional = 0;
float integral = 0;
float previous_error = 0;
float pi = 0;
float set_point_wheel_1 = 90;
float set_point_wheel_2 = -90;
float ki = 0.01;
float kp = 0.075;
bool finished = false;



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(1200);    // so partially extended
  delay(3000);                         // Initiates the linear motor control pin
  // Setup Button box
  pinMode(black_button_pin, INPUT);      // Initiates Black Button
  pinMode(green_button_pin, 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

  encCount_1 = 0;                             // Initialise encoder 1
  encCount_2 = 0; 
  //ignore the start time bit
  start_time = millis();
  Serial.begin(9600);                 //  setup serial
  Serial.print("Time passed: ");
  Serial.print(",");
  Serial.print(" distance 1: ");
  Serial.print(",");
  Serial.print(" distance 2: ");
  Serial.print(",");
  Serial.print(" MotorDemand_1:  ");
  Serial.print(",");
  Serial.print(" MotorDemand_2:  ");
  Serial.print(",");
  Serial.print(" pi ");
Serial.print(",");
 Serial.println(" error ");
}

void loop() {
  time_passed = millis();
  //there is 512 counts per revolution
  
  
  // This while loop is to determine what set of code we want to run,
  // depending on what button we press (we could just have one set of code so this may not be neccessary)
//   while (button_pressed == 0) {
//     if (analogRead(black_button_pin) < 2){
//       button_pressed = 1;
//       timeStarted1 = millis();     
//     }
//     else if (analogRead(green_button_pin) < 2){
//       button_pressed = 2;
//       timeStarted1 = millis();     
//     }
//     else {
//       button_pressed = 0;
//     }
//     delay(50);
// }


//this section of the code uses the user defined function, which use the
//interrupt pins to constantly measure the specific values. This part of the loop
//calls the functions and constantly measures the needed values for the code to use
//there is 512 counts per revolution                 
distance_travelled_1 = encCount_1 * (1/512.0) * 3.141592 * wheel_diameter; 
distance_travelled_2 = -encCount_2 * (1/512.0) * 3.141592 * wheel_diameter; 


//PI control
now = millis();
now = now/1000;
dt = now - previous_time;
previous_time = now;
error = distance_travelled_1 - distance_travelled_2;
proportional = error;
integral += error*dt;
previous_error = error;
pi = (kp * proportional) + (ki * integral);



if (error > 0 && distance_travelled_1 < drop_off_distnace && distance_travelled_2 < drop_off_distnace && actuator_activated == false){
motorDemand_1 = set_point_wheel_1 - pi;
motorDemand_2 = set_point_wheel_2 + pi;
}
else if (error < 0 && distance_travelled_1 < drop_off_distnace && distance_travelled_2 < drop_off_distnace && actuator_activated == false){
motorDemand_1 = set_point_wheel_1 - pi;
motorDemand_2 = set_point_wheel_2 - pi;
}
else if (error == 0 && distance_travelled_1 < drop_off_distnace && distance_travelled_2 < drop_off_distnace && actuator_activated == false){
motorDemand_1 = set_point_wheel_1;
motorDemand_2 = set_point_wheel_2; 
}



if (distance_travelled_1 >= drop_off_distnace && distance_travelled_2 >= drop_off_distnace && actuator_activated == false){
  digitalWrite(motorBrakePin_1, HIGH);
digitalWrite(motorBrakePin_2, HIGH);
motorDemand_1 = 0;
motorDemand_2 = 0;
  linMotor.writeMicroseconds(1000);  // Change to 1000 to retract
  delay (1000);
  actuator_activated = true;
  start_time = millis();
  digitalWrite(motorBrakePin_1, LOW);
digitalWrite(motorBrakePin_2, LOW);
}



if (error > 0 && destination_reached == false && actuator_activated == true){
motorDemand_1 = set_point_wheel_1 - pi;
motorDemand_2 = set_point_wheel_2 + pi;
}
else if (error < 0 && destination_reached == false && actuator_activated == true){
motorDemand_1 = set_point_wheel_1 - pi;
motorDemand_2 = set_point_wheel_2 - pi;
}
else if (error == 0 && destination_reached == false && actuator_activated == true){
motorDemand_1 = set_point_wheel_1;
motorDemand_2 = set_point_wheel_2; 
}



if (distance_travelled_1 > final_distance && distance_travelled_2 > final_distance && destination_reached == false){
destination_reached == true;
motorDemand_1 = 0;
motorDemand_2 = 0;
digitalWrite(motorBrakePin_1, HIGH);
digitalWrite(motorBrakePin_2, HIGH);
finished = true;
}


    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



 Serial.print(time_passed);
    	Serial.print(", ");
    Serial.print(distance_travelled_1);
    	Serial.print(", ");
    Serial.print(distance_travelled_2);
    	Serial.print(", ");
    Serial.print(motorDemand_1);
    	Serial.print(", ");
    Serial.print(motorDemand_2);
    Serial.print(" , ");
    Serial.print( pi);
    Serial.print(" , ");
    Serial.print( error);
    Serial.println();    


}


//void loop ends here






//the following are all user defined functions which i have called upon
//in the main loop
//it uses interrupt pins meaning that it will constantly measure the following values


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