/*   
-------- MECH1010 Coursework 2024 --------
-------- Name:  Isabelle Billington
-------- Username:  mn23i2b
*/
//i have attempted to make the drone hover for 5s after traget height has been reached 
//by either increasing or decreasing the speed 

/*   
-------- Begin Initialise Variables --------
*/
//define pins for H bridge controller 
const int enable_pin = 10; // enables pin of h bridge 
const int direction_pin = 9; // dircetion pin of h bridge 

//define pins for potemtiometer 
const int P_output_pin = A0;

//define pins for output LEDs
const int green_LED_pin = 2;
const int yellow_LED_pin = 3;
const int red_LED_pin = 4;

//intilial height of motor varibles 
float current_height = 0;
float intial_height = 0; 
float current_height_when_target_achived = 0;

//intilise target height 
const float target_height = 0.05;

// intilising the current angular position of the drone 
int current_P_output = 212; 
float target_angle = 410;

// intilising vrables to aloow the height of the drone to be found 
float error = 0; 
float angleTheta = 0;

// intilising the start speed of the motor at a  %
int speed = 200;

//variables to find the time taken for the drone to reach target height 
float StartTime = 0; 
float currentTime = 0; 
float finalTime = 0; 
float currenttime = 0;

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

/*  
-------- Begin Setup Loop --------
*/
void setup() {

  // initialize serial communication at 115200 bits per second.
  Serial.begin(9600);
  Serial.println("0. System Started ");
  Serial.print('\n');

  //set LED pins and potentiometer pin to outputs 
  pinMode(green_LED_pin, OUTPUT);
  pinMode(yellow_LED_pin, OUTPUT);
  pinMode(red_LED_pin, OUTPUT);
  //pinMode(P_output_pin, OUTPUT);

  //set H bridge controller pins to outputs 
  pinMode(enable_pin, OUTPUT);
  pinMode(direction_pin, OUTPUT);

  //set the dircetion to a defult value 
  digitalWrite(direction_pin, HIGH);

  //system initiated print 
  Serial.println("1. System Initiated ");
  Serial.print('\n');

  //lighting all LEDs for 0.5s
  LightLEDs(500);

  //system controller starting print 
  Serial.println("2. Controller starting ");
  Serial.print('\n');

  //find the curent angle in degrees
  current_P_output = analogRead(P_output_pin);
  angleTheta = map(current_P_output, 212, 410, -68.5, 0);

  //converting the intial angular position of the drone into height in m 
  intial_height = sin(angleTheta)*0.491;
  
  //the start height is the intial height 
  current_height = intial_height;

  //setting the intial error 
  error = target_height - current_height;
}
/* 
------- End Setup Loop --------
*/
/*   
-------- Begin Main Loop ---------
*/ 
void loop() 
{
  //set direction of motor 
  digitalWrite(direction_pin, LOW);
  //set the enable pin to high to turn on the motor 
  analogWrite(enable_pin, HIGH);

  //find the start time 
  StartTime = millis();

  //printing out order the values will be printed 
  Serial.print("Time, Height, Angle, Error");
  Serial.print('\n');

  //printting out each hight, time and angle for each loop
  printOutputs(StartTime, current_height, angleTheta, error);

  do
  {
    //making sure thee currenttime is accurate
    currenttime = millis();
    currenttime = currenttime - StartTime;

    //caculating the error between the current height and the traget height 
    //depending on if the drop is higher or lower then the target height
    if (current_height < target_height)
    {
      error = target_height - current_height;
    }
    else 
    {
      error = current_height - target_height ;      
    }

    //printting out each hight, time and angle for each loop
    printOutputs(currenttime, current_height, angleTheta, error);

    if (current_height >= (target_height - 0.02) && current_height <= (target_height + 0.02))
    {
      //getting the current time taken for the drone to reach the target height 
      currentTime = millis();

      //the drone will hover for 5s with green LED
      //caculating current height of the drone
      caculate_motor_control_signal(enable_pin, speed, P_output_pin, current_height, angleTheta);

      //to store the actual height achived so the error can be found before the current height is chnaged
      // to meet the conditions foe the loop to brake
      current_height_when_target_achived = current_height;

      //making drone hover for 5s 
      //putting on green LED for 5s
      digitalWrite(green_LED_pin, HIGH);
      delay(5000);
      digitalWrite(green_LED_pin, LOW);
    
      //caculating the time taken for the drone to reach the target height 
      finalTime = currentTime - StartTime; 

      //stopping the do while loop 
      current_height = target_height;

      //printout angle, height and time with there errors of the drone with respect to reaching the target height
      printOutputs(finalTime, current_height_when_target_achived, angleTheta, error);

    }
    //if the drone is to low (yellow error)
    else if ( current_height < target_height - 0.02 )
    {
      //yellow LED show 
      digitalWrite(yellow_LED_pin, HIGH);
      //adjusting the speed according to the position of the drone  
      //increaing the speed to move the drone higher 
      speed = speed + 3; 
      
      //caculating the current height using the function caculate_motor_control_signal
      caculate_motor_control_signal(enable_pin, speed, P_output_pin, current_height, angleTheta);
  
      //turning of the yellow LED
      digitalWrite(yellow_LED_pin, LOW);
    } 

    //if the drone is to high (red error)
    else if ( current_height > target_height + 0.02 )
    {
      //red LED will show 
      digitalWrite(red_LED_pin, HIGH);

      //moving the speed accoring tothe position of the drone 
      //increaing the speed to move the drone higher 
      speed = speed - 3; 

      //caculting the current height using the function caculate_motor_control_signal
      caculate_motor_control_signal(enable_pin, speed, P_output_pin, current_height, angleTheta);

      //red LED will turn off 
      digitalWrite(red_LED_pin, LOW);
    }  

  //stopping the loop either when 5s is reached or the target height has been achived
  }while(current_height != target_height && currenttime <= 5000);

//landing the drone by turning the speed of the motor down 
speed = 0;

//laneding the drone as the motor speed will be zero
caculate_motor_control_signal(enable_pin, speed, P_output_pin, current_height, angleTheta);

//turning the motor off for the drone 
analogWrite(enable_pin, LOW);

//lighting all LEDs for 1s 
LightLEDs(1000);

//system shutdown print 
Serial.print("4. Shutdown ");
Serial.print('\n');

//the maximun error encounted for after first reaching target height
Serial.print("maximun error: ");
Serial.print('\n');

//the time taken to first reaching target height
Serial.print("Time taken: ");
finalTime = finalTime / 1000;
Serial.print(finalTime);
Serial.print('\n');

//stopping the loop for 5s 
delay(5000);

}
/*   
--------- End Main Loop ----------
*/ 

/*  
--------- Declare User Defined Functions to be called by Main Loop ----------
*/ 

//function to cacaulte the current heigth and angle - used passing by referance
void caculate_motor_control_signal(int enable_pin, int speed, int P_output_pin, float& current_height, float& angleTheta) 
{
  // Changing the motor speed
  analogWrite(enable_pin, speed);

  // Reading the current angular position of the drone
  current_P_output = analogRead(P_output_pin);
  angleTheta = map(current_P_output, 212, 410, -68.5, 0);

  // Finding the current height of the drone
  current_height = sin(angleTheta) * 0.491;

  //allowing the drone to reach the new height due to the new speed at 25Hz
  delay(40);
}

//a function too ligth all LED for a stated time duraction which is passed in 
void LightLEDs (int duration)
{
  //turing the LEDs on
  digitalWrite(green_LED_pin, HIGH);
  digitalWrite(yellow_LED_pin, HIGH);
  digitalWrite(red_LED_pin, HIGH);
  //LEDs stay of the duration specified by the value passed into the fuction
  delay(duration);
  //turning the LEDs off 
  digitalWrite(green_LED_pin, LOW);
  digitalWrite(yellow_LED_pin, LOW);
  digitalWrite(red_LED_pin, LOW);
}

//a function to print time, height, angle with there errors   
void printOutputs(float time, float height, float angle, float Error)
{
  //printing the needed values as an ASCII-encoded decimal
  time = time/1000;
  Serial.print(time);
  Serial.print(",");
  Serial.print(height);
  Serial.print(",");
  Serial.print(angle);
  Serial.print(",");
  Serial.print(Error);
  Serial.print('\n');
}

/*  
--------- End Declare User Defined Functions to be called by Main Loop ----------
*/