From 9d03cb78a66c049092e75ba85877ddf58cfad394 Mon Sep 17 00:00:00 2001 From: LiamSmego Date: Wed, 24 Mar 2021 19:54:12 -0400 Subject: [PATCH] debug tvc --- main/main.ino | 42 +++++++++++++++++++++++---------------- main/mission_constants.hh | 21 ++++++++++---------- 2 files changed, 36 insertions(+), 27 deletions(-) diff --git a/main/main.ino b/main/main.ino index 85d7d3c..5559019 100644 --- a/main/main.ino +++ b/main/main.ino @@ -18,11 +18,11 @@ using namespace std; // (--) instance of workspace class storing all the variables used in the loop Workspace ws; -void main_display_matrix(Matrix param) +void main_display_matrix(Matrix param, float scale) { for (int i=0; iMAX_U) { u.scale(MAX_U/input_magnitude); } + main_display_matrix(u, RAD_2_DEG); //send - float travel_magnitude=powf((pow(u.select(1, 1)-last_u_now.select(1, 1), 2)+pow(u.select(2, 1)-last_u_now.select(2, 1), 2)), 0.5); - if (travel_magnitude>SERVO_SPEED*ws.dt) + 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) { - u=last_u_now+(u-last_u_now).scale(SERVO_SPEED*ws.dt/travel_magnitude); + u=last_u+(u-last_u).scale(SERVO_SPEED*ws.dt/MEGA/travel_magnitude); } - *last_u=u; + 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)}; + vector rotation_values {cos(gamma), -sin(gamma), sin(gamma), cos(gamma)}; Matrix R=Matrix(2, 2, rotation_values); - u=R*u; - + Matrix new_u=R*u; + main_display_matrix(u, RAD_2_DEG); + Serial.println(" "); + //convert to degrees, gear the angle, and round - ws.tvc_y.write(round(GEAR*RAD_2_DEG*u.select(2, 1))+TVC_Y_OFFSET); - ws.tvc_x.write(round(GEAR*RAD_2_DEG*u.select(1, 1))+TVC_X_OFFSET); + 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);} + + + return u; } @@ -84,6 +89,9 @@ void setup() ws.tvc_x.attach(TVC_X_PIN); // attaches declared servo to specified pin ws.tvc_y.attach(TVC_Y_PIN); // attaches declared servo to specified pin + ws.tvc_x.write(TVC_X_OFFSET); + ws.tvc_y.write(TVC_Y_OFFSET); + //Initialize I2C communication Wire.begin(); @@ -185,7 +193,7 @@ void loop() { ws.construct_y(); ws.u=(KC*ws.x).scale(-1);//calculate input - //send_tvc(ws.u, &ws.last_u, ws.yaw); + 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 } break; @@ -201,7 +209,7 @@ void loop() { ws.construct_y(); ws.u=(KC*ws.x).scale(-1); //calculate input - //send_tvc(ws.u, &ws.last_u, ws.yaw); + 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 } break; @@ -216,7 +224,7 @@ void loop() Serial.println(ws.mode); //Serial.print(millis()); - main_display_matrix(ws.x); + //main_display_matrix(ws.last_u, RAD_2_DEG); // TODO: add data record // TODO: add (somewhere else) data struct } diff --git a/main/mission_constants.hh b/main/mission_constants.hh index 090a371..3597e79 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -31,16 +31,17 @@ enum Mode { SHUTDOWN_STABLE = 5 }; -const bool RESTRICT_U_AXIS=false; +const bool NULL_U_X_AXIS=false; +const bool NULL_U_Y_AXIS=false; //most modes change by time //after a predefined time period we switch to the next mode //MODE TIME PERIODS -const int STARTUP_STABLE_PERIOD=15; -const int COUNTDOWN_PERIOD=20; -const int FINAL_COUNTDOWN_PERIOD=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=10; +const int BURN_BABY_BURN_PERIOD=120; //15 //***************************************************************************** // HARDWARE @@ -71,12 +72,12 @@ const int B_LED_PIN=0; const int G_LED_PIN=0; // Thrust-Vector Controller (TVC) //TODO: modify values -const float GEAR = 9; // gearing ratio of the servo to the TVC -const float SERVO_SPEED=.1; -const float TVC_X_OFFSET = 85; // TODO: add description -const float TVC_Y_OFFSET = 83; // TODO: add description +const float GEAR = 6; // gearing ratio of the servo to the TVC //9 +const float SERVO_SPEED=1; +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 -const float BETA = 0.95; // angle (rad) that corrects for misalignment between body frame and TVC frame +const float BETA = 135*DEG_2_RAD; // angle (rad) that corrects for misalignment between body frame and TVC frame // TODO: define these quaternions based on IMU installation in rocket const float QR_IMU_TO_BODY[4] = {1.0, 0.0, 0.0, 0.0}; // (--) right, scalar-first, Hamiltonian quaternion transforming IMU frame to body frame