/* 
--------------------------------------------  
-------- MECH1010 Coursework 2023 --------
-------- Name:  Theo Simms
-------- Username:  mn22ts
--------------------------------------------
*/

//**** 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
//*********************************************************************************

#define start_up_LED 2
#define scanning_LED 3
#define scan_end_LED 4

#define pot_volt_in A0
#define pot_volt_min 729
#define pot_volt_max 271
#define angle_min - 22
#define angle_max 22

#define control_signal_min - 11
#define control_signal_max 11
#define motor_max 85
#define motor_min 45

#define kp 0.5

float start_timer = 0;
float timer_1 = 0;
float timer_2 = 0;
float counter = 0;

float angle = 0;

#define control_signal_start 65
float control_signal_conv = 0;
float control_signal_L = 0;
float control_signal_R = 0;
int control_signal_L_per = 0;
int control_signal_R_per = 0;

void setup() {

  Serial.begin(9600);

  Serial.println("0.System Started");

  //**** 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.writeMicroseconds(1000);
  motor_Right.writeMicroseconds(1000);
  delay(2000); //wait 2s

  //*********************************************************************************

  pinMode(start_up_LED, OUTPUT);
  pinMode(scanning_LED, OUTPUT);
  pinMode(scan_end_LED, OUTPUT);

  pinMode(pot_volt_in, INPUT);

  Serial.println("1.System Initiated");
  Serial.println("2.Controller Starting");

  digitalWrite(start_up_LED, HIGH);
  delay(1000);
  digitalWrite(start_up_LED, LOW);

  motor_Left.write(control_signal_start);
  motor_Right.write(control_signal_start);

  Serial.println("Time, Angle, Error, Control Signal, L Motor, R Motor");
  start_timer = millis() / 1000;
}

void loop() {

  float pot_volt = analogRead(pot_volt_in);

  angle = (pot_volt - pot_volt_min) * (angle_max - angle_min) / (pot_volt_max - pot_volt_min) + angle_min;

  timer_1 = (millis() / 1000) - 3;

  // float control_fraction = angle / 22;

  float setpoint = 0;

  float control_error = setpoint - angle;
  float control_signal = control_error * kp;


  

  control_signal_L = control_signal_start;
  control_signal_R = control_signal_start;

    if (angle > 0 && control_signal_L <= 85) {
     control_signal_L = control_signal_L + 1;
    }
     if (angle < 0 && control_signal_R <= 85) {
     control_signal_R = control_signal_R + 1;
    }
   if (angle > 0 && control_signal_L >= 85) {
     control_signal_R = control_signal_R - 1;
    }
     if (angle > 0 && control_signal_R >= 85) {
     control_signal_L = control_signal_L - 1;
    }

    motor_Left.write(control_signal_L);
    motor_Right.write(control_signal_R);

    control_signal_L_per = (control_signal_L / 40) * 100;
    control_signal_R_per = (control_signal_R / 40) * 100;

    Serial.print(timer_1);
    Serial.print(" , ");
    Serial.print(angle);
    Serial.print(" , ");
    Serial.print(control_error);
    Serial.print(" , ");
    Serial.print(control_signal_conv);
    Serial.print("  ,  ");
    Serial.print(control_signal_L);
    Serial.print("  ,  ");
    Serial.println(control_signal_R);

    motor_Left.write(control_signal_L);
    motor_Right.write(control_signal_R);



  if (-5 < angle && angle < 5) {

  timer_2 = (millis() / 1000);

  counter = timer_2 - start_timer;

  digitalWrite(scanning_LED, HIGH);

  }

  if (counter >= 5) {

  digitalWrite(scanning_LED, LOW);
  digitalWrite(scan_end_LED, HIGH);
  delay(1000);

  motor_Left.write(60);
  motor_Right.write(60);

  digitalWrite(scan_end_LED, LOW);
  Serial.println("End");
  }

}