/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name: Oluseye Arinoso
-------- Username:  201695440
--------------------------------------------
*/
// setting up pins
const int green_LED = 2;
const int yellow_LED = 3;
const int red_LED = 4;
const int direction_pin = 9;
const int enable_pin = 10; 
const int potentiometer = A0;

// setting up variables
float p_val = 0;
const float support_rod = 0.491;
float h_angle = 0 ;

const int ctrl_freq = 25;
 float ctrl_dt =  1000 * (1 / (float) ctrl_freq); // doesn't actually equal 40 for some reason

const int uncertainty = 20;
const float target_height = 0.05;

const int start_speed = 127; // high start speed to get the helicopter moving off the ground 
int motor_speed = 0;


float prev_error = 0;
float integralval = 0;

float Kp =600;
const float Ki = -0.05;
// const float Kd = 2;

const int size = ctrl_freq * 5; // max size of array created in code as readings are 25hz for 5 seconds

int counter = 0;
float target_time = 0;

float sum_errors= 0;

float MaxV= 0;
float max_post_error = 0 ;

int ting = 0;

void setup() {
  delay(500);
  Serial.begin(9600);
  pinMode(green_LED, OUTPUT);
  pinMode(red_LED, OUTPUT);
  pinMode(yellow_LED, OUTPUT);
  pinMode(potentiometer, INPUT);
  pinMode(direction_pin, OUTPUT);
  pinMode(enable_pin, OUTPUT);
  
  Serial.println("System started");
  // Lighting LEDs to max brightness for 0.5 secondss 
  analogWrite(green_LED, 255);
  analogWrite(red_LED, 255);
  analogWrite(yellow_LED, 255); 
  delay(500); 
  analogWrite(green_LED, 0);
  analogWrite(red_LED, 0);
  analogWrite(yellow_LED, 0); 

  motor_speed = start_speed;

  analogWrite(enable_pin, motor_speed); 
  Serial.println("System initiated");
  
}



void loop() {

Serial.println("Controller starting");
Serial.println("Time , Angle , Height , Error");

// reading start of whole control system
unsigned long start_time = millis();
unsigned long current_time = millis();
do{  
  
  // reading time to run loop at 25Hz
unsigned long loop_start = millis();  
unsigned long time = loop_start - start_time;

// reading angle 

 //calc height

 float height = getHeight(potentiometer, support_rod);
 
// control system
 
 float error = target_height - height;
 float mm_error = error *1000;
 
 integralval = controller(error, prev_error, enable_pin, motor_speed, Kp, Ki, ctrl_dt, integralval );

   
  float loop_time = (float) time/1000;
  Serial.print(loop_time,  2);
  Serial.print(",");
  Serial.print(h_angle), 2;
  Serial.print(",");
  Serial.print(height, 2);
  Serial.print(",");
  Serial.println(error, 2);
  

  // Finds max error 
  if(abs(error) > MaxV){
    MaxV = abs(error);
  }
  

  if( mm_error <= uncertainty && mm_error >= - uncertainty){ // in the 20mm limits set for the helicopter height
    analogWrite(green_LED, 255);
    analogWrite(red_LED, 0);
    analogWrite(yellow_LED, 0);

// Chnage the Kp value in here so that when it is close to Set point and error is smaller controller can still suitably change values
// Kp =20;

    if(counter == 0 ){ //checks that it is the first time target height is reached
     target_time = loop_time;
     
         max_post_error = abs(error); 
       sum_errors = sum_errors +abs(error);
     counter = counter + 1;
     
  }

  }
  else if(mm_error < - uncertainty ){
    analogWrite(yellow_LED, 255);
    analogWrite(red_LED, 0);
    analogWrite(green_LED, 0);
    // digital write used as direction only has 2 options, we only want up or down
    //digitalWrite(direction_pin, HIGH);
  //  Kp = 0.6;
  }
  else{
    analogWrite(red_LED, 255);
    analogWrite(green_LED, 0);
    analogWrite(yellow_LED, 0);
    // Kp = 2; // this Kp is bigger as there is more of a problem with other shooting the values, so when overshooting we want the controller to be more reactive
  }
  
  
  
  // IF counter is more than 0 it means target height hsa been reached 
  if(counter > 0 ){ 
    // find max error after target height is reached
    if(abs(error) > max_post_error){
    max_post_error = abs(error);
    
    
   
  }
   // sums all errors after target height is reached
    sum_errors = sum_errors +abs(error);
     counter = counter + 1; // counts through so we know how many data points there are after target height
  };


  // if(motor_speed > 215){
  //   ting = ting + 1;
  // }
  // else if (ting < 215){
  //   ting = 0;
  // }

  prev_error = error;
  
//This code makes sure loop is 25hz 
   unsigned long current_loop_time = millis();
   while (current_loop_time - loop_start < ctrl_dt) {
       current_loop_time = millis();
   }
  current_time = millis();
}while(current_time - start_time < 5000); // stops control system after 5 seconds
// switching all LEDs off briefly after control loop
  analogWrite(green_LED, 0);
  analogWrite(red_LED, 0);
  analogWrite(yellow_LED, 0); 


// Lighting LEDs to max brightness for 1 secondss 
  analogWrite(green_LED, 255);
  analogWrite(red_LED, 255);
  analogWrite(yellow_LED, 255); 
  delay(1000); 
  analogWrite(green_LED, 0);
  analogWrite(red_LED, 0);
  analogWrite(yellow_LED, 0); 

  //switching motor off 
  analogWrite(enable_pin, 0); 
   
   Serial.print("Max Error: ");
   Serial.println(MaxV, 2);
   
   
 
   float average_error= sum_errors/ counter; // counter is how many times target height is reached

  if(counter > 0){
   Serial.print("Time taken to reach target height = ");
   Serial.println(target_time, 2);
   Serial.print("Max error after target reached = ");
   Serial.println(max_post_error, 2);
   Serial.print("Average error after target reached = ");
   Serial.println(average_error, 2);
  }
  else{
    Serial.println("Target height was not reached");
  }
  
  Serial.println("Shutdown");
  
//making sure loop doesn't restart
delay(10000);
}


// function to get heigh of helicopter/drone
float getHeight(float potentiometerpin, float armLength){
  // reads potentiometer value then maps it to angle in radians
  float p_val = analogRead(potentiometerpin);
   h_angle  = map(p_val, 212,410, -68.5, 0);
  //converts angle from degrees to radians
  float rad = (h_angle * PI)/180;

  float helicopter_height = armLength*sin(rad);
 // helicopter_height = helicopter_height *1000; // to get height in mm
  return helicopter_height; 
}

//control system function
float controller(float controlError, float previousError, float output_pin, float output_speed, float KP, float KI, float T, float prevint){
  
  float intergral = (((KI*T)/2)*(controlError + previousError)) + prevint; // p.s. I know i spelt integral wrong it's too late
 Serial.println(intergral);
  
  //Adds to oirignal speed as when target height is reached and error is zero out speed stays the same
  float new_speed = (float) output_speed + (controlError * KP);
  
  
  if(new_speed > 255){ // This if structure used because helicopter blades don't work properly if PWM > 255 and if above propeller speed is too fast it will overshoot the target height by a lot
    new_speed = 255;
  }
  else if(new_speed < 0){
    new_speed = 0;
  }

  // intergrator limit so integral doesn't get too low or too high 
  if(intergral < -48){
    intergral = -48;
  }
  else if (intergral > 50){
    intergral = 50;
  }

  new_speed = new_speed + intergral;

  // if(ting > 50){   // to slow down accent  
  //   new_speed = new_speed - 10;
  // }
 
  Serial.println(new_speed);
  analogWrite(enable_pin, new_speed);
  //
  return intergral;
  //after reading schoalrs compass i realised for our control system we do not really want a derivative term as
  // retuning to set point from overshoot is not difficult or strictly undersiable
  
}

//finds max value of an array
float maxVal(float Array[], int sizeofarray){
  float Max = 0;
    
   for (int i = 0;i < sizeofarray ; i++) {
      if ((abs(Array[i])) > Max) {
         Max = Array[i];
      } 
      Serial.print("Array i : ");
      Serial.println(Array[i]);
      Serial.print("Max value : ");
      Serial.println(Max);
   }
   return Max;
}