From 7ebcf2f7d3f7bbfafc9172cf2bddb47b00c8faca Mon Sep 17 00:00:00 2001
From: Mike Bernard <mike.bernard@uconn.edu>
Date: Tue, 10 Nov 2020 23:51:41 -0500
Subject: [PATCH] finish pulling over AGON1a setup

---
 src/main.ino             | 28 ++++++++++++++++++++++++++++
 src/mission_constants.hh | 10 ++++++++++
 src/workspace.hh         | 19 +++++++++++++++++++
 3 files changed, 57 insertions(+)

diff --git a/src/main.ino b/src/main.ino
index 17c34ea..7f5ee99 100644
--- a/src/main.ino
+++ b/src/main.ino
@@ -14,6 +14,10 @@
 
 using namespace Eigen;
 
+// TODO: what do these do?
+#define CHIPSIZE MB64
+SPIFlash flash(1);
+
 // (--) instance of workspace class storing all the variables used in the loop
 Workspace ws;
 
@@ -182,6 +186,30 @@ void setup()
     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];
+    }
+
+    // set up pyro
+    pinMode(PYRO_PIN, OUTPUT);
+    digitialWrite(PYRO_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;
+    
+    ws.calibrate_time = micros();
 }
 
 
diff --git a/src/mission_constants.hh b/src/mission_constants.hh
index 800d2dd..ec74153 100644
--- a/src/mission_constants.hh
+++ b/src/mission_constants.hh
@@ -71,6 +71,16 @@ 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
 
+/* 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
+ */
+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
 
diff --git a/src/workspace.hh b/src/workspace.hh
index 2a719a5..bf93ff1 100644
--- a/src/workspace.hh
+++ b/src/workspace.hh
@@ -20,7 +20,13 @@ class Workspace
         //*****************************************************************************
         //                              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;
+
         // clock time
+        unsigned long calibrate_time = 0;
         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
 
@@ -36,6 +42,19 @@ class Workspace
         Servo tvc_x;  // TODO: add description
         Servo tvc_y;  // TODO: add description
 
+        // 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};
+
         // state
         float r_body[3] = {0.0, 0.0, 0.0};  // (m) position of the body frame origin TODO: define inertial frame
         float v_body_wrt_inertial_in_inertial[3] = {0.0, 0.0, 0.0};  // (m/s) velocity of body frame origin wrt inertial frame, components resolved in inertial frame