/* 
--------------------------------------------  
-------- MECH1010 Coursework 2023 --------
-------- Name:  Nana Oteng-Gyan
-------- Username:  mn22nog
--------------------------------------------
*/

//**** SETUP LIBRARY ************** LEAVE THESE LINES UNMODIFIED! ******************
#include <Servo.h> //Include the Servo library (for communication with motor controllers)
Servo motor_Left;   //Setup Left Motor
Servo motor_Right;  //Setup Right Motor
//*********************************************************************************

//Create and name the Global variables ********************************************
//LEDs
const int startLED = 2; //when this lights up, it reflects the initiation of the program
const int scanLED = 3; //when this lights up, it reflects that the drone is scanning
const int endLED = 4; //when this lights up, the program has ended
// delays (in milliseconds)
const int start_timer = 1000;
const int scan_timer = 5000;
const int end_timer = 1000;
//sensors
const int AngleSensor = A0; //this is the pin the potentiometer is connected to

void setup() {
	Serial.begin(9600); //sets up the speed of communication to 9600b/s

	//**** SETUP MOTORS ************** LEAVE THESE LINES UNMODIFIED! ******************
	motor_Left.attach(10); //Atttach the left motor controller
	motor_Right.attach(9);
	delay(300);
  //Initialise motors
	motor_Left.writeMicroseconds(1000); 
	motor_Right.writeMicroseconds(1000);
  delay(2000); //wait 2s to initialise the controller
  //*********************************************************************************
	Serial.println("System Started");
	//Initialise Pins *****************************************************************
	//input devices
	pinMode(AngleSensor,INPUT);
	//output devices
	pinMode(startLED, OUTPUT);
	pinMode(scanLED, OUTPUT);
	pinMode(endLED, OUTPUT);

	//Ensure all LEDs are off initially ********************************************
	digitalWrite(startLED, LOW); //green LED off
	digitalWrite(scanLED, LOW); //yellow LED off
	digitalWrite(endLED, LOW); //red LED off

}

void loop(){

	//turn on the green LED for 1 sec to signify the start of the programme *********
	digitalWrite(startLED, HIGH);
	delay(start_timer);
	digitalWrite(startLED, LOW);
	Serial.println("System Initiated");

	motor_Left.write(60);
	motor_Right.write(85);

	Serial.println("Controller Starting");
	balance_controller(); //code for this user defined function found at bottom outside main loop

	//turn on yellow LED for 5 secs to signify scanning *****************************
	digitalWrite(scanLED, HIGH);
	delay(scan_timer);
	digitalWrite(scanLED, LOW);

	//end the programme now that the drone has finished scanning
	digitalWrite(endLED, HIGH);
	delay(end_timer);
	digitalWrite(endLED, LOW);

	//turn off the motors
	motor_Left.write(45); //MOTOR OFF 
	motor_Right.write(45); //MOTOR OFF
	Serial.println("Shutdown");
	delay(5000);
}

//User Defined function to be called by the Main Loop *********************
void balance_controller()
{
	//Set up Local variables ********************************************************** 
	const int setpoint = 0; // ideal angle the drone should be (i.e horizontal)
	float error = 0; 
	int potreading = 0; //potentiometer reading
	float angle = 0; //angle between drone and horizon 
	float control_signal = 0; //based on the error and proportional gain
	float gain = 0.3; //used to determine the output of the motors
	int right_output = 0; //output for right motor
	float LMotor = 0; //percentage ouput of left motor
	float RMotor = 0; //percentage output of right motor
	int left_output = 0; //output for left motor
	float t_old = 0; //start time of the balance controller loop
	unsigned long t_new = 0; //final time of the balance controller loop
	int wait_time = 0; //time the do/while loop should be delayed by
	const int desired_time = 40; //this ensures that the control loop runs at 25Hz

	// Looped if statement for controller balance *****************************************
	for (int i = 0; i <= 10000; i++){

		/* I wasn't ale to print the labels for the data outputed from the balance_controller fuction, I
		tired to put it in in different places, but it froze the code and the code stopped running. So in order to make
		the code work, I didn't include it. The code I would have used would have been:
		Serial.println("Time, Angle, Error, Control Signal, Motor L, Motor R");
		*/

		t_old = millis(); //initiate the timer
		Serial.print(t_old/1000, 2); Serial.print(','); //print the time
		// Read sensor
		potreading = analogRead(AngleSensor); //reads potentiometer to angle
		angle = map(potreading, 729, 371, -22, 22); //calibrates the Vpot readings to angles
		Serial.print(angle,2); Serial.print(','); //print the angle
		
		//calculate the error and calculate the corresponding output
		error = setpoint - angle;
		Serial.print(error, 2); Serial.print(','); //print the error
		control_signal = error * gain; // calculates the output signal based on the error and gain values
		Serial.print(control_signal,2); Serial.print(','); //print the control signal

		/* the old range of the control signal was calculated using the minimum and maximum angles being -22 and 22 and the
		gain of the system to calculate the highest and lowest possile amounts of the control signal */

		left_output = map(control_signal,-22*gain,22*gain,45,85); //converts the controller signal to a control signal for the LH motor
		LMotor = (left_output - 45) * 2.5; //calculate the LH Motor output as a percentage
		Serial.print(LMotor,0); Serial.print(','); //print the left mototr output 
		
		right_output = map(control_signal,-22*gain,22*gain,85,45); //converts the controller signal to a control signal for the right motor
		RMotor = (right_output - 45) * 2.5; //calculate the RH Motor output as a percentage
		Serial.println(RMotor,0); //print the right motor output

		//set the motors to the output
		motor_Left.write(left_output); //MOTOR BETWEEN OFF AND ON
		motor_Right.write(right_output); //MOTOR BETWEEN OFF AND ON
		delay(30); // gives the motors a bit of time to reach the outputs 
	
		if ((angle < -5) || (angle > 5)){ 
			// calculate the desired wait time based on the fact that the loop should run at 25Hz
			t_new = millis(); 
			wait_time = desired_time - (t_new - t_old);
	 		delay(wait_time); //the loop continues until the condition to stop is met
		}
		else{ //else statement fulfilled if axis of drone is within 5 degrees of the horizontal
			break; //exit the loop and complete scanning	
		}
	}	
}