Commit 4fa0a4e5 authored by ChristineC8's avatar ChristineC8
Browse files

Added Final Report and cleaned up PID&test code

parent b8606fa9
...@@ -29,8 +29,8 @@ ...@@ -29,8 +29,8 @@
// TODO: add approprioate timing for cycle time // TODO: add approprioate timing for cycle time
#define CYCLE_TIME 0.02 // The amount of time between interrupts in s #define CYCLE_TIME 0.02 // The amount of time between interrupts in s
#define BUFFER_SIZE 50 #define BUFFER_SIZE 50
#define KI 0.005 // PID Integral Component gain #define KI 0.0475 // PID Integral Component gain
#define KP 0.005 // PID Position Component gain #define KP 0.0025 // PID Position Component gain
static float prior_integral = 0; // TODO: maintain bounds on this integral so it doesn't loop around, and figure out when to reset to 0!! static float prior_integral = 0; // TODO: maintain bounds on this integral so it doesn't loop around, and figure out when to reset to 0!!
static float error_buffer[BUFFER_SIZE]; // should init to zeros static float error_buffer[BUFFER_SIZE]; // should init to zeros
static int oldest_elem_index = 0; static int oldest_elem_index = 0;
...@@ -59,15 +59,6 @@ float pid_update_pwm(float current_angle, float degrees_C){ ...@@ -59,15 +59,6 @@ float pid_update_pwm(float current_angle, float degrees_C){
float diff = abs(current_angle - degrees_C); float diff = abs(current_angle - degrees_C);
float duty = 0; float duty = 0;
// // Position Component of PID Calculations
// if (diff < ERR_THRESHOLD_DEGREES) {
// duty_p = 0;
// } else if (diff < BEGIN_SLOW_DEGREES){
// duty_p = ((diff - ERR_THRESHOLD_DEGREES)/(BEGIN_SLOW_DEGREES-1)) * MAX_DUTY;
// } else {
// duty_p = MAX_DUTY;
// }
// Integral Component of PID Calculations // Integral Component of PID Calculations
// Get rid of the oldest error and put in the newest error // Get rid of the oldest error and put in the newest error
...@@ -85,9 +76,7 @@ float pid_update_pwm(float current_angle, float degrees_C){ ...@@ -85,9 +76,7 @@ float pid_update_pwm(float current_angle, float degrees_C){
float tempduty = (KP * diff) + (KI * integral); float tempduty = (KP * diff) + (KI * integral);
printf("Proposed pre-clamped duty cycle: %.5f given calculated diff: %.5f, and calculated integral: %.5f\n", (double)tempduty, (double) diff, (double) integral); printf("Proposed pre-clamped duty cycle: %.5f given calculated diff: %.5f, and calculated integral: %.5f\n", (double)tempduty, (double) diff, (double) integral);
// TODO make this dependent on error -- KP & KI are scaled to Max_duty // Bound the output of the controller to our chosen min + max duty cycles
// bound the output of the controller to our chosen min + max duty cycles
if ( tempduty > MAX_DUTY){ if ( tempduty > MAX_DUTY){
duty = MAX_DUTY; duty = MAX_DUTY;
} }
...@@ -100,21 +89,14 @@ float pid_update_pwm(float current_angle, float degrees_C){ ...@@ -100,21 +89,14 @@ float pid_update_pwm(float current_angle, float degrees_C){
printf("Proposed duty cycle: %.5f given calculated diff: %.5f, and calculated integral: %.5f\n", (double)tempduty, (double) diff, (double) integral); printf("Proposed duty cycle: %.5f given calculated diff: %.5f, and calculated integral: %.5f\n", (double)tempduty, (double) diff, (double) integral);
printf("Current angle: %.5f, giving diff of: %.5f from desired angle: %.5f\n", (double) current_angle, (double) diff, (double) degrees_C); printf("Current angle: %.5f, giving diff of: %.5f from desired angle: %.5f\n", (double) current_angle, (double) diff, (double) degrees_C);
// Print data into file for testing purposes
// FILE *fp;
// fp = fopen("...\\test_info.txt", "a");
// fprintf(fp, "duty: %.5f, diff: %.5f, integral: %.5f.\n", (double) duty, (double) diff, (double) integral);
// fclose(fp);
//printf("Duty cycle: %.5f\n", (double)duty);
return duty; return duty;
} }
// TODO: Rather than passing in left or right, why not just pass in 'wing' that denotes left or right?
float get_wing_angle(bool left){ ///Will take in sensor values from WING accelerometer (LEFT or RIGHT depends on boolean) float get_wing_angle(bool left){ ///Will take in sensor values from WING accelerometer (LEFT or RIGHT depends on boolean)
acceleration accel; acceleration accel;
acceleration bird = get_bird_accel(); acceleration bird = get_bird_accel();
// check if bird needs reset because all outputs are 0 // Check if bird needs reset because all outputs are 0
if (!valid_magnitude(bird)) { if (!valid_magnitude(bird)) {
return RESET_NUM_CENTER; // this error should pause the wings + reset motors return RESET_NUM_CENTER; // this error should pause the wings + reset motors
} }
...@@ -133,7 +115,7 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler ...@@ -133,7 +115,7 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
float accel_y; float accel_y;
float accel_z; float accel_z;
//accelerometer value is read (for the test done in the Python script, the pinout for the body accelerometer was used) // Accelerometer value is read (for the test done in the Python script, the pinout for the body accelerometer was used)
if (left) { //for left wing if (left) { //for left wing
accel = get_raw_accel(LEFTWING); accel = get_raw_accel(LEFTWING);
// printf("LEFT RAW: LX: %.5f, LY: %.5f, LZ: %.5f, \t", (double)accel.x, (double)accel.y, (double)accel.z); // printf("LEFT RAW: LX: %.5f, LY: %.5f, LZ: %.5f, \t", (double)accel.x, (double)accel.y, (double)accel.z);
...@@ -164,12 +146,6 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler ...@@ -164,12 +146,6 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
accel_z = 0.001; accel_z = 0.001;
} }
// TODO: how to find accel of each wing - so wing vs accel within wing
// if (wing_y == 0 || wing_z == 0) {
// //TODO: reset accel
// }
if(left) { if(left) {
accel_x -= bird.x; accel_x -= bird.x;
accel_y -= bird.y; accel_y -= bird.y;
...@@ -202,15 +178,15 @@ void set_wing_angle(bool left, int degrees_C){ ...@@ -202,15 +178,15 @@ void set_wing_angle(bool left, int degrees_C){
if (float_equality(current_angle, ERROR_HALT)) { if (float_equality(current_angle, ERROR_HALT)) {
set_pwm(left, UP, 0); set_pwm(left, UP, 0);
if(left) { // if(left) {
// printf("Current_angle: %.5f, left wing error; halting\t", (double)current_angle); // // printf("Current_angle: %.5f, left wing error; halting\t", (double)current_angle);
// acceleration accel = get_raw_accel(LEFTWING); // // acceleration accel = get_raw_accel(LEFTWING);
// printf("LEFT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z); // // printf("LEFT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}else { // }else {
// printf("Current_angle: %.5f, right wing error; halting\n", (double)current_angle); // // printf("Current_angle: %.5f, right wing error; halting\n", (double)current_angle);
// acceleration accel = get_raw_accel(RIGHTWING); // // acceleration accel = get_raw_accel(RIGHTWING);
// printf("RIGHT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z); // // printf("RIGHT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z);
} // }
return; return;
} else if (float_equality(current_angle, RESET_NUM_WING)) { } else if (float_equality(current_angle, RESET_NUM_WING)) {
......
...@@ -56,10 +56,10 @@ def plot_data(y_data, x_label, y_label, title) : ...@@ -56,10 +56,10 @@ def plot_data(y_data, x_label, y_label, title) :
# Main : # Main :
# find and print data # find and print data
text_file = "/Users/cc/Documents/School/ee185/software/firmware/circuitpython-main/ports/atmel-samd/common-hal/motor_control/PID_testing/pid_scripts/pid_KI_0.005_KP_0.005.txt" text_file = "/ee185/software/firmware/circuitpython-main/ports/atmel-samd/common-hal/motor_control/PID_testing/pid_scripts/pid_KI_0.005_KP_0.005.txt"
data_label = "duty_left: " data_label = "duty_right: "
data_list = extract_data(text_file, data_label) data_list = extract_data(text_file, data_label)
title = "Calculated PID Left Wing Duty Cycles: KI = 0.04375 & KP = 0.00625" title = "Calculated PID Right Wing Duty Cycles: KI = 0.005 & KP = 0.005"
y_title = "Calculated Left Wing Duty Cycle" y_title = "Calculated Right Wing Duty Cycle"
plot_data(data_list[0:10000], "Time Interval (20 ms)", y_title, title) plot_data(data_list[0:10000], "Time Interval (20 ms)", y_title, title)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment