/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name: RAKAN ALJAMIAH  --------
-------- Username: mn23r2a --------
--------------------------------------------
*/
//defining H-bridge potentiometer pins
const int pot_pin = A0; //potentiometer pin
const int enable_pin = 10; // H-bridge pin(speed)
const int direction_pin = 9; //  H-bridge pin(direction)

// defining LEDS light pins 
const int led_start_pin= 2; //green LED pin 
const int led_scannning_pin = 3; // yellow led pin
const int led_end_pin = 4;  // red led pin

// defining varibles 
float start_angle = -68.5; // the start angle of the drone which is at 212 volts
float end_angle = 0; //the desired angle of the drone which is at 410 volts
float current_angle = 0; //the current angle of the drone during the process
float set_angle_value; // not sure
// defining height varibles
const int height_tolerance = 20; 
int target_height= 0.5;
float current_height=0;
// rod length
const float rod_length = 0.491;
// defining potentiomete varible
float pot_volts = 0;  // initilasing the potentiometer reading
// defining time varibles
unsigned long scanning_time =0; // scanning time
unsigned long previous_time =0; // to store the last time the loop was executed
unsigned long current_time=0;
unsigned long time_difference =0;
const float interval = 40; // loop interval for 25Hz = 40ms


void setup() {
  Serial.begin(9600); 
  Serial.println("0. system started");
  // setting  output pins
  pinMode(led_start_pin, OUTPUT);
  pinMode(led_scannning_pin, OUTPUT);
  pinMode(led_end_pin, OUTPUT);
  pinMode(direction_pin, OUTPUT);
  pinMode(enable_pin, OUTPUT);
  // moter settings 
  digitalWrite(enable_pin, HIGH);
  digitalWrite(direction_pin, LOW);
  // setting input pins
  pinMode(pot_pin, INPUT);
  //Lighting the leds for 0.5 s
  digitalWrite(led_start_pin, HIGH);
  digitalWrite(led_scannning_pin, HIGH);
  digitalWrite(led_end_pin, HIGH);
  delay(500);
  digitalWrite(led_start_pin, LOW);
  digitalWrite(led_scannning_pin, LOW);
  digitalWrite(led_end_pin, LOW);
  Serial.println("1. system initiated");
  previous_time= millis(); // to set a value for the previous time
  Serial.println("2. controller starting");
  Serial.println("Time, Angle, Height,Error"); 
}

void loop() {
  
  drone_angle_calibration();
  current_height = rod_length*(sin((current_angle*PI)/(180))); // converting angular position to current height of the drone
  float error = target_height-current_height;
  float moter_signal = 0;
  float Kp = 0.1;
  moter_signal = Kp*error; 
  Serial.print(millis()/1000);//time
  Serial.print(",");
  Serial.print(current_angle);// it should be in radians
  Serial.print(",");
  Serial.print(current_height);
  Serial.print(",");
  Serial.print(error);
  leds_states( error);
  timing_loop();


}



 
void drone_angle_calibration() {
  pot_volts = analogRead(pot_pin); // It reads the output voltage of the potentiometer
  current_angle = map(pot_volts,212,410,-68.5,0);
  return  current_angle;
}
void leds_states(float error){
  if (error>=20 && error<=-20) {
    digitalWrite(led_start_pin, HIGH);
    digitalWrite(led_scannning_pin, LOW);
    digitalWrite(led_end_pin, LOW);
  } else if (error<-20) {
    digitalWrite(led_start_pin, LOW);
    digitalWrite(led_scannning_pin, HIGH);
    digitalWrite(led_end_pin, LOW);
  } else {
    digitalWrite(led_start_pin, LOW);
    digitalWrite(led_scannning_pin, LOW);
    digitalWrite(led_end_pin, HIGH);
  }
}
void timing_loop(){
  if (current_time-scanning_time <5000){
    time_difference = current_time-previous_time;
    if(time_difference<interval){
      delay(interval-time_difference);
     }
    previous_time = current_time;
   } else{
    digitalWrite(enable_pin, LOW);
   }    
}