Commit 6edbb7fe authored by Vidisha Srivastav's avatar Vidisha Srivastav
Browse files

Merge branch 'integral_testing' into motor-control-loop

parents 89fdb649 b80e01c4
......@@ -61,6 +61,9 @@ static int left_wing_degrees = 0;
static int right_wing_degrees = 0;
extern uint64_t common_hal_time_monotonic_ns(void);
static uint64_t prev_ns = 0;
//Interrupt handler function for performing PID calculation
void control_interrupt_handler(uint8_t index) {
if (index != control_tc_index) return;
......@@ -68,6 +71,12 @@ void control_interrupt_handler(uint8_t index) {
Tc* tc = tc_insts[index];
if (!tc->COUNT16.INTFLAG.bit.MC0) return;
// calculating interrupt interval
uint64_t ns = common_hal_time_monotonic_ns();
printf("Interrupt interval: %lld\n", ns - prev_ns); // TODO: C90 can't print long long ints for some reason, ask Phil for set interrupt length
prev_ns = ns;
set_wing_angle(LEFT,left_wing_degrees); //For setting the LEFT wing angle (degrees is a global variable whose value originates from the Python function control.tasks(degrees))
set_wing_angle(RIGHT, right_wing_degrees); //For setting the RIGHT wing angle (angle needs a global variable from Python script)
......
......@@ -6,22 +6,30 @@
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>
// PWM control constants
#define ERR_THRESHOLD_DEGREES 2
#define BEGIN_SLOW_DEGREES 5
#define MAX_DUTY 0.9 //Set to PWM %high that represents max speed we want the wings to move
#define MIN_DUTY 0.0
#define UP true
#define DOWN false
#define LEFT true
#define RIGHT false
// Error case definitions
#define CALC_ERROR_NUM (float) -5000
#define ERROR_HALT (float) 5000.0
#define RESET_NUM_CENTER -1000.0
#define RESET_NUM_WING (float) 1000.0 // this error should pause the wings + reset motors
#define EPSILON 0.005 // The threshold in which two floats are conisdered equal
#define CYCLE_TIME 20 // The amount of time between interrupts in ms
#define BUFFER_SIZE 50
// PID controller constants
// TODO: add approprioate timing for cycle time
#define KI 0.1 // PID Integral Component gain
#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
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
......@@ -47,6 +55,60 @@ bool valid_magnitude(acceleration accel){
return (mag > 81 && mag < 121);
}
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
float integral = prior_integral + (diff * CYCLE_TIME) - (error_buffer[oldest_elem_index] * CYCLE_TIME);
prior_integral = integral;
// Update the buffer with the newest error value
error_buffer[oldest_elem_index] = diff;
oldest_elem_index += 1;
if (oldest_elem_index > BUFFER_SIZE) {
oldest_elem_index = 0;
}
// Integrate P and I components of PID
float tempduty = (KP * diff) + (KI * integral);
printf("Proposed pre-clamped duty cycle: %f given calculated diff: %f, and calculated integral: %f\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
if ( tempduty > MAX_DUTY){
duty = MAX_DUTY;
}
else if (tempduty < MIN_DUTY){
duty = MIN_DUTY;
}
else{
duty = tempduty;
}
printf("Proposed duty cycle: %f given calculated diff: %f, and calculated integral: %f\n", (double)tempduty, (double) diff, (double) integral);
printf("Current angle: %f, giving diff of: %f from desired angle: %f\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: %f, diff: %f, integral: %f.\n", (double) duty, (double) diff, (double) integral);
// fclose(fp);
//printf("Duty cycle: %f\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;
......@@ -63,7 +125,7 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
bird.z < 9.0 || bird.z > 11.0) {
// Return non-possible number because bird acceleration does not make sense (garbage value)
printf("CENTER RAW: %f %f %f \t", (double)bird.x, (double)bird.y, (double)bird.z);
// printf("CENTER RAW: %f %f %f \t", (double)bird.x, (double)bird.y, (double)bird.z);
return ERROR_HALT;
}
......@@ -73,11 +135,11 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
//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: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
} else { //for right wing
accel = get_raw_accel(RIGHTWING);
printf("RIGHT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
accel = get_raw_accel(LEFTWING);
// printf("LEFT RAW: LX: %f, LY: %f, LZ: %f, \t", (double)accel.x, (double)accel.y, (double)accel.z);
} else { //for right wing
accel = get_raw_accel(RIGHTWING);
// printf("RIGHT RAW: RX: %f, RY: %f, RZ: %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}
accel_x = accel.x; //X acceleration value from wing sensor
accel_y = accel.y; //Y acceleration value from wing sensor
......@@ -93,7 +155,7 @@ float get_wing_angle(bool left){ ///Will take in sensor values from WING acceler
// checking if wings are past -90/90 degrees
if (accel_z < 0.0){
// past setting: set_pwm(LEFT, UP, 0);
printf("Wing is past 90deg");
// printf("Wing is past 90deg");
return ERROR_HALT;
}
......@@ -141,11 +203,11 @@ void set_wing_angle(bool left, int degrees_C){
set_pwm(left, UP, 0);
if(left) {
printf("Current_angle: %f, left wing error; halting\t", (double)current_angle);
// printf("Current_angle: %f, left wing error; halting\t", (double)current_angle);
// acceleration accel = get_raw_accel(LEFTWING);
// printf("LEFT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}else {
printf("Current_angle: %f, right wing error; halting\n", (double)current_angle);
// printf("Current_angle: %f, right wing error; halting\n", (double)current_angle);
// acceleration accel = get_raw_accel(RIGHTWING);
// printf("RIGHT RAW: %f %f %f \t", (double)accel.x, (double)accel.y, (double)accel.z);
}
......@@ -172,7 +234,7 @@ void set_wing_angle(bool left, int degrees_C){
}else {
printf("right wing reset\n");
}
return;
}
......@@ -181,7 +243,7 @@ void set_wing_angle(bool left, int degrees_C){
common_hal_time_delay_ms(1);
accel_reset(CENTER);
printf("bird accel reset\n");
// printf("bird accel reset\n");
}
// Check for more errors.
......@@ -189,61 +251,34 @@ void set_wing_angle(bool left, int degrees_C){
set_pwm(left, UP, 0);
if(left) {
printf("left wing out of bounds; halting\n");
// ("left wing out of bounds; halting\n");
}else {
printf("right wing out of bounds; halting\n");
// printf("right wing out of bounds; halting\n");
}
return;
}
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
float integral = prior_integral + (diff * CYCLE_TIME) - (error_buffer[oldest_elem_index] * CYCLE_TIME);
prior_integral = integral;
// Update the buffer with the newest error value
error_buffer[oldest_elem_index] = diff;
oldest_elem_index += 1;
if (oldest_elem_index > BUFFER_SIZE) {
oldest_elem_index = 0;
}
// Integrate P and I components of PID
duty = (KP * diff) + (KI * integral);
// TODO make this dependent on error -- KP & KI are scaled to Max_duty
float duty = pid_update_pwm(current_angle, degrees_C);
// Execute the calculated duty cycle on given wing
if (current_angle < degrees_C){
if(left){ //send the PWM signal to move the left wing up
printf("LEFT UP: Goal: %d, current: %f \t", degrees_C, (double)current_angle);
printf("LEFT UP: GoalLeft: %d, current_angle_left: %f, duty_left: %f \t", degrees_C, (double)current_angle, (double) duty);
set_pwm(LEFT,UP,duty);
}
else{ //send the PWM signal moves the right wing up
printf("RIGHT UP: Goal: %d, current: %f \n", degrees_C, (double)current_angle);
printf("RIGHT UP: GoalRight: %d, current_angle_right: %f, duty_right: %f \n", degrees_C, (double)current_angle, (double)duty);
set_pwm(RIGHT,UP,duty);
}
} else {
if(left){ //send a PWM signal to stop the left wing (TODO: program this so that it moves the left wing down)
printf("LEFT DOWN: Goal: %d, current: %f\t", degrees_C, (double)current_angle);
printf("LEFT STOP DOWN: Goal: %d, current_stop_angle_left: %f, duty_stop_left: %f\t", degrees_C, (double)current_angle, (double) duty);
set_pwm(LEFT, DOWN, duty);
}
else{ //send a PWM signal to stop the right wing (TODO: program this so that it moves the right wing down)
printf("RIGHT DOWN: Goal: %d, current: %f\n", degrees_C, (double)current_angle);
printf("RIGHT STOP DOWN: Goal: %d, current_stop_angle_right: %f, duty_stop_right: %f\n", degrees_C, (double)current_angle, (double) duty);
set_pwm(RIGHT, DOWN, duty);
}
}
......
# # from matplotlib import pyplot as plt
f = open("/Users/cc/Documents/School/Pid.txt", "r")
count = 0
duty_list = []
diff_list = []
while True:
count += 1
# Get next line from file
line = f.readline()
# if line is empty
# end of file is reached
if not line:
break
# print("Line{}: {}".format(count, line.strip()))
# Find duty cycle data
duty_cycle_index = line.find('Proposed duty cycle: ')
# print("result: ", duty_cycle_index)
# print(type(line))
if duty_cycle_index != -1 :
# print(line[duty_cycle_index + 21: duty_cycle_index + 30])
duty_list.append(float(line[duty_cycle_index + 21: duty_cycle_index + 30]))
# Find diff data
diff_index = line.find('calculated diff: ')
if diff_index != -1 :
# print("diff: ", line[diff_index + 17: diff_index + 26])
diff_list.append(float(line[diff_index + 17: diff_index + 25]))
print(diff_list)
# plt.plot(duty_list)
# plt.show()
f.close()
\ No newline at end of file
Markdown is supported
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