/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name:  MANAN ADEEL
-------- Username:  MN23M2A
--------------------------------------------


/* 
--------------------------------------------  
-------- Begin Initialise Variables --------
--------------------------------------------
*/
// STAGE 1
// Define LEDS here

const int Green_LED = 2; // system start
const int Yellow_LED = 3; // scanning start
const int Red_LED = 4; // Scanning complete

// define potentiometer
const int potentiometer_pin = A0; 
int potentiometer_value = 0;

// variables to control H bridge 
int Hbridge_direction = 9;
int Hbridge_switch = 10;

// variables for length
const int rod_length = 49.1;

// variables for angle and height
const float target_height = 50.0; 
float error_height = 0.0;
float current_height = 0;
float max_height = 0;

// define variables for angle
int angle_value = 0;
float angle_abs = 0;

// define speeds values 
int motor_speed = 50;
float power_out= 0.0;
int power = 100;

unsigned long currentTime = 0;

/* 
--------------------------------------------  
-------- End Initialise Variables ----------
--------------------------------------------
*/


/* 
----------------------------------  
-------- Begin Setup Loop --------
----------------------------------
*/


void setup() {
// initialize serial communication at 9600 bits per second.
  Serial.begin(9600);
  Serial.println("0. System started");
//**** STAGE 2a **** 
  //Configure the Arduino Pins
  pinMode(Green_LED,OUTPUT);
  pinMode(Yellow_LED, OUTPUT);
  pinMode(Red_LED, OUTPUT);

  pinMode(potentiometer_pin, INPUT);
  pinMode(Hbridge_switch, OUTPUT);
  pinMode(Hbridge_direction, OUTPUT);
//**** STAGE 2b ****
  unsigned long startTime = micros();// start time for process

  //Light all LEDS for 0.5 seconds
  LED_start();

  Serial.println("1. System initialised");
 // initialise the drone
 start_drone();
  
 // Setup format for blackbox
 Serial.print("Time");
 Serial.print(",");
 Serial.print("Angle");
 Serial.print(",");
 Serial.print("Height");
 Serial.print(",");
 Serial.println("Error");
}

/* 
----------------------------------  
-------- Begin Loop --------------
----------------------------------
*/

void loop() {
//**** STAGE 3a **** 
float initialTime = millis();// get the initial time of the loop
 
 //obtain angle value 
  potentiometer_value = analogRead(potentiometer_pin);
  angle_value = map(potentiometer_value, 212, 410, -68.5, 0);

 // calculate desired height
 float angle_max = 68.5*(PI/180);
 float desired_height = (rod_length*sin(angle_max))+50;

 // obtain current angle positive
  angle_abs = abs(angle_value);

// obtain current height
  max_height = (rod_length*sin(angle_abs));
  current_height = rod_length*sin(angle_abs*(PI/180));
  current_height = max_height-current_height;
 if (angle_value>0){
  current_height = max_height+current_height;
 }

  // blackbox time
  currentTime = millis();

//**** STAGE 3b **** 
  // calculate error
  error_height = (desired_height - current_height);
  // run blacbox to obtain data in a loop
  blackbox();
  
  proportionalController();
  land_drone();
  
 float endTime = millis();
 float elapsedTime = endTime-initialTime;
 float delayTime = 40000-elapsedTime;
 delayMicroseconds(delayTime);
//**** STAGE 4 **** 
 if (millis() > 5000){
 Serial.println("4. shutdown");
 LED_end();
 stop_drone();
 delay(1000000);}
}


void start_drone() { // starts drone at max power
  digitalWrite(Hbridge_direction, LOW);
  power_out = map(power,0,100,0,255);
  analogWrite(Hbridge_switch,power_out);
  delay(1350);
  power_out = map(motor_speed, 0, 100, 0 , 255);
  analogWrite(Hbridge_switch,power_out);
  Serial.println("2. controller starting");
}

void land_drone() { // reduces power of drone blades
int powerLand = 30;
digitalWrite(Hbridge_direction, LOW);
  power_out = map(powerLand,0,100,0,255);
  analogWrite(Hbridge_switch,power_out);
  
}
void stop_drone() { // stops drone blades
int powerStop = 0;
digitalWrite(Hbridge_direction, LOW);
  power_out = map(powerStop,0,100,0,255);
  analogWrite(Hbridge_switch,power_out);
}
void blackbox(){ // Obtains a row of values for quantities 
 Serial.print(currentTime);
 Serial.print(",");
 Serial.print(angle_value);
 Serial.print(",");
 Serial.print(current_height);
 Serial.print(",");
 Serial.println(error_height);
 
}
// changes speed based on height lighting LEDS accordingly
void proportionalController(){

float speedincrease = 0.00;
float speeddecrease = 0.00;
float speedsame = 0.00;

if (error_height>20){
   digitalWrite(Yellow_LED, LOW);
   digitalWrite(Green_LED, LOW);
   digitalWrite(Red_LED, HIGH);
   speedincrease = motor_speed*1.02;
   speedincrease = constrain(speedincrease,0,100);
   power = map(speedincrease, 0, 100, 0 , 255);
   analogWrite(Hbridge_switch, speedincrease);
   motor_speed = speedincrease;
}
if (error_height<20){
   digitalWrite(Yellow_LED, HIGH);
   digitalWrite(Green_LED, LOW);
   digitalWrite(Red_LED, LOW);
   speeddecrease = motor_speed*0.98;
   speeddecrease = constrain(speeddecrease,0,100);
   power = map(speeddecrease, 0, 100, 0 , 255);
   analogWrite(Hbridge_switch, speeddecrease);
   motor_speed = speeddecrease;
}
if (error_height<=20 && error_height>=-20){
   digitalWrite(Yellow_LED, LOW);
   digitalWrite(Green_LED, HIGH);
   digitalWrite(Red_LED, LOW);
   speedsame = motor_speed;
   speedsame = constrain(speedsame,0,100);
   power = map(speedsame, 0, 100, 0 , 255);
   analogWrite(Hbridge_switch, speedsame);
   motor_speed = speedsame;
}
}
void LED_start() { // Lights LEDS for 1/2 a second and turns off
  digitalWrite(Green_LED, HIGH);
  digitalWrite(Yellow_LED, HIGH);
  digitalWrite(Red_LED, HIGH);
  delay(500);
  digitalWrite(Green_LED, LOW);
  digitalWrite(Yellow_LED, LOW);
  digitalWrite(Red_LED, LOW);
}

void LED_end (){ // causes the LEDS to light for a second and turn off
  digitalWrite(Green_LED, HIGH);
  digitalWrite(Yellow_LED, HIGH);
  digitalWrite(Red_LED, HIGH);
  delay(1000);
  digitalWrite(Green_LED, LOW);
  digitalWrite(Yellow_LED, LOW);
  digitalWrite(Red_LED, LOW);
}