From f313f98b7e00dbb6c86129a4bb9d3a636fc864f9 Mon Sep 17 00:00:00 2001 From: LiamSmego Date: Thu, 25 Mar 2021 01:06:00 -0400 Subject: [PATCH] tvc update --- main/main.ino | 74 +++++++++++++++++++++++++-------------- main/mission_constants.hh | 22 ++++++------ main/workspace.hh | 36 +++++++++++++++---- 3 files changed, 89 insertions(+), 43 deletions(-) diff --git a/main/main.ino b/main/main.ino index 5559019..45dff1a 100644 --- a/main/main.ino +++ b/main/main.ino @@ -35,32 +35,50 @@ void main_display_matrix(Matrix param, float scale) Matrix send_tvc(Matrix u, Matrix last_u, float yaw) { //scale down angle to physical tvc limit - float input_magnitude=powf((powf(u.select(1, 1), 2)+powf(u.select(2, 1), 2)), 0.5); + float input_magnitude=sqrtf(u.values[0]*u.values[0]+u.values[1]*u.values[1]); if (input_magnitude>MAX_U) { u.scale(MAX_U/input_magnitude); } - main_display_matrix(u, RAD_2_DEG); - //send - float travel_magnitude=powf((powf(u.select(1, 1)-last_u.select(1, 1), 2)+powf(u.select(2, 1)-last_u.select(2, 1), 2)), 0.5); - if (travel_magnitude>SERVO_SPEED*ws.dt/MEGA) + //main_display_matrix(u, RAD_2_DEG); + + if(NULL_U_X_AXIS){ u.values[0]=0; } + if(NULL_U_Y_AXIS){ u.values[1]=0; } + //main_display_matrix(u, RAD_2_DEG); + + vector rotation_values {cos(yaw), -sin(yaw), sin(yaw), cos(yaw)}; + Matrix R=Matrix(2, 2, rotation_values); + u=R*u; + //main_display_matrix(u, RAD_2_DEG); + + + Matrix travel=u-last_u; + float travel_magnitude=sqrtf(travel.values[0]*travel.values[0]+travel.values[1]*travel.values[1]); + /* + if (travel_magnitudeSERVO_SPEED*ws.dt) { - u=last_u+(u-last_u).scale(SERVO_SPEED*ws.dt/MEGA/travel_magnitude); + u=last_u+travel.scale(SERVO_SPEED*ws.dt/travel_magnitude); } - main_display_matrix(u, RAD_2_DEG); + */ + //main_display_matrix(u, RAD_2_DEG); + + + //rotate input are body z axis - float gamma=yaw+BETA; - vector rotation_values {cos(gamma), -sin(gamma), sin(gamma), cos(gamma)}; - Matrix R=Matrix(2, 2, rotation_values); + rotation_values = {cos(BETA), -sin(BETA), sin(BETA), cos(BETA)}; + R.values=rotation_values; Matrix new_u=R*u; - main_display_matrix(u, RAD_2_DEG); Serial.println(" "); //convert to degrees, gear the angle, and round - if (!NULL_U_X_AXIS) {ws.tvc_x.write(round(GEAR*RAD_2_DEG*new_u.select(1, 1))+TVC_X_OFFSET);} - if (!NULL_U_Y_AXIS) {ws.tvc_y.write(round(GEAR*RAD_2_DEG*new_u.select(2, 1))+TVC_Y_OFFSET);} - + ws.tvc_x.write(round(GEAR*RAD_2_DEG*new_u.values[0])+TVC_X_OFFSET); + ws.tvc_y.write(round(GEAR*RAD_2_DEG*new_u.values[1])+TVC_Y_OFFSET); + delay(1); return u; } @@ -78,6 +96,9 @@ void setup() pinMode(0, INPUT); digitalWrite(0, LOW); + + pinMode(G_LED_PIN, OUTPUT); + digitalWrite(G_LED_PIN, HIGH); //TODO flash setup //flash.begin(9600); // begins flash chip at specified baud rate @@ -125,8 +146,9 @@ void setup() void loop() { // update the clock - ws.dt = float(micros() - ws.t_prev_cycle); // (us) time step since previous loop - ws.t_prev_cycle += ws.dt; // (us) update time for next loop + unsigned long current_time=micros(); + ws.dt = float((current_time - ws.t_prev_cycle)/MEGA); // (us) time step since previous loop + ws.t_prev_cycle = current_time; // (us) update time for next loop bno055_read_euler_hrp(&ws.euler_1); bno055_read_euler_hrp(&ws.euler_2); @@ -144,7 +166,8 @@ void loop() } else { - + ws.construct_y(); + ws.construct_x(false); } break; } @@ -159,7 +182,7 @@ void loop() else { ws.construct_y(); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors + ws.construct_x(false); } break; } @@ -167,9 +190,6 @@ void loop() { if (change_mode_to_prep_tvc(millis() > ws.next_mode_time)) { - digitalWrite(15, HIGH); - delay(1); - digitalWrite(15, HIGH); transition_to_prep_tvc(); ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I; ws.mode = PREP_TVC; @@ -177,7 +197,7 @@ void loop() else { ws.construct_y(); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors + ws.construct_x(false); } break; } @@ -188,13 +208,14 @@ void loop() transition_to_burn_baby_burn(); ws.next_mode_time=millis()+BURN_BABY_BURN_PERIOD*KILO_I; ws.mode = BURN_BABY_BURN; + digitalWrite(G_LED_PIN, LOW); } else { ws.construct_y(); ws.u=(KC*ws.x).scale(-1);//calculate input ws.last_u=send_tvc(ws.u, ws.last_u, ws.yaw); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors + ws.construct_x(false); } break; } @@ -210,19 +231,20 @@ void loop() ws.construct_y(); ws.u=(KC*ws.x).scale(-1); //calculate input ws.last_u=send_tvc(ws.u, ws.last_u, ws.yaw); - ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation using Kalman filter + ws.construct_x(KALMAN_ENABLED); } break; } case (SHUTDOWN_STABLE): { - // TODO: implement this block + ws.construct_y(); + ws.construct_x(false); break; } } Serial.println(ws.mode); - + main_display_matrix(ws.x, 1); //Serial.print(millis()); //main_display_matrix(ws.last_u, RAD_2_DEG); // TODO: add data record diff --git a/main/mission_constants.hh b/main/mission_constants.hh index 3597e79..f06c77d 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -31,6 +31,7 @@ enum Mode { SHUTDOWN_STABLE = 5 }; +const bool KALMAN_ENABLED=false; const bool NULL_U_X_AXIS=false; const bool NULL_U_Y_AXIS=false; @@ -41,7 +42,7 @@ const int STARTUP_STABLE_PERIOD=2; //15 const int COUNTDOWN_PERIOD=2; //20 const int FINAL_COUNTDOWN_PERIOD=2; const int PREP_TVC_PERIOD=3; -const int BURN_BABY_BURN_PERIOD=120; //15 +const int BURN_BABY_BURN_PERIOD=300; //15 //***************************************************************************** // HARDWARE @@ -69,11 +70,12 @@ const int IMU_1_PIN=0; const int IMU_2_PIN=0; const int R_LED_PIN=0; const int B_LED_PIN=0; -const int G_LED_PIN=0; +const int G_LED_PIN=4; // Thrust-Vector Controller (TVC) //TODO: modify values -const float GEAR = 6; // gearing ratio of the servo to the TVC //9 -const float SERVO_SPEED=1; +const float GEAR = 9; // gearing ratio of the servo to the TVC //9 +const float SERVO_SPEED=3; +const float MIN_STEP=DEG_2_RAD/GEAR; const float TVC_X_OFFSET = 100; // TODO: add description const float TVC_Y_OFFSET = 55; // TODO: add description const float MAX_U= 5*DEG_2_RAD; //maximum gimbal angle @@ -89,10 +91,10 @@ const float EULER_Y_OFFSET; //rocket physical parameters const float THRUST=10; //TODO: make exact -const float MOMENT_ARM=.265; //TODO: make exact -const float MOMENT_INERTIA_XX=1; //TODO: make exact -const float MOMENT_INERTIA_YY=1;//TODO: make exact -const float MASS=1; +const float MOMENT_ARM=.2413; //TODO: make exact +const float MOMENT_INERTIA_XX=.01984; //TODO: make exact +const float MOMENT_INERTIA_YY=.01669;//TODO: make exact +const float MASS=.611; //control constants TODO: fill these out const vector A_VALUES {0,THRUST/MASS,0,0,0,0, @@ -111,8 +113,8 @@ const vector B_VALUES {THRUST/MASS, 0, 0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix const Matrix B=Matrix(6, 2, B_VALUES); -const vector K_VALUES {-2.582,-6.9449,-.5665,0,0,0, - 0,0,0,2.582,-6.9449,-.5665}; //controller gain +const vector K_VALUES {-.6325,-5.5608,-1.4869,0,0,0, + 0,0,0,.6325,-5.5608,-1.4869}; //controller gain const Matrix KC=Matrix(2, 6, K_VALUES); const vector C_VALUES {1,0,0,0,0,0, diff --git a/main/workspace.hh b/main/workspace.hh index 456f8a9..0ccd4c0 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -60,11 +60,11 @@ class Workspace vector initialize_4 {0, 0, 0, 0}; vector initialize_2 {0, 0}; Matrix x=Matrix(6, 1, initialize_6); + Matrix x_raw=Matrix(6, 1, initialize_6); Matrix y=Matrix(4, 1, initialize_4); + Matrix last_y=Matrix(4, 1, initialize_4); Matrix u=Matrix(2, 1, initialize_2); Matrix last_u=Matrix(2, 1, initialize_2); - vector y_values {0, 0, 0, 0}; - vector last_y_values {0, 0, 0, 0}; float yaw; // state @@ -75,6 +75,8 @@ class Workspace void construct_y() { + last_y.values=y.values; + theta_0[2]=float(euler_1.h/16.00); theta_1[2]=float(euler_2.h/16.00); theta_0[1]=float(euler_1.r/16.00); @@ -82,12 +84,32 @@ class Workspace theta_0[0]=float(euler_1.p/16.00); theta_1[0]=float(euler_2.p/16.00); - y_values[0]=0; - y_values[1]=(theta_0[1]+theta_1[1])/2.0*DEG_2_RAD; - y_values[2]= 0; - y_values[3]= (theta_0[0]+theta_1[0])/2.0*DEG_2_RAD; + y.values[0]=0; + y.values[1]=(theta_0[1]+theta_1[1])/2.0*DEG_2_RAD; + y.values[2]= 0; + y.values[3]= (theta_0[0]+theta_1[0])/2.0*DEG_2_RAD; yaw=(theta_0[2]+theta_1[2])/2.0*DEG_2_RAD; - y.redefine(y_values); + } + + void construct_x(bool kalman) + { + x_raw.values[0]=0; + x_raw.values[1]=y.values[1]; + x_raw.values[2]=(y.values[1]-last_y.values[1])/dt; + x_raw.values[3]=0; + x_raw.values[4]=y.values[3]; + x_raw.values[5]=(y.values[3]-last_y.values[3])/dt; + + if (kalman) + { + x=x+(A*x+B*last_u+L*(y-(C*x))).scale(dt); + x.values[0]=0; + x.values[3]=0; + } + else + { + x.values=x_raw.values; + } } };