add function to send tvc signal

main/main.ino

@@ -8,6 +8,9 @@
 #include <Servo.h>
 #include "matrix.hh"
 #include "mission_constants.hh"
+#include <cmath>
+
+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;

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
 

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
 }

main/moding.hh

@@ -5,14 +5,12 @@
 #include <cstdint>
 
 
-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();

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