/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name:  Amir Ali
-------- Username:  mn23aaga
--------------------------------------------
*/

//Assign LED, potentiometer, motor to pins.
int Green_LED = 2;
int Yellow_LED = 3;
int Red_LED = 4;
int output_vpot = A0;
int Hbridge_enable = 10;
int Hbridge_direction = 9;

//other variables
unsigned long start_stopwatch = 0;
float target_height = 50;

// Name given variables and assign values(Rod)
float Rod = 491;

//Ensure the controller is running at 25Hz by making variable loopTime
// 1/25Hz = 0.04sec, 0.04s = 40s
int loopTime = 40;


void setup() {
Serial.begin(9600);

//Assign pin modes
pinMode(2,OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(A0, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);

//Light LEDS for 0.5 seconds to indicate start
digitalWrite(Green_LED, HIGH);
digitalWrite(Yellow_LED, HIGH);
digitalWrite(Red_LED, HIGH);
delay(500);

//Turn off after the 0.5 seconds
digitalWrite(Green_LED, LOW);
digitalWrite(Yellow_LED, LOW);
digitalWrite(Red_LED, LOW);

//Start a timer to record the time from the start of the loop
//start_stopwatch = millis();
}

void loop() {
delay(40);
float start_stopwatch = millis();
//turn on the drone
digitalWrite(10, HIGH);
analogWrite(10, 161);

//read the potentiometer
int drone_angle_position = analogRead(A0);

//Map angle given values (212 corresponds to -68.5, 410 corresponds to 0)
float current_angle = map(drone_angle_position, 212, 410, -68.5, 0);

//Calculate the drones current height from potentiometer
float drone_current_height = (Rod * sin(current_angle));

//Calculate the erorr
float erorr = (target_height - drone_current_height);

// Print all the calculated values (relay telemtry)
Serial.print("Time             seconds       ");
Serial.println(start_stopwatch/1000);
Serial.print("Current angle    degrees       ");
Serial.println(current_angle);
Serial.print("height           millimeters   ");
Serial.println(drone_current_height);
Serial.print("Erorr            millimeters   ");
Serial.println(erorr);
Serial.println();

//Set up lights to indicate wether drone is at correct height
if (erorr >= 20 && erorr <= 20) {
  digitalWrite(2, HIGH); // Green LED ON
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);}
else if (erorr < -20) {
  digitalWrite(2, LOW);
  digitalWrite(3, HIGH); // Yellow LED ON
  digitalWrite(4, LOW);}
else {
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, HIGH); // Red LED ON
}


if (start_stopwatch > 10000) {
  //digitalWrite(10, LOW);
  digitalWrite(9, HIGH);
  landing();
  stop_motor();
  while (1);
 } 
}

void landing() {
digitalWrite(9, HIGH);
analogWrite(10,20);
}

void stop_motor() {
  delay (4000);
digitalWrite(10, LOW);  
//Light LEDS for 1 second to indicate shutdown
digitalWrite(Green_LED, HIGH);
digitalWrite(Yellow_LED, HIGH);
digitalWrite(Red_LED, HIGH);
delay(1000);

//Turn off after the 1 seconds
digitalWrite(Green_LED, LOW);
digitalWrite(Yellow_LED, LOW);
digitalWrite(Red_LED, LOW);

}