/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name: Chittaranjan Roy --------
-------- Username: Mn23cr --------
--------------------------------------------
*/

// define pins as global variables for use throughout the code
int red_led_pin=4;
int yellow_led_pin=3;
int green_led_pin=2;
int potentiometer_pin=A0;
int bridge_direction_led_pin=9;
int bridge_enable_led_pin=10;


void setup() {
//Inatialise serial communication 
  Serial.begin(9600);
//Setup pins for input and output...
pinMode(potentiometer_pin, INPUT);
pinMode(red_led_pin, OUTPUT);
pinMode(yellow_led_pin, OUTPUT);
pinMode(green_led_pin, OUTPUT);
pinMode(bridge_direction_led_pin, OUTPUT);
pinMode(bridge_enable_led_pin, OUTPUT);

// first, to confirm the system is working, we light all led's for 0.5 seconds
digitalWrite(red_led_pin, HIGH);
digitalWrite(green_led_pin, HIGH);
digitalWrite(yellow_led_pin, HIGH);
digitalWrite(bridge_direction_led_pin, HIGH);
digitalWrite(bridge_enable_led_pin, HIGH);

delay(500);
digitalWrite(red_led_pin, LOW);
digitalWrite(green_led_pin, LOW);
digitalWrite(yellow_led_pin, LOW);
digitalWrite(bridge_direction_led_pin, LOW);
digitalWrite(bridge_enable_led_pin, LOW);
delay(500);
//let the user know the system is starting...
Serial.print("Starting and initialising system");
Serial.println();

//Turn on the motor, we will change the speed dynamically in the void loop later...for now we are just starting the system...
digitalWrite(bridge_direction_led_pin, LOW);
analogWrite(bridge_enable_led_pin,232);
//let the user know the scanning is starting...
Serial.print("Starting scan....obtaining data...");
Serial.println();
}

void loop() {
// We start a timer and make a variable where it stores the value 5000(5 seconds), this will be used in the Do...while loop...
unsigned long timer=millis();
unsigned long timer_5_sec= 5000;

// A do...while loop is used so that the below code (for telemetry) can be run for only 5 seconds
do {
//read the value from the potentiometer
float sensor_value= analogRead(potentiometer_pin);
//then map the values to -68.5 degrees to 10 degrees
float angular_position= map(sensor_value, 212, 410, -68.5, 0);
Serial.print("angular position in degrees =  ");
Serial.print(angular_position);
Serial.println();

//use the angular position to find the height of the drone
//first find the angle in radians
float angle_rad= angular_position*(PI/180);
//use trig functions to calculate height
float height_drone= (sin(angle_rad))*0.491;
Serial.print("Height of drone=  ");
Serial.print(height_drone);
Serial.println();

//here we use a series of if statements to light LED's according to where the drone is 
if (height_drone <=0.07 && height_drone>=0.03) {
  digitalWrite(green_led_pin, HIGH);
  digitalWrite(red_led_pin, LOW);
  digitalWrite(yellow_led_pin,LOW);
}
if (height_drone <0.03){
  digitalWrite(yellow_led_pin, HIGH);
  digitalWrite(red_led_pin, LOW);
  digitalWrite(green_led_pin, LOW);
}
if (height_drone>0.07){
  digitalWrite(red_led_pin, HIGH);
  digitalWrite(yellow_led_pin, LOW);
  digitalWrite(green_led_pin, LOW);
}

//here we use a kp system using errors which will dynamically change the speed of the drone as it reaches the target height
float error= 0.05 - height_drone;
//multiply the error by a factor, the factor basically determines the rate at which the speed of the drone will increase/decrease
int kp = error*70 ;
analogWrite(bridge_enable_led_pin,200+kp);
//print the error...
Serial.print("Error (difference between target height and current height) =  ");
Serial.print(error);
Serial.println();

} while (millis()< timer_5_sec); //use timer_5_sec here so that this loop runs on the condition that the timer is below 5 seconds since it started
//let the user know that the system has finished
Serial.print("Scanning complete!");
Serial.println();
Serial.print("The system has reached its end...");
Serial.println();
//print the total time for the operation of the code
unsigned long end_time = millis();
Serial.print("End time is (in millis):  ");
Serial.print(end_time);
Serial.println();
// light LEDS for 1 second to indicate the start of the shutdown of the system
digitalWrite(red_led_pin, HIGH);
digitalWrite(green_led_pin, HIGH);
digitalWrite(yellow_led_pin, HIGH);
digitalWrite(bridge_direction_led_pin, HIGH);
digitalWrite(bridge_enable_led_pin, HIGH);

delay(1000);
digitalWrite(red_led_pin, LOW);
digitalWrite(green_led_pin, LOW);
digitalWrite(yellow_led_pin, LOW);
digitalWrite(bridge_direction_led_pin, LOW);
digitalWrite(bridge_enable_led_pin, LOW);
//print a statement saying the system is about to shut down and turn off the motor
Serial.print("Ending system... shutting down the drone and communications...");
analogWrite(bridge_enable_led_pin,0);
delay(1000);
Serial.println();
Serial.print("Shutdown complete");
delay(20000000);
//we put a long delay here to indefinitely delay the void loop function, as you cannot really stop the void loop function...
//this way we can "shut down" the system
}