/* ---------------------------------------
-------- MECH1010 Coursework 2023 --------
---------- Name: Daniel Fidler -----------
----------- Username: mn22d2f ------------
--------------------------------------- */

// Initialising the servo motor variables:
#include <Servo.h> 	// Include the Servo library (for communication with motor controllers)
Servo motor_Left;   // Declares a name for the left motor
Servo motor_Right;  // Declares a name for the right motor

// Initialise intput / output pins:
const int pot_Pin = A0; 			// Mapping the potentiometer reading to analog pin 1
const int start_LED = 2; 			// Mapping the green starting indicator LED to digital pin 2
const int scanning_LED = 3; 	// Mapping the yellow scanning indicator LED to digital pin 3
const int shutdown_LED = 4; 	// Mapping the red shutdown indicator LED to digital pin 4

// Initialise control variables:
int pot_Val = 0;							// Output voltage of the potentiometer, as read by the ADC 
float angle = 0.00;						// The potenteometer voltage, scaled to represent the physical angle of the propeller arm
float error = 0;							// The absolute distance away from the target angle for scanning (+/- 5 degrees)
const float gain = (0.345);		// A proportional gain value FOUND EXPERIMENTALLY THROUGH TRIAL AND ERROR
float control_Signal = 0.00;	// Control signal to be used in adjusting the signal sent to each motor
int motor_Left_Signal = 65;		// The signal sent to the LEFT motor - 45 = off, 65 = 50% activation, 85 = full power
int motor_Right_Signal = 65;	// The signal sent to the RIGHT motor - 45 = off, 65 = 50% activation, 85 = full power


// Setup function: Initalises serial comms, indicates the system is starting, and then initialises the motors
void setup() {

	Serial.begin(9600);	// Initialising serial communication at 9600 bits per second

	Serial.println("0.System Started");	// Print the program position to serial communication or "BlackBox datafile"
	
	initialise_Motors();	// Call the user defined function initialise_Motors

}


void loop() {

	/* Deploying the default motor control signal and waiting for 2 seconds
	This allows the physical system to settle at its uncorrected angle before starting to adjust the power of each motor */
	motor_Left.write(motor_Left_Signal);
	motor_Right.write(motor_Right_Signal);
	delay(2000);

	start_LED_Flash(); // Calls the user defined funcion which flashes the start LED
	Serial.println("1.System Initiated");


	/* Closed system proportional control loop: 
	Uses a proportional gain value to continuously adjust the motor speed and correct the propeller arm towards horizontal
	Detects the first instance the angle of tilt is less than 5 degrees from horizontal
	Determines when the scan is complete, five seconds after the angular deviation first decreases below 5 degrees
	*/

	// Initializing the variables which track the progress of the system towards completion of the scan:
	bool scan_Complete = false; // The boolean which exits the control loop when the scan is completed
	int scan_Progress = 0;			// The timing variable which tracks how long the scan has been activated for

	// Declaring local variables to be used in the control loop:
	// First set of variables will be used to calculate the current power output for each motor as a percentage
	float motor_Left_Fraction = 0;
	int motor_Left_Percentage = 0;
	float motor_Right_Fraction = 0;
	int motor_Right_Percentage = 0;

	// Second set of variables allow the timing of the control loop:
	unsigned long current_Time = 0;					// Current time at any given postiion in the control loop
	float current_Time_Secs = 0;						// Current time, but converted to seconds for the BlackBox datafile
	unsigned long elapsed_Time = 0;					// Elapsed time since completing the last iteration of the control loop
	unsigned long previous_Time = millis();	// Recording the initial elapsed time before entering the loop

	// Displaying program progress and data record headers in the BlackBox Datafile
	Serial.println("2.Controller Starting");
	Serial.println("Time,Angle,Error,Control Signal,Motor L,Motor R");
  
  const int loop_Start_Time = millis();

	// Entering the control loop
	while (scan_Complete == false) {

		pot_Val = analogRead(pot_Pin);	// Reading the potentiometer output value

		// Converting potentiometer value into degrees for angular deviation of the arm away from horizontal:
		if (pot_Val <= 552) {
			// The 22 degree range for when angle > 0 is covered by potenteometer outputs 371 - 522
      // Range of readings is 151 and we assume the potenteometer output increases linearly with angle
			angle = (pot_Val - 522) / (6.863636);	// Scaling the potentiometer reading by 151 / 22
		} else {
			// The 22 degree range for when angle > 0 is covered by potenteometer outputs 522 - 729
      // Range of readings is 207 and we assume the potenteometer output increases linearly with angle.    
			angle = (pot_Val - 522) / (8.045455); // Scaling the potentiometer reading by 207 / 22
		}

		// Checking if the angle is within 5 degrees away from horizontal, and then calculating the deviation from this target
		if (abs(angle) > 5) {
			error = abs(angle) - 5.00;
		} else {
			error = 0.00;
    }
    
		control_Signal = (gain * angle); // Scaling the angular deviation by the proportional gain value to find the control adjustment signal

		// Calculating the control adjustment for the power of the left and right motors
		motor_Left_Signal = (65 + control_Signal);
		motor_Left_Fraction = ((motor_Left_Signal - 45) / 40.00); // Calculating the activation as a %. for serial output
		motor_Left_Percentage = motor_Left_Fraction * 100; 

		motor_Right_Signal = (65 - control_Signal);
		motor_Right_Fraction = ((motor_Right_Signal - 45) / 40.00); // Calculating the activation as a %. for serial output
		motor_Right_Percentage = motor_Right_Fraction * 100.00; 

		// Sending each adjusted signal to each motor
		motor_Left.write(motor_Left_Signal); 
		motor_Right.write(motor_Right_Signal);


		/* Conditional statements to determine if scanning can begin
		Checks to see if scanning has begun already and if angular deviation is within 5 degrees */
		if ( ((angle > -5)&&(angle < 5)) && (scan_Progress == 0) ) {
			digitalWrite(scanning_LED, HIGH);	// If scanning has not begun, light scanning LED
			scan_Progress += 40;
		} else if (scan_Progress > 0) {
			scan_Progress += 40; // If scanning has already begun, just increment scan duration by 40ms
		}

		// Checking to determine if the scan has been completed (scan is completed one cycle before the value would reach 5000ms)
		if (scan_Progress >= 4960) {
			scan_Complete = true;
		}
    
		current_Time = millis();
		current_Time_Secs = ((current_Time - loop_Start_Time) / 1000.00);

		// Serial output for the "BlackBox datafile" 
		Serial.print(current_Time_Secs);				// Current elapsed time since program start
		Serial.print(",");
		Serial.print(angle);										// Current angular deviation from horizontal
		Serial.print(",");
		Serial.print(error);										// Current magnitude of angular deviation
		Serial.print(",");
		Serial.print(control_Signal);						// Current value for control signal
		Serial.print(",");
		Serial.print(motor_Left_Percentage);		// Current power (%) for L motor
		Serial.print(",");
		Serial.println(motor_Right_Percentage);	// Current power (%) for R motor

		// Measure the elapsed time and adjust the delay time accordingly to cycle through the loop at 25Hz
		current_Time = millis();
    unsigned long elapsed_Time = current_Time - previous_Time;
    if (elapsed_Time < 40) {
     delay(40 - elapsed_Time);
	 	}

		// Measuring the time of finishing the current cycle of the control loop
    previous_Time = millis();
		
	}

	// Scan is complete, switching off the scan indicator LED
	digitalWrite(scanning_LED, LOW);

	// Shutdown sequence to finish the program
	shutdown_LED_Flash();
	Serial.println("4.Shutdown");

	// 10 second delay between cycles of the scanning sequence
	delay(10000);
}

/* User defined function initialise_Motors()
Called at the beginning of the program to declare pins 9 and 10 as servo controllers
Completes the required initialisation process for the servos used */
void initialise_Motors() {

	motor_Left.attach(10); // Atttaches the left motor controller
	motor_Right.attach(9); // Attatches the right motor controller
	delay(300);

  //Initialising motors
	motor_Left.writeMicroseconds(1000); 
	motor_Right.writeMicroseconds(1000);
  delay(2000); // Waiting 2 seconds to initialise the controller

}


/* User defined function start_LED_Flash()
Flashes the green control loop start indicator for1 second */
void start_LED_Flash() {
	digitalWrite(start_LED, HIGH);
	delay(1000);
	digitalWrite(start_LED, LOW);
}


/* User defined function shutdown_LED_Flash()
Flashes the red system shutdown indicator for 1 second */
void shutdown_LED_Flash() {
	digitalWrite(shutdown_LED, HIGH);
	delay(1000);
	digitalWrite(shutdown_LED, LOW);
}