diff --git a/main/main.ino b/main/main.ino index 47b10da..89242cf 100644 --- a/main/main.ino +++ b/main/main.ino @@ -1,16 +1,13 @@ #include // C STD Int library, might not be needed -// #include // another of version of Eigen313.h #include "I2Cdev.h" -#include "mission_constants.hh" #include "moding.hh" #include // TODO: what's this? #include // flash chip library #include "Wire.h" // Arduino library #include "workspace.hh" // variable storage +#include #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,9 +150,8 @@ 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; @@ -171,10 +159,7 @@ void setup() // 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 } \ No newline at end of file diff --git a/main/matrix.cpp b/main/matrix.cpp index 11466b6..643b752 100644 --- a/main/matrix.cpp +++ b/main/matrix.cpp @@ -112,4 +112,21 @@ void display_matrix(Matrix A) } cout << "\n"; } -} \ No newline at end of file +} + +/** 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; +} +*/ \ No newline at end of file diff --git a/main/matrix.hh b/main/matrix.hh index fd3b69f..6b4335e 100644 --- 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__ \ No newline at end of file diff --git a/main/mission_constants.hh b/main/mission_constants.hh index 240c281..e09982c 100644 --- 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 index ddb65f2..35999a1 100644 --- 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,10 +95,10 @@ 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 @@ -87,5 +106,7 @@ void transition_to_burn_baby_burn() */ void transition_to_shutdown_stable() { - // TODO: engine shutdown and safing occurs here + //TODO: change LED + //transfer data + //TODO: change LED } \ No newline at end of file diff --git a/main/moding.hh b/main/moding.hh index 79e8b49..2370e0e 100644 --- a/main/moding.hh +++ b/main/moding.hh @@ -10,7 +10,6 @@ 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(); @@ -18,7 +17,6 @@ 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__ \ No newline at end of file diff --git a/main/workspace.hh b/main/workspace.hh index 5b8275a..e23b5b9 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -4,8 +4,8 @@ #include #include "mission_constants.hh" #include "moding.hh" -#include // TODO: what's this? -#include "Wire.h" // Arduino library +//#include // 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