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 @@
// TODO: add approprioate timing for cycle time
#define CYCLE_TIME 0.02 // The amount of time between interrupts in s
#define BUFFER_SIZE 50
#define KI 0.005 // PID Integral Component gain
#define KP 0.005 // PID Position Component gain
#define KI 0.0475 // PID Integral 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 error_buffer[BUFFER_SIZE]; // should init to zeros
static int oldest_elem_index = 0;
......@@ -59,15 +59,6 @@ float pid_update_pwm(float current_angle, float degrees_C){
float diff = abs(current_angle - degrees_C);
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
// 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){
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);
// 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){
duty = MAX_DUTY;
}
......@@ -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("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;
}
// 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)
acceleration 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)) {
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
float accel_y;
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
accel = get_raw_accel(LEFTWING);
// 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
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) {
accel_x -= bird.x;
accel_y -= bird.y;
......@@ -202,15 +178,15 @@ void set_wing_angle(bool left, int degrees_C){
if (float_equality(current_angle, ERROR_HALT)) {
set_pwm(left, UP, 0);
if(left) {
// printf("Current_angle: %.5f, left wing error; halting\t", (double)current_angle);
// acceleration accel = get_raw_accel(LEFTWING);
// printf("LEFT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}else {
// printf("Current_angle: %.5f, right wing error; halting\n", (double)current_angle);
// acceleration accel = get_raw_accel(RIGHTWING);
// printf("RIGHT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}
// if(left) {
// // printf("Current_angle: %.5f, left wing error; halting\t", (double)current_angle);
// // acceleration accel = get_raw_accel(LEFTWING);
// // printf("LEFT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z);
// }else {
// // printf("Current_angle: %.5f, right wing error; halting\n", (double)current_angle);
// // acceleration accel = get_raw_accel(RIGHTWING);
// // printf("RIGHT RAW: %.5f %.5f %.5f \t", (double)accel.x, (double)accel.y, (double)accel.z);
// }
return;
} else if (float_equality(current_angle, RESET_NUM_WING)) {
......
......@@ -56,10 +56,10 @@ def plot_data(y_data, x_label, y_label, title) :
# Main :
# 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"
data_label = "duty_left: "
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_right: "
data_list = extract_data(text_file, data_label)
title = "Calculated PID Left Wing Duty Cycles: KI = 0.04375 & KP = 0.00625"
y_title = "Calculated Left Wing Duty Cycle"
title = "Calculated PID Right Wing Duty Cycles: KI = 0.005 & KP = 0.005"
y_title = "Calculated Right Wing Duty Cycle"
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