added comments, removed redundant variable, added TODOs

src/main.ino

@@ -1,20 +1,20 @@
 #include <cstdint>  // C STD Int library, might not be needed
-// #include <Eigen30.h>  // TODO: what's this and why is it commented-out?
-#include <Eigen313.h>  // TODO: what's this?
+// #include <Eigen30.h>  // another of version of Eigen313.h
+#include <Eigen313.h>  // linear algebra library
 #include "I2Cdev.h"
-#include <LU>  // TODO: what's this?
+#include <LU>  // needed for Eigen313.h to work
 #include "mission_constants.hh"
 #include "moding.hh"
 #include "MPU6050.h"  // MPU 6050 IMU Library
 #include <SH.h>  // TODO: what's this?
-#include <SPIFlash.h>  // TODO: what's this?
+#include <SPIFlash.h>  // flash chip library
 #include "Wire.h"  // Arduino library
 #include "workspace.hh"  // variable storage
 
 
 using namespace Eigen;
 
-// TODO: what do these do?
+// declare flash chip 
 #define CHIPSIZE MB64
 SPIFlash flash(1);
 
@@ -53,7 +53,7 @@ void calibrate_imu_linear_acceleration(int16_t *imu_acc_bias[3])
 
     for (int i = 0; i < num_loops; i++)
     {
-        imu.getMotion6(&ax, &ay, &az, &wx, &wy, &wz);
+        imu.getMotion6(&ax, &az, &ay, &wx, &wy, &wz); //we want to label the vertical axis as z. the MPUs are orientated with y as vertical
         a_sums[0] += ax;
         a_sums[1] += ay;
         a_sums[2] += az;
@@ -116,15 +116,13 @@ void get_latest_measurements(MPU6050 imu, float *a[3], float *w[3], bool debug=f
  *
  * TODO: add docstring
  */
-void tvc_abs(int x, int y, double gamma, int d)
+void send_tvc_signal(int x, int y, double gamma, int d)
 {
-    // TODO: why are x and y in parentheses?
-    // TODO: where are cos and sin defined?
-    double u = (x)*cos(gamma) + (y)*sin(gamma);
-    double v = (y)*cos(gamma) - (x)*sin(gamma);
+    // 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);
-    delay(d);  // TODO: why is this delay here?
 }
 
 
@@ -153,20 +151,21 @@ void setup()
         Fastwire::setup(400, true);
     #endif
 
-    flash.begin(9600);  // TODO: what does this do?
-    // flash.eraseChip();  // TODO: what does this do and why is it commented-out?
+    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);  // TODO: add description
-    ws.tvc_y.attach(TVC_Y_PIN);  // TODO: add description
-    ws.tvc_top.attach(TVC_TOP_PIN);  // TODO: add description
+    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(10); delay(1000);  // TODO: what is the "10" doing and can it be a mission constant?
+    ws.tvc_top.write(drop_mechanism_hold); delay(1000);  // TODO: what is the "10" doing and can it be a mission constant?
 
-    // TODO: what's going on in this block?
+    // 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);
@@ -175,39 +174,31 @@ void setup()
     tvc_abs(  0, -30, BETA, TVC_DELAY*100);
     tvc_abs(  0,   0, BETA, TVC_DELAY*100);
 
-    // set up IMU
-    ws.imu.initialize();
-    ws.imu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);  // WARNING: changing this will require changing LSB_LINEAR mission constant
-    ws.imu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);  // WARNING: changing this will require changing LSB_ANGULAR mission constant
-    ws.imu.CalibrateAccel(30);  // TODO: What is "30"? Can this be made a mission constant?
-    ws.imu.CalibrateGyro(30);  // TODO: What is "30"? Can this be made a mission constant?
+    // 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_ACC_BIAS);
 
     // TODO: finish pulling over the rest of AGON1a setup code below this point
 
-    // blink for good luck
-    for (int num_blink_loops = 0; num_blink_loops < 4; num_blink_loops++)
-    {
-        // TODO: what's going on here?
-        ws.blink_master[i] = BLINK_0[i];
-        ws.blink_master[i+9] = BLINK_1[i];
-        ws.blink_master[i+18] = BLINK_2[i];
-        ws.blink_master[i+27] = BLINK_3[i];
-    }
+    // TODO: (and not necessarily here) organize blink patterns by mode
 
     // set up pyro
-    pinMode(PYRO_PIN, OUTPUT);
-    digitialWrite(PYRO_PIN, LOW);
+    pinMode(MOTOR_PIN, OUTPUT);
+    digitialWrite(MOTOR_PIN, LOW);
 
     // set up controller
-    // TODO: where are these values from and can they be mission constants?
-    ws.x << 0, 0, 0, 0, 0, 0;
-    ws.xControl << 0, 0, 0, 0, 0, 0;
-    ws.Ka << 0.34641, 1.72254, 0.32694, 0, 0, 0, 0, 0, 0, 0.34641, -1.88376, -0.3991;
-    ws.uLast[0] = 0;
-    ws.uLast[1] = 0;
+    // TODO : move stuff below
+    ws.x << 0, 0, 0, 0, 0, 0; //state vector
+    ws.Ka << 0.34641, 1.72254, 0.32694, 0, 0, 0, 0, 0, 0, 0.34641, -1.88376, -0.3991; //LQR gains
+    ws.uLast[0] = 0;  //last commanded tvc x input
+    ws.uLast[1] = 0; //last commanded tvc y input
 
     ws.calibrate_time = micros();
 }
@@ -273,5 +264,15 @@ void loop()
             break;
         }
     }
+    // TODO: formalize control loop
+    //construct x
+    //construct y
+    ws.u=-ws.K*ws.x;  //calculate tvc input
+    ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)) //calculate next state estimate
+    //send u to tvc
+
 
+    // TODO: add blink
+    // TODO: add data record
+    // TODO: add (somewhere else) data struct
 }

src/mission_constants.hh

@@ -16,6 +16,12 @@ const float MEGA = 1.0e9;  // (--) SI Mega prefix
 //*****************************************************************************
 //                                  MISSION
 //*****************************************************************************
+//TODO: modify modes
+
+
+
+const bool eraseFlash=false;
+
 // Missing modes (controls what happens in the loop)
 enum Mode {
     STARTUP_STABLE = 0,
@@ -28,22 +34,16 @@ enum Mode {
 const int COUNTDOWN_WAIT = 10;  // (TODO: units) TODO: what is this?
 const int CALIBRATE_TIME = 0;  // (TODO: units) TODO: what is this?
 
-// TODO: determine what all these are for
-const float fireTBuffer = 0.01*MEGA;  // (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?
 
-// TODO: what are these?
-long fireTime;
-long liftoffTime;
-long landTime;
-bool buffStatus;
-float buffCountDown;
-
 //*****************************************************************************
 //                                  HARDWARE
 //*****************************************************************************
@@ -67,37 +67,40 @@ const float LSB_ANGULAR = 131.0;  // (deg/s/count) note documentation gives inco
  */
 int16_t IMU_ACC_BIAS[3];  // (count) IMU linear acceleration along each axis
 
-// LEDs
-const int B_LED_1 = 6;  // TODO: add description
-const int G_LED_1 = 4;  // TODO: add description
-const int B_LED_2 = 9;  // TODO: add description
-const int G_LED_2 = 8;  // TODO: add description
+// LEDs 
+//TODO:modify values
+const int B_LED_1 = 6;  // Blue LED1 Pin
+const int G_LED_1 = 4;  // Green LED1 Pin
+const int B_LED_2 = 9;  // Blue LED2 Pin
+const int G_LED_2 = 8;  // Green LED2 Pin
 
-/* BLINK INDICES:
+/* BLINK INDICES:  
  *   - 0: Rate (Hz)
  *   - 1-4: A, command for each of Green 1, Blue 1, Green 2, Blue 2
  *   - 5-8: B, command for each of Green 1, Blue 1, Green 2, Blue 2
  */
+//TODO:debug on board
 const int BLINK_0[9] = {1, 1, 0, 1, 0, 0, 0, 0, 0};
 const int BLINK_1[9] = {1, 0, 1, 0, 1, 0, 1, 0, 1};
 const int BLINK_2[9] = {1, 1, 0, 1, 0, 1, 0, 1, 0};
 const int BLINK_3[9] = {1, 0, 1, 0, 1, 0, 1, 0, 1};
 
-// Pyro TODO: more descriptive name?
-const int PYRO_PIN = 22;  // TODO: add description
+const int MOTOR_PIN = 22;  // Pin that signals motor to fire
+
+// Thrust-Vector Controller (TVC)  //TODO: modify values
+const int DROP_PIN = 5;  // pin for the servo in the drop mechanism
+const int TVC_X_PIN = 20;  // pin for the servo that actuates TVC around x body axis
+const int TVC_Y_PIN = 21;  // pin for the servo that actuates TVC around y body axis
+const int GEAR = 9;  // gearing ratio of the servo to the TVC
 
-// Thrust-Vector Controller (TVC)
-const int TVC_TOP_PIN = 5;  // TODO: add description
-const int TVC_X_PIN = 20;  // TODO: add description
-const int TVC_Y_PIN = 21;  // TODO: add description
+const int drop_mechanism_hold=10; //TODO: modify after assembly
+const int drop_mechanism_release=120; //TODO: modify after assembly
 
 const int TVC_X_OFFSET = 85;  // TODO: add description
 const int TVC_Y_OFFSET = 83;  // TODO: add description
 
-const float BETA = 0.95;  // TODO: add description
-const int GEAR = 9;  // TODO: add description
-const float DAMPER = 1.5;  // TODO: add description
-const int TVC_DELAY = 10;  // TODO: add description
+const float BETA = 0.95;  // angle (rad) that corrects for misalignment between body frame and TVC frame
+const int TVC_DELAY = 2;  // time (in miliseconds) for servo to move 1 deg--> how much delay between servo commands
 
 // 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

src/workspace.hh

@@ -15,45 +15,47 @@ class Workspace
         //*****************************************************************************
         //                              HARDWARE SETUP
         //*****************************************************************************
-        MPU6050 imu;  // (--) the IMU to read data from
+        MPU6050 imu_0(0x68);  // (--) first IMU to read data from
+        MPU6050 imu_1(0x69);  // (--) second IMU to read data from
 
         //*****************************************************************************
         //                              LOOP VARIABLES
         //*****************************************************************************
         // LED blinks TODO: add descriptions to these
-        int blink_master[36];  // TODO: default value?
-        long next_blink;  // TODO: default value?
-        bool blink_flag = false;
+        int blink_master[36];  // TODO: maybe delay
+        long next_blink;  // the next time (us) when the led colors should switch
+        bool blink_flag = false;  //led colors switch when blink_flag is true
 
         // clock time
-        unsigned long calibrate_time = 0;
+        unsigned long calibrate_time = 0;  //momment main loop starts
         unsigned long t_prev_cycle = 0;  // (us) contains the time of the previous cycle at the start of each loop
         unsigned long dt = 0;  // (us) used to store time difference between t_prev_cycle and return of micros() at the start of each loop
 
         // sensor measurements
-        float a[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
-        float w[3] = {0.0, 0.0, 0.0};  // (rad/s) angular velocity, used for storing sensor measurements
+        float a_0[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
+        float a_1[3] = {0.0, 0.0, 0.0};  // (m/s^2) linear acceleration, used for storing sensor measurements
+        float w_0[3] = {0.0, 0.0, 0.0};  // (rad/s) angular velocity, used for storing sensor measurements
+        float w_1[3] = {0.0, 0.0, 0.0};  // (rad/s) angular velocity, used for storing sensor measurements
 
         // flight mode
         Mode mode = STARTUP_STABLE;
 
-        // Thrust-Vector Controller (TVC)
-        Servo tvc_top;  // TODO: add description
-        Servo tvc_x;  // TODO: add description
-        Servo tvc_y;  // TODO: add description
+        // Servos
+        Servo servo_top;  // servo in the drop mechanism
+        Servo tvc_x;  // servo that actuates TVC around x body axis
+        Servo tvc_y;  // servo that actuates TVC around x body axis
 
         // controller parameters
-        // TODO: what are all these?
-        MatrixXf Ka(2,6);
-        VectorXi x(6); 
-        VectorXf xControl(6); 
-        VectorXf ua(2);
-        float ub[2];
-        float uc[2];
-        MatrixXi A[9];
-        MatrixXi B[9];
-        float maxU = 5*3.14159/180;
-        long uLast[2] = {0, 0};
+        // TODO: define parameters, move to mission constants
+        MatrixXf K(2,6); //LQR -> updates mid flight
+        VectorXi x(6); //state vector = {vx, theta_y, d_theta_y_dt, vy, theta_x, d_theta_x_dt} global frame and euler angles
+        VectorXf u(2); //input vector
+        MatrixXf A(36); //dynamics matrix
+        MatrixXf B(12); //input matrix
+        MatrixXf C(24); //Sensor matrix
+        MatrixXf L(24); //Kalman gain
+        float maxU = 5*3.14159/180;  //maximum gimbal angle
+        long uLast[2] = {0, 0};  //commanded servo angle
 
         // state
         float r_body[3] = {0.0, 0.0, 0.0};  // (m) position of the body frame origin TODO: define inertial frame