/*
--------------------------------------------
-------- MECH1010 Coursework 2024 --------
-------- Name: Amir Ali
-------- Username: mn23aaga
--------------------------------------------
*/
//Assign LED, potentiometer, motor to pins.
int Green_LED = 2;
int Yellow_LED = 3;
int Red_LED = 4;
int output_vpot = A0;
int Hbridge_enable = 10;
int Hbridge_direction = 9;
//other variables
unsigned long start_stopwatch = 0;
float target_height = 50;
// Name given variables and assign values(Rod)
float Rod = 491;
//Ensure the controller is running at 25Hz by making variable loopTime
// 1/25Hz = 0.04sec, 0.04s = 40s
int loopTime = 40;
void setup() {
Serial.begin(9600);
//Assign pin modes
pinMode(2,OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(A0, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
//Light LEDS for 0.5 seconds to indicate start
digitalWrite(Green_LED, HIGH);
digitalWrite(Yellow_LED, HIGH);
digitalWrite(Red_LED, HIGH);
delay(500);
//Turn off after the 0.5 seconds
digitalWrite(Green_LED, LOW);
digitalWrite(Yellow_LED, LOW);
digitalWrite(Red_LED, LOW);
//Start a timer to record the time from the start of the loop
//start_stopwatch = millis();
}
void loop() {
delay(40);
float start_stopwatch = millis();
//turn on the drone
digitalWrite(10, HIGH);
analogWrite(10, 161);
//read the potentiometer
int drone_angle_position = analogRead(A0);
//Map angle given values (212 corresponds to -68.5, 410 corresponds to 0)
float current_angle = map(drone_angle_position, 212, 410, -68.5, 0);
//Calculate the drones current height from potentiometer
float drone_current_height = (Rod * sin(current_angle));
//Calculate the erorr
float erorr = (target_height - drone_current_height);
// Print all the calculated values (relay telemtry)
Serial.print("Time seconds ");
Serial.println(start_stopwatch/1000);
Serial.print("Current angle degrees ");
Serial.println(current_angle);
Serial.print("height millimeters ");
Serial.println(drone_current_height);
Serial.print("Erorr millimeters ");
Serial.println(erorr);
Serial.println();
//Set up lights to indicate wether drone is at correct height
if (erorr >= 20 && erorr <= 20) {
digitalWrite(2, HIGH); // Green LED ON
digitalWrite(3, LOW);
digitalWrite(4, LOW);}
else if (erorr < -20) {
digitalWrite(2, LOW);
digitalWrite(3, HIGH); // Yellow LED ON
digitalWrite(4, LOW);}
else {
digitalWrite(2, LOW);
digitalWrite(3, LOW);
digitalWrite(4, HIGH); // Red LED ON
}
if (start_stopwatch > 10000) {
//digitalWrite(10, LOW);
digitalWrite(9, HIGH);
landing();
stop_motor();
while (1);
}
}
void landing() {
digitalWrite(9, HIGH);
analogWrite(10,20);
}
void stop_motor() {
delay (4000);
digitalWrite(10, LOW);
//Light LEDS for 1 second to indicate shutdown
digitalWrite(Green_LED, HIGH);
digitalWrite(Yellow_LED, HIGH);
digitalWrite(Red_LED, HIGH);
delay(1000);
//Turn off after the 1 seconds
digitalWrite(Green_LED, LOW);
digitalWrite(Yellow_LED, LOW);
digitalWrite(Red_LED, LOW);
}