/* 
--------------------------------------------  
-------- MECH1010 Coursework 2024 --------
-------- Name: Benjamin Chadwick --------
-------- Username: mn23bc --------
--------------------------------------------
*/
//initialise variables
int h_direction = 9; //H-bridge direction
int h_enable = 10; //H-bridge on/off

int pot_voltage = A0; //potentiometer output voltage

//LEDs used to indicate the status of the control system/error
int LED_green = 2; //error less than +- 20mm
int LED_yellow = 3; // error <- 20mm
int LED_red = 4; // error >+ 20mm

//drone location variables
int pot_value = 212;
int drone_angle = -68.5;
int drone_height = 0;
int height_dif = 0;
int target_height = 506.835;
//drone height at 0 degrees
int drone_norm = 456.8350259;

//Simulation constants
int rod_length = 491; // (mm) length of rod attatched to "drone"

//Motor speed
float motor_speed = 5;

//Time variables
unsigned long final_time = 0;
unsigned long initial_time = 0;
unsigned char time_dif = 0;
///introduce intelligent timing structure

//loop length in ms
int loop_length = 40;

void setup() {
	//Initialize serial communication
  Serial.begin(9600);
  
  //Configure pins as an input or an output
  pinMode(h_direction, OUTPUT);
  pinMode(h_enable, OUTPUT);
  pinMode(pot_voltage, INPUT);
  pinMode(LED_green, OUTPUT);
  pinMode(LED_yellow, OUTPUT);
  pinMode(LED_red, OUTPUT);
	
  //show that program has started by turning all LEDs on for 0.5s
  digitalWrite(LED_green, HIGH);
  digitalWrite(LED_yellow, HIGH);
  digitalWrite(LED_red, HIGH);
  delay(500);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_yellow, LOW);
  digitalWrite(LED_red, LOW);

  //Switch motor on
  digitalWrite(h_enable,HIGH);

//map potentiometer
map(pot_voltage,0,1023,212,426.8942535);//target voltage 426.89426
}



void loop() {
  //ensure the controller is running at 25Hz by adding a 40ms delay
  delay(time_dif);

  //calculate time that this loop started
  initial_time = millis();

  //Read the potentiometer to determine the drone's angle
  pot_value = analogRead(pot_voltage);
  drone_angle = ((pot_value*(137/396))-141.8434343); //determined linearly (equ calculated elsewhere)
  //Convert to radians for use in sine function
  drone_angle =  drone_angle*((PI)/180);

  //Now calculate drone height
  drone_height = (rod_length*sin(drone_angle)) + drone_norm;

  //calculate difference in drone height compared to target
  height_dif = drone_height - target_height;


  Serial.print(drone_height);

//If loop adjusting speed dependent on height
if (height_dif < -250)  {
motor_speed = motor_speed + 1;
digitalWrite(h_enable, motor_speed);
digitalWrite(LED_green, LOW);
digitalWrite(LED_yellow, HIGH);
digitalWrite(LED_red, LOW); 
}
else if (height_dif < -100) {
  motor_speed = motor_speed + 0.5;
  digitalWrite(h_enable, motor_speed);
  digitalWrite(LED_green, LOW);
digitalWrite(LED_yellow, HIGH);
digitalWrite(LED_red, LOW);
}
else if (height_dif < -20) {
  motor_speed = motor_speed + 0.25;
  digitalWrite(h_enable, motor_speed);
  digitalWrite(LED_green, LOW);
digitalWrite(LED_yellow, HIGH);
digitalWrite(LED_red, LOW);
}
else if (height_dif >= -20 && height_dif <= 20) {
  digitalWrite(h_enable, motor_speed);
  digitalWrite(LED_green, HIGH);
  digitalWrite(LED_yellow, LOW);
  digitalWrite(LED_red, LOW);
}
else if (height_dif > 20) {
  motor_speed = motor_speed - 0.25;
  digitalWrite(h_enable, motor_speed);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_yellow, LOW);
  digitalWrite(LED_red, HIGH);
}
else if (height_dif > 100){ 
  motor_speed = motor_speed - 0.5;
  digitalWrite(h_enable, motor_speed);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_yellow, LOW);
  digitalWrite(LED_red, HIGH);
}
else if (height_dif > 250) {
  motor_speed = motor_speed - 1;
  digitalWrite(h_enable, motor_speed);
  digitalWrite(LED_green, LOW);
  digitalWrite(LED_yellow, LOW);
  digitalWrite(LED_red, HIGH);
} 
else {
}
//calculate how long the loop has run for and the delay needed for a 25Hz loop
final_time = millis();
time_dif = final_time - initial_time;
time_dif = loop_length - time_dif;
}