//**** 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);
}