//**** 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
//*********************************************************************************

//**** SETUP LED PINS ************** 
const int startup_LED = 2;
const int scanning_LED = 3;
const int scan_end_LED = 4;
//*********************************************************************************
const int potentiometer = A0;
const float m = -0.1227; // Scaling constant
const float b = 68.18; // Offset constant

float calculateAngle(int voltage) {
  return m * voltage + b;


}
void setup() {
  
  //**** SETUP MOTORS ************** LEAVE THESE LINES UNMODIFIED! ******************
  motor_Left.attach(10); //Attach the left motor controller
  motor_Right.attach(9); //Attach the right motor controller
  delay(300);
  //Initialise motors
  motor_Left.writeMicroseconds(1000); 
  motor_Right.writeMicroseconds(1000);
  delay(2000); //wait 2s to initialise the controller
  //*********************************************************************************

  //**** SETUP LEDS ************** 
  pinMode(startup_LED, OUTPUT); // Set the startup LED as an output
  pinMode(scanning_LED, OUTPUT); // Set the scanning LED as an output
  pinMode(scan_end_LED, OUTPUT); // Set the scan end LED as an output
  digitalWrite(startup_LED, HIGH); // Turn on the startup LED
  delay(1000); // Wait for one second
  digitalWrite(startup_LED, LOW); // Turn off the startup LED
  //*********************************************************************************

  //Setup serial communication 
  Serial.begin(9600); // Starting serial communication at 9600 baud rate
  Serial.println("Start"); 
  pinMode(potentiometer, INPUT);

 motor_Left.write(0); //MOTOR OFF (use 180 for full power)
  motor_Right.write(0); //MOTOR OFF (use 180 for full power)

 
}

void loop() {
  


digitalWrite(scanning_LED, HIGH); //Turn on scanning LED
  
  int sensor_value = analogRead(potentiometer); //Read the value from the potentiometer connected to the A0 pin
  float angle = calculateAngle(sensor_value); //Calculate the angle from the voltage read from the potentiometer
  Serial.print("Angle: ");
  Serial.println(angle); //Print the angle in the serial monitor
  
  //Calculate motor control signal
  int motor_power = map(angle, -22, 0, 45, 180);
  Serial.print("Motor Power: ");
  Serial.println(motor_power);
  
  //Send motor control signal to motors
  motor_Left.write(motor_power);
  motor_Right.write(motor_power);

  delay(100); 
  

}