diff --git a/main/main.ino b/main/main.ino index e402bcc..0674200 100644 --- a/main/main.ino +++ b/main/main.ino @@ -1,15 +1,13 @@ #include // C STD Int library, might not be needed // #include // another of version of Eigen313.h -#include // linear algebra library #include "I2Cdev.h" -#include // Eigen313 dependency #include "mission_constants.hh" #include "moding.hh" -#include "MPU6050.h" // MPU 6050 IMU Library #include // TODO: what's this? #include // flash chip library #include "Wire.h" // Arduino library #include "workspace.hh" // variable storage +#include "matrix.hh" using namespace Eigen; @@ -207,7 +205,11 @@ void setup() // set up controller // TODO : move stuff below - ws.x << 0, 0, 0, 0, 0, 0; //state vector + float * initialize={0, 0, 0, 0, 0, 0}; + ws.x=Matrix(6, 1, initialize); + ws.y=Matrix(4, 1, initialize); + ws.u=Matrix(2, 1, initialize); + ws.uLast[0] = 0; //last commanded tvc x input ws.uLast[1] = 0; //last commanded tvc y input @@ -269,17 +271,10 @@ void loop() { //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 - if (ws.thrust_curve_count+1(ws.fire_time+T_time[ws.thrust_curve_count+1])*MEGA) - { - ws.thrust_curve_count++; - ws.K=K_template*T[ws.thrust_curve_count]; - ws.A=A_template*T[ws.thrust_curve_count]; - ws.B=B_template*T[ws.thrust_curve_count]; - ws.L=L_template*T[ws.thrust_curve_count]; - } //construct x //construct y - ws.u=-ws.K*ws.x; //calculate input + ws.u=sMult(mMult(), -1); //calculate input + ws.x=mAdd(ws.x, sMult(mAdd(mAdd(), ), ws.dt)); ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)); //calculate next state //process u //send u to tvc diff --git a/main/mission_constants.hh b/main/mission_constants.hh index e7ca0dd..56d87a0 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -2,8 +2,7 @@ #define __MISSION_CONSTANTS_HH__ #include -#include // linear algebra library -#include // needed for Eigen313.h to work +#include "matrix.hh" //***************************************************************************** @@ -36,6 +35,7 @@ enum Mode { 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 @@ -45,6 +45,7 @@ const float fireGoal = 0.0; // -g TODO: what is this? 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 @@ -78,52 +79,60 @@ 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: - * - 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}; - 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 +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 int TVC_X_OFFSET = 85; // TODO: add description -const int TVC_Y_OFFSET = 83; // TODO: add description +const float TVC_X_OFFSET = 85; // TODO: add description +const float TVC_Y_OFFSET = 83; // 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 +const int TVC_DELAY = 4; // 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 const float QR_BODY_TO_IMU[4] = {1.0, 0.0, 0.0, 0.0}; // (--) right, scalar-first, Hamiltonian quaternion transforming body frame to IMU frame - -//control constants -MatrixXf A_template(36); //dynamics matrix -MatrixXf B_template(12); //input matrix -MatrixXf L_template(36); //Kalman gain -MatrixXf K_template(12); //LQR -> updates mid flight -MatrixXf C(36); //Sensor matrix -C=MatrixXd::Identity(6, 6); - -//thrust curve -const float T={30, 9}; //Thrust values (in Newtons) -const float T_time={0, .33}; //time in the burn when we start to use the corresponding thrust value -const int T_intervals=2; //how many thrust data points do we have - +const float THRUST=10; //TODO: make exact +const float MOMENT_ARM=.265; //TODO: make exact +const float MOMENT_INERTIA_XX=1; //TODO: make exact +const float MOMENT_INERTIA_YY=1;//TODO: make exact + +//control constants TODO: fill these out +float A_VALUES [36] = {0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0}; //dynamics matrix + + float B_VALUES [12] = {0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0}; //input matrix + + float K_VALUES [12] = {0,0,0,0,0,0, + 0,0,0,0,0,0}; //controller gain + + float C_VALUES [24] = {0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0, + 0,0,0,0,0,0}; //sensor matrix + + float L_VALUES [24] = {0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0, + 0, 0, 0, 0}; //kalman gain #endif // __MISSION_CONSTANTS_HH__ \ No newline at end of file diff --git a/main/workspace.hh b/main/workspace.hh index 7490745..1a33fd3 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -53,8 +53,10 @@ class Workspace Servo tvc_y; // servo that actuates TVC around x body axis //control vectors - //state vector = {vx, theta_y, d_theta_y_dt, vy, theta_x, d_theta_x_dt} global frame and euler angles - //input vector + 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