/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name: James Vivian --------
-------- Username: lkpq9675 --------
--------------------------------------------
*/

//initialise global variables
const float rod_length = 0.491;

//define variables
int pot_value = 0;
float angle = 0.0;
float error = 0.0;
float height = 0.0;
boolean LED_red_state = false;
boolean LED_yellow_state = false;
boolean LED_green_state = false;
boolean motor_direction = false;
float time_seconds = 0.0;

//define pin connections
const int motor_direction_pin = 9;
const int motor_enable_pin = 10;
const int pot_pin = A0;
const int LED_pin_green = 2;
const int LED_pin_yellow = 3;
const int LED_pin_red = 4;

//motor pwm control variables
int motor_strength = 0;
int pwm_command = 0;

//optional task variables
float time_to_height = 0.0;
boolean height_reached = false;
float previous_error = 0.0;
float max_error = 0.0;
float error_sum = 0.0;
float average_error = 0.0;
int instances = 0;

void setup() {
	//begin serial communication
  Serial.begin(9600);

  //delay to allow serial communication to begin
  delay(1000);

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

  //setup pins
  pinMode(motor_direction_pin,OUTPUT);
  pinMode(motor_enable_pin,OUTPUT);
  pinMode(pot_pin,INPUT);
  pinMode(LED_pin_green,OUTPUT);
  pinMode(LED_pin_yellow,OUTPUT);
  pinMode(LED_pin_red,OUTPUT);

  //light green LED for 0.5s
	digitalWrite(LED_pin_green, HIGH);
  delay(500);
  digitalWrite(LED_pin_green, LOW);
  Serial.println("1.System Initiated");
  delay(10);
}

void loop() {
  //start controller
  Serial.println("2.Controller Starting");
  digitalWrite(motor_direction_pin, LOW);

  // time initialise
  unsigned long initial_time = 0;
  unsigned long current_time = 0;
  unsigned long elapsed_time = 0;

  //telemetry data begin
  Serial.println("Time,Angle,Height,Error");

  //Flight Control
  //begin timer
  initial_time = millis();
  //do while loop for controller time of 5 seconds
  do {
    //take reading of previous error for max height time
    previous_error = error;

    //read potentiometer value
    pot_value = analogRead(pot_pin);

    //potentiometer to angle
    pot_to_angle();

    //angle to error
    error_calculation();

    //calculate height for telemetry
    height_calculation();

    //calculate elapsed time
    current_time = millis();
    elapsed_time = current_time - initial_time;
    time_seconds = (float)elapsed_time/1000;

    //check if the height has reached the maximum for the first time
    if (error >= 0.0 && error > previous_error && height_reached == false) {
      time_to_height = time_seconds;
      height_reached = true;
    }

    //maximum error logic
    if (abs(error) > abs(max_error) && height_reached == true) {
      max_error = error;
    }

    //sum errors for the average error once target height is reached
    if (height_reached == true) {
      error_sum = error_sum + error;
    }

    //motor control
    motor_control();
    motor_strength = constrain(motor_strength,0,100);
    pwm_command = map(motor_strength,0,100,0,255);
    analogWrite(motor_enable_pin,pwm_command);

    //LED control
    LED_control();

    //output telemetry data
    Serial.print(time_seconds);
    Serial.print(",");
    Serial.print(angle);
    Serial.print(",");
    Serial.print(height);
    Serial.print(",");
    Serial.print(error);
    Serial.println();

    //delay to ensure controller runs at 25Hz
    delay(40);

  } while (elapsed_time < 5000);

  //Land and Shutdown
  //light all LEDS
  digitalWrite(LED_pin_red, HIGH);
  digitalWrite(LED_pin_yellow, HIGH);
  digitalWrite(LED_pin_green, HIGH);

  //set motor to 30% for one second
  pwm_command = map(30,0,100,0,255);
  analogWrite(motor_enable_pin,pwm_command);
  delay(1000);

  //turn off motor
  analogWrite(motor_enable_pin,0);

  Serial.println("4.Shutdown");

  //calculate the average error
  instances = (time_seconds - time_to_height)*25;
  average_error = error_sum/instances;

  //present data from optional tasks
  Serial.print("Time to target height = ");
  Serial.print(time_to_height);
  Serial.println(" seconds");

  Serial.print("Maximum error = ");
  Serial.print(max_error);
  Serial.println(" meters");

  Serial.print("Average error = ");
  Serial.print(average_error);
  Serial.println(" meters");

  //code to make sure the loop only runs once
  while (true);
}

//function to convert potentiometer reading to angle
void pot_to_angle() {
  angle = map(pot_value,212,410,-68.5,0);
}

//function to calculate error
void error_calculation() {
  error = rod_length*sin(angle*PI/180);
}

//function for motor control based on error
void motor_control() {
  if (error <= -0.35) {
    motor_strength = motor_strength + 10;
  }
  else if (error > -0.35 && error <= 0.3 && motor_strength > 40) {
    motor_strength = motor_strength - 10;
  }
  else if (error > -0.35 && error <= 0.3 && motor_strength < 30) {
    motor_strength = motor_strength + 5;
  }
  else if (error > -0.1 && error <= -0.02 && motor_strength <= 40) {
    motor_strength = motor_strength + 5;
  }
  else if (error > 0.02 && error <= 0.1) {
    motor_strength = motor_strength - 5;
  }
  else if (error > 0.1) {
    motor_strength = motor_strength - 10;
  }
}

//function to control LED output based on error
void LED_control() {
  //yellow pin control
  if (error < -0.02) {
    digitalWrite(LED_pin_yellow, HIGH);
    digitalWrite(LED_pin_green, LOW);
    digitalWrite(LED_pin_red, LOW);
  }
  //red pin control
  else if (error > 0.02) {
    digitalWrite(LED_pin_red, HIGH);
    digitalWrite(LED_pin_yellow, LOW);
    digitalWrite(LED_pin_green, LOW);
  }
  //green pin control
  else {
    digitalWrite(LED_pin_green, HIGH);
    digitalWrite(LED_pin_yellow, LOW);
    digitalWrite(LED_pin_red, LOW);
  }
}

void height_calculation() {
  height = rod_length*sin(68.5*PI/180) + error;
}