//  This program is a control system for a motor controled arm that is used to modle a helicopter
// please contact the email asociated with mn232aw at leeds uni if you have any questions
//date last eddited 03/05/24(dd/mm/yy)
/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name:  Andrew Wilkins
-------- Username:  MN232AW
--------------------------------------------
*/

//Initiate variables 

  int motor_add=10;   //motor address
  float motor_speed=0.0;  //motor speed (0-255)
  int direction_add=1; //motor direction adress
  int motor_dir=9;    //motor direction adress 
  float kp= -0.55; //controler proportionality constant
  float ki=-0.08; //controler integral constant
  float sum_int=-400.00;  //controler integrator 

  unsigned long program_time=0;   //Total program time
  unsigned long loop_time=0;      // Time for each loop(maintains 25Hz)
  double time_seconds=0.00;       //Time in seconds 

  int led_green=2;    //led adress
  int led_yellow=3;   //led adress
  int led_red=4;      //led adress

  int angle_poten=A0;          //The adress of the angle measuror 
  int angle_adc_output=0;      //Calibrated angle value 

  float height_mm=0;          //Calculated height(millimeters)
  float height_m=0.00;        //height (meters)

  int error_mm=0;           // distance from target height(mm)
  float error_m=0.00;       //distance from target height (m)
  float theta_deg=30;       //Arm angle



 //Statistics to be printed at end of program
  int avarage_deviation=0;  //avarage Height eviation from target height
  //*********IMPORTANT COMMENT BELLOW******************//
  //RMS DEVIATION IS USED INSTEAD OF AVARAGE AS IT IS MORE USEFULL DATA THIS CAN BE CHANGED UPPON CUSTOMERS REQUEST//
  //*********IMPORTANT COMMENT ABOVE******************//
  int values=0;       //number of values in avarage deviation
  int max_deviation=0;//max diviation
  int max_error=0;
  int target_time=0;  // time from start to target 
  
  int sys_start_flag=0;         //flag for system start
  int sys_initialised_flag=1;   // flag for sys initialised 



void setup() {      //sets up output& input pins
  Serial.begin(9600);
  pinMode(led_red, OUTPUT);
  pinMode(led_yellow, OUTPUT);
  pinMode(led_green, OUTPUT);
  pinMode(angle_poten, INPUT);
  pinMode(motor_add, OUTPUT);
  pinMode(direction_add, OUTPUT);
}


void loop() {

do{}while((millis()-loop_time)<1000); //ensures helicopter has landed after other students run  


printdebug();   // function: print debug state & csv collum heddings
program_time=millis();	//starts timer
loop_time=millis();     //loop time is 

do{ //program start loop 
  digitalWrite(led_red, HIGH);    //lights All LEDS
  digitalWrite(led_green, HIGH);
  digitalWrite(led_yellow, HIGH);
}while((millis()-loop_time)<500); //Waits 0.5s 



/////////////***Main loop begins ****////////////
do{ 
  get_values();        //gets values;& RESETS LOOP TIME
  print_values();     //prints values
  indicate_height_led();        //funtion: sets LED state
  target_height_check();        //gathers (optional) statistics
  
  //motor_speed = map (height_mm, -420, 50, 255, 75);  // proportional controler
  //motor_speed*=kp;   //K value for controler

  sum_int=sum_int+error_mm/25;              //integral of position 
 // motor_speed_int=(kp*error_mm)+(sum_int*ki);
  motor_speed=(kp*error_mm)+(sum_int*ki); //motor speed control function
 
  //Serial.println(motor_speed);


  if(motor_speed<0){  //checks motor direction & sets direction
    digitalWrite(motor_dir, HIGH);
  }else{digitalWrite(motor_dir, LOW);}
  if(motor_speed>255){  //checks motor direction & sets direction
    motor_speed=255;
  }
analogWrite(motor_add, motor_speed); //sends speed to motor
delay(40-(millis()-loop_time));       //delays to get 25Hz
}while((millis()-program_time)<5500);  //ends loop after 5 seconds  



 //**** landing loop***////

 //** pauses program after program ends**// 
do{       //turns motor off and sets leds to high
  get_values(); 
  if(height_mm>-350){   //sets motor speed to low to give soft landing
  motor_speed=50;
  }else{motor_speed=0;} //stops motor
  analogWrite(motor_add, motor_speed);
  digitalWrite(led_red, HIGH);
  digitalWrite(led_green, HIGH);
  digitalWrite(led_yellow, HIGH);
  


  
}while((millis()-program_time)<6500); //waits 1 aditional second
  motor_speed=0;
  analogWrite(motor_add, motor_speed);  //failsafe motor stop
  Serial.println("4. Shutdown");
  digitalWrite(led_red, LOW);
  digitalWrite(led_green, LOW);
  digitalWrite(led_yellow, LOW);

Serial.print("time to target height: ");    //prints optional statistics from target height check
Serial.print(target_time-1000);//program is set to wait 1s before beginning to ensure other students helecopter has landed 
Serial.println("ms");
Serial.print("maximum error from target height: ");
Serial.print(max_deviation);
Serial.println("mm");
Serial.print("Average rms error:");
Serial.print(avarage_deviation); 
Serial.println("mm");

do{}while(1==1);    //pauses the program indefinatly, i think this is better than a break command
}



// Functions//



int printdebug(){   // function: print debug state & csv collum heddings
  Serial.println("0. System Started");  
  if(sys_initialised_flag==1){          //checks if system initialised 
    Serial.println("1. System Initiated");//print to user
  }else{
    Serial.println("1. System did not Initiated");//reports error if....
  }
  Serial.println("2. Controller Starting"); // reports controler starting
  Serial.print("Time");     //prints title heddings for .csv
  Serial.print(",");
  Serial.print("Angle");
  Serial.print(",");
  Serial.print(" Height");
  Serial.print(",");
  Serial.println("Error");
  }


int get_values(){  //Function: resets loop gets values calibrtes then prints
  loop_time=millis();       //rsets loop time
  angle_adc_output=analogRead(angle_poten);//gets digital 0-1023 input
  //theta_deg=map (angle_adc_output, 212, 410, -68.5, 0);
  theta_deg=angle_adc_output*0.34596-141.84;  //calibrates input to deg(-180 to 180)
  height_mm = 491*sin(3.14159/180*theta_deg); //calculates height (mm)
  height_m=height_mm/1000.000;                //heigh in (m)
  error_mm=height_mm-50;
  error_m=height_m-0.05;                      // caclculates distance from target height (50mm=0.05m)
  time_seconds=(millis()); time_seconds=-1+time_seconds/1000; //do not seperate 2 lines of code
}


 int print_values(){  //Function: to print values to user display
  Serial.print(time_seconds , 2 );  //prints values 2dp
  Serial.print(",");
  Serial.print(theta_deg , 2 );
  Serial.print(",");
  Serial.print(height_m , 2 );
  Serial.print(",");
  Serial.println(error_m , 2 );
}  



int indicate_height_led(){          // function to check height& light corisponding LEDS
  if (30<=height_mm && 70>=height_mm){  //if height  50+_20 light green
    digitalWrite(led_green, HIGH);
    digitalWrite(led_red, LOW);
    digitalWrite(led_yellow, LOW);  
    }
  if(height_mm>70) {              //if Too high light red
    digitalWrite(led_red, HIGH);
    digitalWrite(led_green, LOW);
    digitalWrite(led_yellow, LOW);
       }
  if(height_mm<29){             //if low light yellow
    digitalWrite(led_yellow, HIGH);
    digitalWrite(led_red, LOW);
    digitalWrite(led_green, LOW);
    }
}
 



int target_height_check(){    //function to calculate optional  statistics 
  if((values>0)||(height_mm>50)   ){    //runs after target height is reached 
      values++; //records the amount of times loop has run
      target_time+=((1-((1%values)))*millis()); //only records the first time (faster than an if statment)
      avarage_deviation= (avarage_deviation*(values-1)+ abs(50-height_mm))/values; //kees log of ram avarage deviation
      if(abs(50-height_mm)>max_deviation){    //checks if height is maxima/minima
        max_deviation=abs(50-height_mm);      //updates maximia/ minima
      }
}
}