/* 
--------------------------------------------  
-------- MECH1010 Coursework 2023 --------
-------- Name:  Moustafa Mesharafa
-------- Username:  mn22mm
--------------------------------------------
*/

//**** SETUP LIBRARY ************** LEAVE THESE LINES UNMODIFIED! ******************
#include <Servo.h> //Include the Servo library (for communication with motor controllers)
Servo motor_Left;   //Setup Left Motor
Servo motor_Right;  //Setup Right Motor
//*********************************************************************************

//Pin definitions
const int potentiometerPin = A0;
const int startupLedPin = 2;
const int scanningLedPin = 3;
const int scanEndLedPin = 5;
const int shutdownLedPin = 4;

//Calibration data for potentiometer
float potentiometerVoltage = 0.0;
const float potentiometerZeroAngleVoltage = 552.0;
const float potentiometerMinAngleVoltage = 729.0;
const float potentiometerMaxAngleVoltage = 371.0;
const float potentiometerMinAngle = -22.0;
const float potentiometerMaxAngle = 22.0;

//Controller parameters
 const float targetAngle = 0.0;
double kp = 3;
double  ki = 0.055;
double  kd = 1;
float pTerm = 0;
float iTerm = 0;
float dTerm =0;
float PID;
const unsigned long dt = 40;  //time step in milliseconds

//Control variables
float currentAngle = 0.0;
float error = 0.0;
float lastError = 0.0;
float sumError = 0.0;
float motorControlSignal = 0.0;
float newMotorControlSignal= 0.0;
int motorLeftPower = 0;
int motorRightPower = 0;
float time, elapsedTime, prevTime;
const unsigned long scanDuration = 5000;


void setup() {

  //**** SETUP MOTORS ************** LEAVE THESE LINES UNMODIFIED! ******************
  motor_Left.attach(10); //Atttach the left motor controller
  motor_Right.attach(9);
  delay(300);
  //Initialise motors
  motor_Left.write(1000);
  motor_Right.write(1000);
  delay(2000); //wait 2s to initialise the controller
  time = millis();
  
  //*********************************************************************************
	motor_Left.write(45); //MOTOR OFF (use 180 for full power)
	motor_Right.write(45); //MOTOR OFF (use 180 for full power)
  //Setup LED pins as outputs
  pinMode(startupLedPin, OUTPUT);
  pinMode(scanningLedPin, OUTPUT);
  pinMode(scanEndLedPin, OUTPUT);
  pinMode(shutdownLedPin, OUTPUT);
  
  //Turn on startup LED for 1 second
  digitalWrite(startupLedPin, HIGH);
  delay(1000);
  digitalWrite(startupLedPin, LOW);
  
  //Set up serial communication
  Serial.begin(9600);
}

void loop() {
Serial.println("2. Controller Starting");
Serial.println("Time, Angle, Error, Control Signal, Motor R, Motor L");
prevTime = time;
time=millis();
elapsedTime=(time-prevTime)/1000;
potentiometerVoltage = analogRead(potentiometerPin);
currentAngle = (552 - potentiometerVoltage) * (22.0/179.0); //Linear conversion from voltage to angle range 
error= currentAngle - 0;
pTerm = kp*error; //PID terms

dTerm = kd*((error - lastError)/elapsedTime);
PID= pTerm + iTerm + dTerm;
constrain(PID, -120, 120); //limit PID signal


motorLeftPower =  5 - PID; //motor control signal to power
motorRightPower = 5 + PID;
constrain(motorLeftPower, 45, 85); // limit power between max and min
constrain(motorRightPower, 45, 85);
motor_Left.write(motorLeftPower);
motor_Right.write(motorRightPower);
lastError=error;


//Send telemetry data over serial
  Serial.print(time / 1000.0, 2);
  Serial.print(",");
  Serial.print(currentAngle, 2);
  Serial.print(",");
  Serial.print(error, 2);
  Serial.print(",");
  Serial.print(motorControlSignal, 2);
  Serial.print(",");
  Serial.print(motorLeftPower);
  Serial.print(",");
  Serial.println(motorRightPower);
  do { //start scanning if within 5 degrees
    int scanTimer=millis();
  digitalWrite(scanningLedPin, HIGH);
  if (millis()-scanTimer < 5000)
{
    while(millis()-scanTimer < 5000){ //keep scanning while timer less than 5s
    
    potentiometerVoltage = analogRead(potentiometerPin); // repeat to keep balancing even when scanning
    currentAngle = (552 - potentiometerVoltage) * (22.0/179.0);
    error= currentAngle - 0;
    pTerm = kp*error;
    iTerm = iTerm +(ki*error);  
    dTerm = kd*((error - lastError)/elapsedTime);
    PID= pTerm + iTerm + dTerm;
    constrain(PID, -120, 120);
    motorLeftPower =  5 - PID;
    motorRightPower = 5 + PID;
    constrain(motorLeftPower, 45, 85);
    constrain(motorRightPower, 45, 85);
    motor_Left.write(motorLeftPower);
    motor_Right.write(motorRightPower);
    lastError=error;


    //Send telemetry data over serial
    Serial.print(millis() / 1000.0, 2);
    Serial.print(",");
    Serial.print(currentAngle, 2);
    Serial.print(",");
    Serial.print(error, 2);
    Serial.print(",");
    Serial.print(motorControlSignal, 2);
    Serial.print(",");
    Serial.print(motorLeftPower /0.4 );
    Serial.print("%,");
    Serial.print(motorRightPower/ 0.4);
    Serial.println("%");

  if (millis()-scanTimer > 5000)
      {
        digitalWrite(scanningLedPin, LOW); //shutdown sequence
        digitalWrite(shutdownLedPin, HIGH);
        delay(1000);
        digitalWrite(shutdownLedPin, LOW);
         motor_Left.write(45);
        motor_Right.write(45);
        Serial.println("4. Shutdown");
        delay(150);
        exit(0);
      }
  
  } 
  }
  } while(lastError<=5 && lastError>=-5);
}