/*
--------------------------------------------
-------- mn23jf2_mech1010 --------
-------- Name: John Fairhurst
-------- Username: mn23jf2
--------------------------------------------
*/
int led_green = 2;
int led_yellow = 3;
int led_red = 4;
int h_enable = 10;
int h_direction = 9;
int pometer = A0;
//definining the purpose of each pin as global variables
int counter = 0;
int error_counter = 0;
double total_error = 0;
int first = 0;
int loop_counter = 1;
double max_error = 0;
//defining variables to be accessed from any function in the code
void setup() {
Serial.begin(9600);
delay(400);
Serial.println("0.System Started");
pinMode(led_green, OUTPUT);
pinMode(led_yellow, OUTPUT);
pinMode(led_red, OUTPUT);
pinMode(h_enable, OUTPUT);
pinMode(h_direction, OUTPUT);
pinMode(pometer, INPUT);
//defining if the pins are input and output
Serial.println("1.System Initiated");
LED_start(); //function to light all LEDS
}
void loop()
{
if(loop_counter==1) //only runs if the system hasn't run before
{
double target_time = Heli_start(); //runs the motor for the 5 seconds
digitalWrite(led_green,LOW);
digitalWrite(led_yellow,LOW);
digitalWrite(led_red,LOW);
//turns off all leds
Heli_Land();
delay(9000); //waits 9s for the helicopter to land
Serial.println("\n4. Shutdown");
Shutdown();
double average_error = (double) total_error/error_counter;
//calculates average error after target height reached
Serial.print("Average error after reaching target = ");
Serial.print(average_error);
Serial.println("m");
Serial.print("Max error after reaching target height = ");
Serial.print(max_error);
Serial.println("m");
Serial.print("Time when target height first reached = ");
Serial.print(target_time);
Serial.println("s");
//prints conclusions
loop_counter = 0; //loop counter set to 0 if loop has been ran once
}
else
{
analogWrite(h_enable,0);
}
}
void LED_start()
{
digitalWrite(led_green,HIGH);
digitalWrite(led_yellow,HIGH);
digitalWrite(led_red,HIGH);
//turning all the LEDS on
delay(500);
//waiting half a second
digitalWrite(led_green,LOW);
digitalWrite(led_yellow,LOW);
digitalWrite(led_red,LOW);//turning all the LEDs off
Serial.println("2.Controller Starting");
}
double Heli_start()
{
double target_time = 0;
Serial.println("Time,Angle,Height,Error");
unsigned long startTime = millis(); //recording the current time to compare for the 5 second duration
// creating variables for time, used to compare to 5 second running time
double timeElapsed = 0;
do
{
unsigned long motor_start = millis(); //measures start time of loop so motor is 25hz
timeElapsed = Get_Time(startTime); //function gets the time elapsed
Serial.print(timeElapsed);
Serial.print(",");
double current_height = Height_Reading(); //The height of the helicopter is determined at the start of every loop
LED_error(current_height); //uses the current height to determine LED error light
first = Motor_Control(current_height); //motor function, returns weather the target height has met for the first time
if(first == 1)
{
target_time = timeElapsed; //measures time if first time height reached
}
unsigned long motor_end = millis();
float motor_time = (float) motor_end - motor_start; //calculates time elapsed in loop so delay can be calculated
delay((40 - motor_time));
}while(timeElapsed<5);
return target_time; //time to target returned to be printed
}
double Height_Reading()
{
double pot_value = analogRead(pometer);
//Read the value from the potentiometer
//Using y=mx+c a formula can be used to convert the potentiometer reading into an angle
// Angle = 0.346*Pot_Reading - 141.84
// 90 is then added so the start position is horizontal
double angle_degrees = (0.346*pot_value) - 141.84; //angle from, potentiometer is converted into degrees
double angle_radians = angle_degrees * (PI/180); //angle converted from degrees to radians so it can be inputted into the sin function
double height = sin(angle_radians)*0.491;
//angle is converted into a height by trigonometry
Serial.print(angle_degrees);
Serial.print(",");
Serial.print(height);
Serial.print(",");
//height and angle printed for telemetry data
return height;
}
void LED_error(double current_height)
{
digitalWrite(led_green,LOW);
digitalWrite(led_yellow,LOW);
digitalWrite(led_red,LOW);
//LEDs all turned off before error indicator is shown
if(current_height>0.07)
{
digitalWrite(led_red,HIGH); // if the helicopter is above 70mm then the error is >+20mm so the red LED shows
}
else if(current_height<0.03)
{
digitalWrite(led_yellow,HIGH); //if the helicopter is below 30mm then the error is <-20mm so the yellow LED shows
}
else
{
digitalWrite(led_green,HIGH); //if the helicopter is neither above 70mm or below 30mm it must be within 20mm of the target height so the green LED shows
}
}
int Motor_Control(double current_height)
{
first=0;
double target_height = 0.05;
digitalWrite(h_direction,LOW); //the motor will only go down when the helicopter is landing
double hover_power = 25;//low power to stop overshoot when target height is reached
double add_hover = 0;
double motor_speed = 0;
double distance = 0;
//defining variables to be used in if statements
if ((current_height<(target_height-0.02)) && (current_height > -0.35))
{
distance = target_height - current_height;
add_hover = (distance/0.4)*130; //proportional calculation as move closer to target time
motor_speed = 90 + add_hover; //the power mapped is added to the hover power so the helicopter ascends
}
else if(current_height>(target_height+0.02))
{
distance = current_height - target_height; //works out the distance between the helicopter and target height
motor_speed = 10; //motor speed reduced so height drops
}
else if((current_height>(target_height-0.02)) && (current_height<(target_height+0.02)))
{
motor_speed = hover_power;
distance = abs(target_height - current_height);
}
else
{
motor_speed = 240; //if the final else is reached, the helicopter is below the ground, so max power is used
distance = abs(current_height) + target_height;
}
if((current_height>0.04) && (current_height<0.06)) //if target heigh range is reached
{
counter++;
if(counter == 1)
{
first = 1; //first set to 1 if counter shows the target height has been reached for the first time
}
}
if(counter>0)
{
error_counter++;
total_error = total_error + distance; //calculations to be used for average error
if(distance>max_error)
{
max_error = distance;
}
}
Serial.println(distance);
analogWrite(h_enable,motor_speed);
return first;
}
double Get_Time(unsigned long startTime)
{
unsigned long currentTime = millis();
double TimeElapsed = (double) currentTime - startTime; //calculates elapsed time with millis function
TimeElapsed = TimeElapsed/1000; //converts from ms to s
return TimeElapsed;
}
void Heli_Land()
{
digitalWrite(h_direction,HIGH);
analogWrite(h_enable,0); //motor stopped to drop helicopter for landing
}
void Shutdown()
{
analogWrite(h_enable,0); //motor shutdown
digitalWrite(led_green,HIGH);
digitalWrite(led_yellow,HIGH);
digitalWrite(led_red,HIGH);
//turning all the LEDS on
delay(1000);
//waiting half a second
digitalWrite(led_green,LOW);
digitalWrite(led_yellow,LOW);
digitalWrite(led_red,LOW);
//turning all the LEDs off
}