/*
--------------------------------------------
-------- MECH1010 Coursework 2024 --------
-------- Name: Oluseye Arinoso
-------- Username: 201695440
--------------------------------------------
*/
// setting up pins
const int green_LED = 2;
const int yellow_LED = 3;
const int red_LED = 4;
const int direction_pin = 9; // direction pin seems to have no use in this piece of code
const int enable_pin = 10;
const int potentiometer = A0;
// setting up variables
float p_val = 0;
const float support_rod = 0.491;
float h_angle = 0 ;
const int ctrl_freq = 25; // set frequency
float ctrl_dt = 1000 * (1 / (float) ctrl_freq); // works out the time interval of each loop
const int uncertainty = 20; // in mm
const float target_height = 0.05;
const int start_speed = 127; // arbitrary start speed to get the helicopter moving off the ground
int motor_speed = 0;
float prev_error = 0;
float integralval = 0;
const float Kp =600;
const float Ki = -0.05;
const int size = ctrl_freq * 5; // max size of array created in code as readings are 25hz for 5 seconds
int counter = 0; // counts when target height has been reached
float target_time = 0;
float sum_errors= 0;
float MaxV= 0;
float max_post_error = 0 ;
void setup() {
delay(500);
Serial.begin(9600);
pinMode(green_LED, OUTPUT);
pinMode(red_LED, OUTPUT);
pinMode(yellow_LED, OUTPUT);
pinMode(potentiometer, INPUT);
pinMode(direction_pin, OUTPUT);
pinMode(enable_pin, OUTPUT);
Serial.println("0.System started");
// Lighting LEDs to max brightness for 0.5 secondss
analogWrite(green_LED, 255);
analogWrite(red_LED, 255);
analogWrite(yellow_LED, 255);
delay(500);
analogWrite(green_LED, 0);
analogWrite(red_LED, 0);
analogWrite(yellow_LED, 0);
motor_speed = start_speed;
analogWrite(enable_pin, motor_speed);
Serial.println("1.System initiated");
}
void loop() {
Serial.println("2.Controller starting");
Serial.println("Time , Angle , Height , Error");
// reading start of whole control system, so we can calculate when 5 seconds have passed
unsigned long start_time = millis();
unsigned long current_time = millis();
do{
// reading time to run loop at 25Hz
unsigned long loop_start = millis();
unsigned long time = loop_start - start_time;
//Function that calculates hegiht
float height = getHeight(potentiometer, support_rod);
// control system
// calculates error
float error = target_height - height;
float mm_error = error *1000;
integralval = controller(error, prev_error, enable_pin, motor_speed, Kp, Ki, ctrl_dt, integralval );
float loop_time = (float) time/1000;
Serial.print(loop_time, 2);
Serial.print(",");
Serial.print(h_angle), 2;
Serial.print(",");
// Making it so that the helicopter height starts from the ground not relative to the reference
float ground = (68.5 * PI)/180;
height = height + (support_rod * sin(ground));
Serial.print(height, 2);
Serial.print(",");
Serial.println(error, 2);
// Finds max error
if(abs(error) > MaxV){
MaxV = abs(error);
}
if( mm_error <= uncertainty && mm_error >= - uncertainty){ // in the 20mm limits set for the helicopter height
analogWrite(green_LED, 255);
analogWrite(red_LED, 0);
analogWrite(yellow_LED, 0);
;
if(counter == 0 ){ //checks that it is the first time target height is reached
target_time = loop_time;
max_post_error = abs(error);
sum_errors = sum_errors +abs(error);
counter = counter + 1;
}
}
else if(mm_error < - uncertainty ){
analogWrite(yellow_LED, 255);
analogWrite(red_LED, 0);
analogWrite(green_LED, 0);
}
else{
analogWrite(red_LED, 255);
analogWrite(green_LED, 0);
analogWrite(yellow_LED, 0);
}
// IF counter is more than 0 it means target height hsa been reached
if(counter > 0 ){
// find max error after target height is reached
if(abs(error) > max_post_error){
max_post_error = abs(error);
}
// sums all errors after target height is reached
sum_errors = sum_errors +abs(error);
counter = counter + 1; // counts through so we know how many data points there are after target height
};
// records previous error for KI controller
prev_error = error;
//This code makes sure loop is 25hz
unsigned long current_loop_time = millis();
while (current_loop_time - loop_start < ctrl_dt) {
current_loop_time = millis();
}
current_time = millis();
}while(current_time - start_time < 5000); // stops control system after 5 seconds
// switching all LEDs off briefly after control loop
analogWrite(green_LED, 0);
analogWrite(red_LED, 0);
analogWrite(yellow_LED, 0);
// Lighting LEDs to max brightness for 1 secondss
analogWrite(green_LED, 255);
analogWrite(red_LED, 255);
analogWrite(yellow_LED, 255);
delay(1000);
analogWrite(green_LED, 0);
analogWrite(red_LED, 0);
analogWrite(yellow_LED, 0);
//switching motor off
analogWrite(enable_pin, 0);
Serial.println("4.Shutdown");
Serial.print("Max Error: ");
Serial.println(MaxV, 2);
float average_error= sum_errors/ counter; // counter is how many times target height is reached
if(counter > 0){ // if target height is reached print performance statistics
Serial.print("Time taken to reach target height = ");
Serial.println(target_time, 2);
Serial.print("Max error after target reached = ");
Serial.println(max_post_error, 2);
Serial.print("Average error after target reached = ");
Serial.println(average_error, 2);
}
else{
Serial.println("Target height was not reached");
}
//making sure loop doesn't restart
delay(10000);
}
// function to get heigh of helicopter/drone
float getHeight(float potentiometerpin, float armLength){
// reads potentiometer value then maps it to angle in radians
float p_val = analogRead(potentiometerpin);
h_angle = map(p_val, 212,410, -68.5, 0);
//converts angle from degrees to radians
float rad = (h_angle * PI)/180;
float helicopter_height = armLength*sin(rad);
// helicopter_height = helicopter_height *1000; // to get height in mm
return helicopter_height;
}
//control system function
float controller(float controlError, float previousError, float output_pin, float output_speed, float KP, float KI, float T, float prevint){
// calculates intergral part of PID
float intergral = (((KI*T)/2)*(controlError + previousError)) + prevint; // p.s. I know i spelt integral wrong it's too late
Serial.println(intergral);
//Adds proportional control to oirignal speed as when target height is reached if error is zero out speed stays the same
float new_speed = (float) output_speed + (controlError * KP);
if(new_speed > 255){ // This if structure used because helicopter blades don't work properly if PWM > 255 and if above propeller speed is too fast it will overshoot the target height by a lot
new_speed = 255;
}
else if(new_speed < 0){
new_speed = 0;
}
// intergrator limit so integral doesn't get too low or too high
if(intergral < -48){
intergral = -48;
}
else if (intergral > 50){
intergral = 50;
}
new_speed = new_speed + intergral;
Serial.println(new_speed);
analogWrite(enable_pin, new_speed);
//
return intergral;
//after reading schoalrs compass i realised for our control system we do not really want a derivative term as
// retuning to set point from overshoot is not difficult or strictly undersiable
}
//finds max value of an array
float maxVal(float Array[], int sizeofarray){
float Max = 0;
for (int i = 0;i < sizeofarray ; i++) {
if ((abs(Array[i])) > Max) {
Max = Array[i];
}
Serial.print("Array i : ");
Serial.println(Array[i]);
Serial.print("Max value : ");
Serial.println(Max);
}
return Max;
}