/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name:  Rayyan Choudhury
-------- Username: mn23rc
--------------------------------------------
*/


//defined pins
const int LEDgreen = 2;
const int LEDyellow = 3;
const int LEDred = 4;
const int enable = 10;
const int direction = 9;
const int potPin = A0;

//defined variables
float targetHeight = 0.05;//target height in metres
float currentHeight = 0;
float currentAngle = 0;
float error = 0;
int controlSignal = 0;
float angle = 0;
float angleRad = 0;
int bladeSpeed = 0;
float Kp = 660;
float currentTime;
float startTime;
float elapsedTime;
float previousTime;
unsigned long interval = 40; //25 Hz frequency as a time period in milliseconds
bool running = true;




void setup() {
  //begin serial transmission with a baud rate of 9600
	  Serial.begin(9600);
  delay(20);
  Serial.print("0. System Started");


  //pins are setup here
 pinMode(enable, OUTPUT);
 pinMode(direction, OUTPUT);
 pinMode(LEDgreen, OUTPUT);
 pinMode(LEDyellow, OUTPUT);
 pinMode(LEDred, OUTPUT);
 pinMode(potPin, INPUT);


	//light all leds for 0.5 seconds

digitalWrite(LEDgreen, HIGH);
digitalWrite(LEDyellow, HIGH);
digitalWrite(LEDred, HIGH);
delay(500);
digitalWrite(LEDgreen, LOW);
digitalWrite(LEDyellow, LOW);
digitalWrite(LEDred, LOW);

Serial.print('\n');
Serial.print("1. System Initiated");

//get start time
  startTime = millis();

//initialise system
Serial.print('\n');
Serial.print("2. Controller Starting");
Serial.print('\n');
Serial.print("Time(s), Angle(degrees), Height(m), Error (m)");
Serial.print('\n');
}



void loop() {

if (running) { //used to prevent shutdown procedure running more than once
if (elapsedTime < 5000) {

  //get current time
   currentTime = millis();
   
   //calculate elapsed time
    elapsedTime = currentTime - startTime;

 //use an if statement to regulate the drone to 25 Hz
  if ((currentTime - previousTime) >= interval) 
  {   previousTime = currentTime; 

//calling the function to start the drone using a proportional controller
  startDrone();


//calling function to find height
  heightCalculation();


//error
  error = targetHeight - currentHeight;


//calling function to send telemetry data as blackbox file
  telemetryData();


//calling function for the LEDs to light up corresponding to error
  errorLEDs();


}

} else if (elapsedTime >= 5000) {
//shutdown; shutdown is printed off
Serial.print('\n');
Serial.print("4.Shutdown");
shutdown();

float maxError; //max error calculated using an if statement, similar to the interval system above
if (maxError < error) {
  maxError = error;
Serial.print('\n');
Serial.print("Maximum Error: ");
Serial.print('maxError');


}
//this boolean true/false logic prevents the shutdown function calling multiple times, instead it calls once then stops
running = false;
}
}
}




  


// function to start drone 
void startDrone() {
float setPoint = targetHeight; //define setpoint

  error = setPoint - currentHeight; //error defined again
  controlSignal = error*Kp;
  bladeSpeed = controlSignal; //drone programmed to alter speed until targetHeight reached
  bladeSpeed = constrain(controlSignal, 0, 255); //blade speed constrained within the values of the potentiometer
  analogWrite(enable, bladeSpeed);
  digitalWrite(direction, LOW);
}

//function to find height
void heightCalculation() {
//obtaining potentiometer reading
float potValue = analogRead(potPin);

//potentiometer reading converted to current height 
angle = map(potValue, 212, 420, -68.5, 0);
angleRad = radians(angle); //angle converted from degrees to radians
currentHeight = (sin(angleRad)*0.491); //height calculated using trig
}

void telemetryData() {
//telemetry data sent using serial communications
delay(20);
Serial.print(elapsedTime / 1000, 2); //converted to seconds 
Serial.print(", ");
Serial.print(angle, 2);
Serial.print(", ");
Serial.print(currentHeight, 2);
Serial.print(", ");
Serial.print(error, 2);
Serial.print('\n');
}

void errorLEDs() {
//light the LEDs based on the amount of error
 if (error < -0.020) {
    // Error less than -20mm, light up the yellow LED
    digitalWrite(LEDgreen, LOW);
    digitalWrite(LEDyellow, HIGH);
    digitalWrite(LEDred, LOW);
    
  } else if (error > 0.020) {
    // Error greater than +20mm, light up the red LED
    digitalWrite(LEDgreen, LOW);
    digitalWrite(LEDyellow, LOW);
    digitalWrite(LEDred, HIGH);
  
  } else if (error >= -0.020 && error <= 0.020) {
    // Error within +-20mm, light up the green LED
    digitalWrite(LEDgreen, HIGH);
    digitalWrite(LEDyellow, LOW);
    digitalWrite(LEDred, LOW);

   }
}


void shutdown() {
//turning lights on
digitalWrite(LEDgreen, HIGH);
digitalWrite(LEDyellow, HIGH);
digitalWrite(LEDred, HIGH);
//lights on for a second
delay(1000);

//lights turned off
digitalWrite(LEDgreen, LOW);
digitalWrite(LEDyellow, LOW);
digitalWrite(LEDred, LOW);

//speed is turned down so drone falls slowly for 2 seconds
bladeSpeed = 110;
delay(2000);

//motor disabled and turned off
analogWrite(enable, 0);
digitalWrite(direction, HIGH);
}