change some of the moding

Propulsive-Landing · Mar 14, 2021 · cd3e8e8 · cd3e8e8
1 parent caf627d
commit cd3e8e8
Show file tree

Hide file tree

Showing 7 changed files with 107 additions and 90 deletions.
diff --git a/main/main.ino b/main/main.ino
@@ -1,16 +1,13 @@
 #include <cstdint>  // C STD Int library, might not be needed
-// #include <Eigen30.h>  // another of version of Eigen313.h
 #include "I2Cdev.h"
-#include "mission_constants.hh"
 #include "moding.hh"
 #include <SH.h>  // TODO: what's this?
 #include <SPIFlash.h>  // flash chip library
 #include "Wire.h"  // Arduino library
 #include "workspace.hh"  // variable storage
+#include <Servo.h>
 #include "matrix.hh"
-
-
-using namespace Eigen;
+#include "mission_constants.hh"
 
 // declare flash chip 
 #define CHIPSIZE MB64
@@ -143,16 +140,8 @@ void send_tvc_signal(int x, int y, double gamma, int d)
 void setup()
 {
     // set up pins (OUTPUT and LOW are defined in Arduino.h)
-    pinMode(B_LED_1, OUTPUT);
-    pinMode(G_LED_1, OUTPUT);
-    pinMode(B_LED_2, OUTPUT);
-    pinMode(G_LED_2, OUTPUT);
-
-    digitalWrite(B_LED_1, LOW);
-    digitalWrite(G_LED_1, LOW);
-    digitalWrite(B_LED_2, LOW);
-    digitalWrite(G_LED_2, LOW);
 
+    //TODO do we still need this????
     // join I2C bus (I2Cdev library doesn't do this automatically)
     // TODO: do these need to be #if's, i.e. could they be regular C++ if's?
     #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
@@ -161,20 +150,16 @@ void setup()
         Fastwire::setup(400, true);
     #endif
 
+    //TODO flash setup
     flash.begin(9600);  // begins flash chip at specified baud rate
-    if (eraseFlash)
-        flash.eraseChip();  // erases flash chip
 
     // set initial mission mode
     ws.mode = STARTUP_STABLE;
 
     // set up Thrust-Vector Controller (TVC)
     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_top.attach(DROP_PIN);  // attaches declared servo to specified pin
 
-    ws.tvc_top.write(drop_mechanism_hold); delay(1000);  // TODO: what is the "10" doing and can it be a mission constant?
-
     // 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);
@@ -185,35 +170,18 @@ void setup()
     tvc_abs(  0,   0, BETA, TVC_DELAY*100);
 
     // set up IMUs
-    ws.imu_0.initialize();
-    ws.imu_0.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);  // WARNING: changing this will require changing LSB_LINEAR mission constant
-    ws.imu_0.setFullScaleGyroRange(MPU6050_GYRO_FS_250);  // WARNING: changing this will require changing LSB_ANGULAR mission constant
-    ws.imu_1.initialize();
-    ws.imu_1.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);  // WARNING: changing this will require changing LSB_LINEAR mission constant
-    ws.imu_1.setFullScaleGyroRange(MPU6050_GYRO_FS_250);  // WARNING: changing this will require changing LSB_ANGULAR mission constant
 
     // calibrate the IMU linear acceleration sensors
     calibrate_imu_linear_acceleration(*IMU_LIN_ACC_BIAS, *IMU_ANG_VEL_BIAS);
 
-    // TODO: finish pulling over the rest of AGON1a setup code below this point
-
-    // TODO: (and not necessarily here) organize blink patterns by mode
-
-    // set up pyro
-    pinMode(MOTOR_PIN, OUTPUT);
-    digitialWrite(MOTOR_PIN, LOW);
-
     // set up controller
-    // TODO : move stuff below
-    float * initialize_6={0, 0, 0, 0, 0, 0};
-    float * initialize_4={0, 0, 0, 0};
-    float * initialize_2={0, 0};
+    float initialize_6 [6]={0, 0, 0, 0, 0, 0};
+    float initialize_4 [4]={0, 0, 0, 0};
+    float initialize_2 [2]={0, 0};
     ws.x=Matrix(6, 1, initialize_6);
     ws.y=Matrix(4, 1, initialize_4);
     ws.u=Matrix(2, 1, initialize_2);
-
-    ws.uLast[0] = 0;  //last commanded tvc x input
-    ws.uLast[1] = 0; //last commanded tvc y input
+    ws.last_u=Matrix(2, 1, initialize_2);
 
     ws.calibrate_time = micros();
 }
@@ -236,50 +204,87 @@ void loop()
     {
         case (STARTUP_STABLE):
         {
-            if (change_mode_to_navigation(mode, false))
+            if (change_mode_to_navigation(ws.mode, false))
             {
                 transition_to_navigation();
                 ws.mode = NAVIGATION;
             }
             else
             {
-                // TODO: implement this block
+
             }
             break;
         }
         case (NAVIGATION):
         {
-            if (change_mode_to_burn_baby_burn(mode, false))
+            if (change_mode_to_countdown(ws.mode, false))
+            {
+                transition_to_countdown();
+                ws.mode = COUNTDOWN;
+            }
+            else
+            {
+                //TODO: construct y
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation only using sensors
+            }
+            break;
+        }
+        case (COUNTDOWN)
+        {
+            if (change_mode_to_final_countdown(ws.mode, false))
+            {
+                transition_to_final_countdown();
+                ws.mode = FINAL_COUNTDOWN;
+            }
+            else
+            {
+                //TODO: construct y
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation only using sensors
+            }
+            break;
+        }
+        case(FINAL_COUNTDOWN)
+        {
+            if (change_mode_to_prep_tvc(ws.mode, false))
+            {
+                transition_to_prep_tvc();
+                ws.mode = PREP_TVC;
+            }
+            else
+            {
+                //TODO: construct y
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation only using sensors
+            }
+            break;
+        }
+        case(PREP_TVC)
+        {
+            if (change_mode_to_burn_baby_burn(ws.mode, false))
             {
                 transition_to_burn_baby_burn();
                 ws.mode = BURN_BABY_BURN;
             }
             else
             {
-                //construct x
-                //construct y
-                ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)) //calculate next state
+                //TODO: construct y
+                ws.u=(K*ws.x).scale(-1);//calculate input
+                //send signal to tvc
+                ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation only using sensors
             }
             break;
         }
         case (BURN_BABY_BURN):
         {
-            if (change_mode_to_shutdown_stable(mode, false))
+            if (change_mode_to_shutdown_stable(ws.mode, false))
             {
                 transition_to_shutdown_stable();
                 ws.mode = SHUTDOWN_STABLE;
             }
             else
             {
-                //checks whether the burn time has surpassed the pre-defined burn interval
-                //if so, then updates the control constant by multiplying by the current thrust
-                //construct x
-                //construct y
+                //TODO: construct y
                 ws.u=(K*ws.x).scale(-1); //calculate input
-                ws.x=mAdd(ws.x, sMult(mAdd(mAdd(), ), ws.dt));
-                ws.x=ws.x+((A*ws.x)+(B*ws.u)+(L*(ws.y-(C*ws.x)))).scale(ws.dt); //calculate next state
-                //process u
-                //send u to tvc
+                ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation using Kalman filter
             }
             break;
         }
@@ -289,7 +294,6 @@ void loop()
             break;
         }
     }
-    // TODO: add blink
     // TODO: add data record
     // TODO: add (somewhere else) data struct
 }
diff --git a/main/matrix.cpp b/main/matrix.cpp
@@ -112,4 +112,21 @@ void display_matrix(Matrix A)
         }
         cout << "\n";
     }
-}
+}
+
+/** for testing
+int main()
+{
+    float a_values [9] ={1, 0, 1, 0, 1, 1, 1, 0, 1};
+    float b_values [9] ={3, 0, 2, 2, 1, 3, 2, 2, -1};
+    Matrix A=Matrix(3, 3, a_values);
+    Matrix B=Matrix(3, 3, b_values);
+    
+    float d_values [6]= {-3, -10, 7, 2, 5, 0};
+    Matrix D=Matrix(3, 2, d_values);
+    Matrix E=(A+B)*D;
+    display_matrix(E);
+
+    return 0;
+}
+*/
diff --git a/main/matrix.hh b/main/matrix.hh
@@ -29,10 +29,6 @@ class Matrix
         Matrix operator*(Matrix);
 };
 
-//Matrix mMult(Matrix, Matrix);
-//Matrix mAdd(Matrix, Matrix);
-//Matrix sMult(Matrix, float);
-//Matrix mSub(Matrix, Matrix);
 void display_matrix(Matrix);
 
 #endif  // __MODING_HH__
diff --git a/main/mission_constants.hh b/main/mission_constants.hh
@@ -17,7 +17,6 @@ const float MEGA = 1.0e9;  // (--) SI Mega prefix
 //*****************************************************************************
 //                                  MISSION
 //*****************************************************************************
-//TODO: modify modes
 
 
 
@@ -31,26 +30,9 @@ enum Mode {
     FINAL_COUNTDOWN = 3,
     PREP_TVC = 4,
     BURN_BABY_BURN = 5,
-    TRANSFER_DATA = 6,
-    SHUTDOWN_STABLE =7
+    SHUTDOWN_STABLE = 6
 };
 
-// timing
-const int COUNTDOWN_WAIT = 10;  // (TODO: units) TODO: what is this?
-const int CALIBRATE_TIME = 0;  // (TODO: units) TODO: what is this?
-
-/**
-// z-acceleration > fireGoal+fireBuffer continuously for fireTBuffer before switching to 
-const float fireTBuffer = 0.01*MEGA;  // (us) TODO: what is this?
-const float fireBuffer = 3276.0/2.0/16.0;  // (m/s^2) ~0.25 TODO: what is this
-const float fireGoal = 0.0;  // -g TODO: what is this?
-
-// |z-acceleration| < landBuffer+landGoal continuously for landTBuffer before switching to 
-const float landTBuffer = 0.5*MEGA;  // (TODO: units) TODO: what is this? 
-const float landBuffer = 3276.0/2.0/8.0; // (TODO: units) TODO: what is this?
-const float landGoal = 0.0;  // (TODO: units) TODO: what is this?
-*/
-
 //*****************************************************************************
 //                                  HARDWARE
 //*****************************************************************************
@@ -97,6 +79,7 @@ const int drop_mechanism_release=120; //TODO: modify after assembly
 const float TVC_X_OFFSET = 85;  // TODO: add description
 const float TVC_Y_OFFSET = 83;  // TODO: add description
 
+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 int TVC_DELAY = 4;  // time (in miliseconds) for servo to move 1 deg--> how much delay between servo commands
 

diff --git a/main/moding.cpp b/main/moding.cpp
@@ -66,8 +66,27 @@ bool change_mode_to_shutdown_stable(Mode current_mode, bool cond)
  */
 void transition_to_navigation()
 {
+    //TODO: reset navigation
+    //TODO: change LED
 }
 
+void transition_to_countdown()
+{
+    //TODO: set final countdown point
+    //TODO: change LED
+}
+
+void transition_to_final_countdown()
+{
+    //TODO: set prep tvc point
+    //TODO: change LED
+}
+
+void transition_to_prep_tvc()
+{
+    //TODO: set burn time
+    //TODO: change LED
+}
 
 /* transition_to_burn_baby_burn
  * 
@@ -76,16 +95,18 @@ void transition_to_navigation()
  */
 void transition_to_burn_baby_burn()
 {
-    // TODO: engine ignition occurs here
+    //set stop time
+    //TODO: change LED
 }
 
-
 /* transition_to_shutdown_stable
  * 
  * Handles anything that needs to happen only once when
  * changing modes from BURN_BABY_BURN to SHUTDOWN_STABLE.
  */
 void transition_to_shutdown_stable()
 {
-    // TODO: engine shutdown and safing occurs here
+    //TODO: change LED
+    //transfer data
+    //TODO: change LED
 }
diff --git a/main/moding.hh b/main/moding.hh
@@ -10,15 +10,13 @@ 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_transfer_data(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();
 void transition_to_burn_baby_burn();
-void transition_to_transfer_data();
 void transition_to_shutdown_stable();
 
 #endif  // __MODING_HH__
diff --git a/main/workspace.hh b/main/workspace.hh
@@ -4,8 +4,8 @@
 #include <cstdint>
 #include "mission_constants.hh"
 #include "moding.hh"
-#include <Servo.h>  // TODO: what's this?
-#include "Wire.h"  // Arduino library
+//#include <Servo.h>  // TODO: what's this?
+//#include "Wire.h"  // Arduino library
 
 
 /* Workspace
@@ -55,9 +55,7 @@ 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
-
-        float max_u = 5*DEG_2_RAD;  //maximum gimbal angle
-        float last_u[2] = {0, 0};  //commanded servo angle
+        Matrix last_u;        
 
         // state
         float r_body[3] = {0.0, 0.0, 0.0};  // (m) position of the body frame origin TODO: define inertial frame