/* 
--------------------------------------------  
-------- MECH1010 Coursework 2023 --------
-------- Name:  --------
-------- Username:  --------
--------------------------------------------
*/

//**** SETUP LIBRARY ************** LEAVE THESE LINES UNMODIFIED! ******************
#include <Servo.h>

// Set up variables and constants
const int LEFT_MOTOR_PIN = 10;
const int RIGHT_MOTOR_PIN = 9;
const int ANGLE_SENSOR_PIN = A0;
const int STARTUP_LED_PIN = 2;
const int SCANNING_LED_PIN = 3;
const int SCAN_END_LED_PIN = 4;
const int MOTOR_OFF_SIGNAL = 45;
const int MOTOR_FULL_POWER_SIGNAL = 85;
const int ANGLE_SENSOR_READ_DELAY_MS = 20;
const int TARGET_ANGLE = 0;
const int ANGLE_TOLERANCE = 5;
const int SCAN_TIME_MS = 5000;

int currentAngle = 0;
int motorSignal = 0;
unsigned long lastScanTime = 0;

// Initialize the system
void setup() {
  Serial.begin(9600); // Set up serial communication
  Serial.println("0. System Started");
  pinMode(LEFT_MOTOR_PIN, OUTPUT);
  pinMode(RIGHT_MOTOR_PIN, OUTPUT);
  pinMode(STARTUP_LED_PIN, OUTPUT);
  pinMode(SCANNING_LED_PIN, OUTPUT);
  pinMode(SCAN_END_LED_PIN, OUTPUT);
  
  digitalWrite(LEFT_MOTOR_PIN, MOTOR_OFF_SIGNAL);
  digitalWrite(RIGHT_MOTOR_PIN, MOTOR_OFF_SIGNAL);

}

// Main control loop
void loop() {
  // Read current angle
  currentAngle = readAngle();
  
  
}

// Read angle from sensor
int readAngle() {
  int sensorValue = analogRead(ANGLE_SENSOR_PIN);
  int angle = map(sensorValue, 371, 729, -22, 22); // Convert sensor value to angle
  //angle=constrain(angle, -22,22);
  delay(ANGLE_SENSOR_READ_DELAY_MS);
  
Serial.print(sensorValue);
Serial.print("    ");
Serial.println(angle);
  return -angle;
}