From ea8b60dd138cbdbed5d4309d274515eb1b9be6d7 Mon Sep 17 00:00:00 2001 From: LiamSmego Date: Sun, 14 Mar 2021 15:14:04 -0400 Subject: [PATCH] add function to send tvc signal --- main/main.ino | 45 +++++++++++++++++++++++++-------------- main/mission_constants.hh | 36 +++++++++++++++---------------- main/moding.cpp | 44 ++++++++++++++++++++------------------ main/moding.hh | 2 -- main/workspace.hh | 3 ++- 5 files changed, 72 insertions(+), 58 deletions(-) diff --git a/main/main.ino b/main/main.ino index 11c00f6..af9dbf3 100644 --- a/main/main.ino +++ b/main/main.ino @@ -8,6 +8,9 @@ #include #include "matrix.hh" #include "mission_constants.hh" +#include + +using namespace std; // declare flash chip #define CHIPSIZE MB64 @@ -21,13 +24,32 @@ Workspace ws; * * TODO: add docstring */ -void send_tvc_signal(int x, int y, double gamma, int d) +void send_tvc(Matrix u, Matrix * last_u, double yaw) { - // cos and sin are native to arduino - double u = x*cos(gamma) + y*sin(gamma); - double v = y*cos(gamma) - x*sin(gamma); - ws.tvc_y.write(v + TVC_Y_OFFSET); - ws.tvc_x.write(u + TVC_X_OFFSET); + //scale down angle to physical tvc limit + float input_magnitude=powf((pow(u.select(1, 1), 2)+pow(u.select(2, 1), 2)), 0.5); + if (input_magnitude>MAX_U) + { + u.scale(MAX_U/input_magnitude); + } + + //send + float travel_magnitude=powf((pow(u.select(1, 1)-last_u.select(1, 1), 2)+pow(u.select(2, 1)-.select(2, 1), 2)), 0.5); + if (travel_magnitude>SERVO_SPEED*ws.dt) + { + u=last_u+(u-last_u).scale(SERVO_SPEED*ws.dt/travel_magnitude); + } + *last_u=u; + + //rotate input are body z axis + float gamma=yaw+BETA; + float rotation_values [4]={cos(gamma), sin(gamma), -sin(gamma), cos(gamma)}; + Matrix R=Matrix(2, 2, rotation_values); + u=R*u; + + //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); } @@ -58,15 +80,6 @@ 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 - // actuate servo along x and then along y axis at startup - tvc_abs( 0, 0, BETA, TVC_DELAY*100); - tvc_abs( 30, 0, BETA, TVC_DELAY*100); - tvc_abs(-30, 0, BETA, TVC_DELAY*100); - tvc_abs( 0, 0, BETA, TVC_DELAY*100); - tvc_abs( 0, 30, BETA, TVC_DELAY*100); - tvc_abs( 0, -30, BETA, TVC_DELAY*100); - tvc_abs( 0, 0, BETA, TVC_DELAY*100); - // set up IMUs // calibrate the IMU linear acceleration sensors @@ -166,7 +179,7 @@ void loop() { //TODO: construct y ws.u=(K*ws.x).scale(-1);//calculate input - //send signal to tvc + send_tvc(ws.u, &ws.last_u, ws.yaw); ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation only using sensors } break; diff --git a/main/mission_constants.hh b/main/mission_constants.hh index e09982c..a56d53b 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -13,26 +13,33 @@ const float RAD_2_DEG = 57.29577951308232; // (deg/rad) conversion factor from const float G_2_MS2 = 9.80665; // (m/s^2/gee) conversion factor from gee to m/s^2 const float MEGA = 1.0e9; // (--) SI Mega prefix +const int MEGA_I=1e9; +const float KILO=1.0e3; +const float KILO_I=1e3; //***************************************************************************** // MISSION //***************************************************************************** - - -const bool eraseFlash=false; - // Missing modes (controls what happens in the loop) enum Mode { STARTUP_STABLE = 0, - NAVIGATION = 1, - COUNTDOWN = 2, - FINAL_COUNTDOWN = 3, - PREP_TVC = 4, - BURN_BABY_BURN = 5, - SHUTDOWN_STABLE = 6 + COUNTDOWN = 1, + FINAL_COUNTDOWN = 2, + PREP_TVC = 3, + BURN_BABY_BURN = 4, + SHUTDOWN_STABLE = 5 }; +//most modes change by time +//after a predefined time period we switch to the next mode +//MODE TIME PERIODS +const int STARTUP_STABLE_PERIOD=10; +const int COUNTDOWN_PERIOD=20; +const int FINAL_COUNTDOWN_PERIOD=7; +const int PREP_TVC_PERIOD=3; +const int BURN_BABY_BURN_PERIOD=10; + //***************************************************************************** // HARDWARE //***************************************************************************** @@ -50,14 +57,6 @@ const float LSB_LINEAR = 2048.0; // (milli-gee/count) note documentation gives */ const float LSB_ANGULAR = 131.0; // (deg/s/count) note documentation gives incorrectly inverted units -/* These bias calibration values account for sensor bias - * in linear acceleration and angular velocity measurements. - * Can't be declared a const since it's calculated - * on-the-fly every startup. - */ -int16_t IMU_LIN_ACC_BIAS[3]; // (count) IMU linear acceleration bias along each axis -int16_t IMU_ANG_VEL_BIAS[3]; // (count) IMU angular velocity bias about each axis - // LEDs //TODO:modify values const int B_LED_1 = 6; // Blue LED1 Pin @@ -76,6 +75,7 @@ const float GEAR = 9; // gearing ratio of the servo to the TVC const int drop_mechanism_hold=10; //TODO: modify after assembly const int drop_mechanism_release=120; //TODO: modify after assembly +const float SERVO_SPEED; const float TVC_X_OFFSET = 85; // TODO: add description const float TVC_Y_OFFSET = 83; // TODO: add description diff --git a/main/moding.cpp b/main/moding.cpp index 35999a1..be87e9c 100644 --- a/main/moding.cpp +++ b/main/moding.cpp @@ -1,16 +1,7 @@ #include "mission_constants.hh" #include "moding.hh" - -/* change_mode_to_navigation - * - * Determines whether to change the mode to NAVIGATION. - * - * Conditions: - * - current mode is STARTUP_STABLE - * - TODO: determine any remaining conditions - */ -bool change_mode_to_navigation(Mode current_mode, bool cond) +bool change_mode_coundown(Mode current_mode, bool cond) { bool ret = false; if (current_mode == STARTUP_STABLE && cond) @@ -20,6 +11,25 @@ bool change_mode_to_navigation(Mode current_mode, bool cond) return ret; } +bool change_mode_to_final_countdown(Mode current_mode, bool cond) +{ + bool ret = false; + if (current_mode == COUNTDOWN && cond) + { + ret = true; + } + return ret; +} + +bool change_mode_to_prep_tvc(Mode current_mode, bool cond) +{ + bool ret = false; + if (current_mode == FINAL_COUNTDOWN && cond) + { + ret = true; + } + return ret; +} /* change_mode_to_burn_baby_burn * @@ -32,7 +42,7 @@ bool change_mode_to_navigation(Mode current_mode, bool cond) bool change_mode_to_burn_baby_burn(Mode current_mode, bool cond) { bool ret = false; - if (current_mode == NAVIGATION && cond) + if (current_mode == PREP_TVC && cond) { ret = true; } @@ -59,19 +69,11 @@ bool change_mode_to_shutdown_stable(Mode current_mode, bool cond) } -/* transition_to_navigation - * - * Handles anything that needs to happen only once when - * changing modes from STARTUP_STABLE to NAVIGATION. - */ -void transition_to_navigation() -{ - //TODO: reset navigation - //TODO: change LED -} void transition_to_countdown() { + //TODO : change led + //reset navigation //TODO: set final countdown point //TODO: change LED } diff --git a/main/moding.hh b/main/moding.hh index 2370e0e..3267380 100644 --- a/main/moding.hh +++ b/main/moding.hh @@ -5,14 +5,12 @@ #include -bool change_mode_to_navigation(bool); bool change_mode_to_countdown(bool); bool change_mode_to_final_countdown(bool); bool change_mode_to_prep_tvc(bool); bool change_mode_to_burn_baby_burn(bool); bool change_mode_to_shutdown_stable(bool); -void transition_to_navigation(); void transition_to_countdown(); void transition_to_final_countdown(); void transition_to_prep_tvc(); diff --git a/main/workspace.hh b/main/workspace.hh index e23b5b9..d8bfb2d 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -55,7 +55,8 @@ class Workspace Matrix x; //state vector = {vx, theta_y, d_theta_y_dt, vy, theta_x, d_theta_x_dt} global frame and euler angle Matrix u; //input vector Matrix y; //output vector - Matrix last_u; + Matrix last_u; + float yaw; // state float r_body[3] = {0.0, 0.0, 0.0}; // (m) position of the body frame origin TODO: define inertial frame