diff --git a/main/main.ino b/main/main.ino index af9dbf3..5a757e5 100644 --- a/main/main.ino +++ b/main/main.ino @@ -1,9 +1,8 @@ -#include // C STD Int library, might not be needed #include "I2Cdev.h" #include "moding.hh" -#include // TODO: what's this? -#include // flash chip library -#include "Wire.h" // Arduino library +//#include // flash chip library +#include "BNO055_support.h" //Contains the bridge code between the API and Arduino +#include #include "workspace.hh" // variable storage #include #include "matrix.hh" @@ -13,8 +12,8 @@ using namespace std; // declare flash chip -#define CHIPSIZE MB64 -SPIFlash flash(1); +//#define CHIPSIZE MB64 +//SPIFlash flash(1); // (--) instance of workspace class storing all the variables used in the loop Workspace ws; @@ -26,18 +25,18 @@ Workspace ws; */ void send_tvc(Matrix u, Matrix * last_u, double yaw) { + Matrix last_u_now= *last_u; //scale down angle to physical tvc limit float input_magnitude=powf((pow(u.select(1, 1), 2)+pow(u.select(2, 1), 2)), 0.5); if (input_magnitude>MAX_U) { u.scale(MAX_U/input_magnitude); } - //send - float travel_magnitude=powf((pow(u.select(1, 1)-last_u.select(1, 1), 2)+pow(u.select(2, 1)-.select(2, 1), 2)), 0.5); + float travel_magnitude=powf((pow(u.select(1, 1)-last_u_now.select(1, 1), 2)+pow(u.select(2, 1)-last_u_now.select(2, 1), 2)), 0.5); if (travel_magnitude>SERVO_SPEED*ws.dt) { - u=last_u+(u-last_u).scale(SERVO_SPEED*ws.dt/travel_magnitude); + u=last_u_now+(u-last_u_now).scale(SERVO_SPEED*ws.dt/travel_magnitude); } *last_u=u; @@ -61,17 +60,9 @@ void setup() { // set up pins (OUTPUT and LOW are defined in Arduino.h) - //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 - Wire.begin(); - #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - Fastwire::setup(400, true); - #endif //TODO flash setup - flash.begin(9600); // begins flash chip at specified baud rate + //flash.begin(9600); // begins flash chip at specified baud rate // set initial mission mode ws.mode = STARTUP_STABLE; @@ -80,21 +71,18 @@ void setup() 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 - // set up IMUs + //Initialize I2C communication + Wire.begin(); + + //Initialization of the BNO055 + BNO_Init(&ws.myBNO); //Assigning the structure to hold information about the device - // calibrate the IMU linear acceleration sensors - calibrate_imu_linear_acceleration(*IMU_LIN_ACC_BIAS, *IMU_ANG_VEL_BIAS); + //Configuration to NDoF mode + bno055_set_operation_mode(OPERATION_MODE_NDOF); - // set up controller - 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.last_u=Matrix(2, 1, initialize_2); + delay(1); - ws.calibrate_time = micros(); + ws.next_mode_time=millis()+STARTUP_STABLE_PERIOD*KILO_I; } @@ -110,92 +98,90 @@ void loop() ws.dt = micros() - ws.t_prev_cycle; // (us) time step since previous loop ws.t_prev_cycle += ws.dt; // (us) update time for next loop + bno055_read_euler_hrp(&ws.myEulerData); + ws.yaw=float(ws.myEulerData.h)/16.00*DEG_2_RAD; + ws.theta_0[0]=ws.myEulerData.r; + ws.theta_1[0]=ws.myEulerData.r; + ws.theta_0[1]=ws.myEulerData.p; + ws.theta_1[1]=ws.myEulerData.p; + // check for moding change conditions switch (ws.mode) { case (STARTUP_STABLE): { - if (change_mode_to_navigation(ws.mode, false)) - { - transition_to_navigation(); - ws.mode = NAVIGATION; - } - else - { - - } - break; - } - case (NAVIGATION): - { - if (change_mode_to_countdown(ws.mode, false)) + if (change_mode_to_countdown(millis()>ws.next_mode_time)) { transition_to_countdown(); + ws.next_mode_time=millis()+COUNTDOWN*KILO_I; 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) + case (COUNTDOWN): { - if (change_mode_to_final_countdown(ws.mode, false)) + if (change_mode_to_final_countdown(millis()>ws.next_mode_time)) { transition_to_final_countdown(); + ws.next_mode_time=millis()+FINAL_COUNTDOWN_PERIOD*KILO_I; 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 + ws.construct_y(); + ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors } break; } - case(FINAL_COUNTDOWN) + case(FINAL_COUNTDOWN): { - if (change_mode_to_prep_tvc(ws.mode, false)) + if (change_mode_to_prep_tvc(millis()>ws.next_mode_time)) { transition_to_prep_tvc(); + ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I; 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 + ws.construct_y(); + ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors } break; } - case(PREP_TVC) + case(PREP_TVC): { - if (change_mode_to_burn_baby_burn(ws.mode, false)) + if (change_mode_to_burn_baby_burn(millis()>ws.next_mode_time)) { transition_to_burn_baby_burn(); + ws.next_mode_time=millis()+BURN_BABY_BURN*KILO_I; ws.mode = BURN_BABY_BURN; } else { - //TODO: construct y - ws.u=(K*ws.x).scale(-1);//calculate input + ws.construct_y(); + ws.u=(KC*ws.x).scale(-1);//calculate input send_tvc(ws.u, &ws.last_u, ws.yaw); - ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt) //state estimation only using sensors + ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors } break; } case (BURN_BABY_BURN): { - if (change_mode_to_shutdown_stable(ws.mode, false)) + if (change_mode_to_shutdown_stable(millis()>ws.next_mode_time)) { transition_to_shutdown_stable(); ws.mode = SHUTDOWN_STABLE; } else { - //TODO: construct y - ws.u=(K*ws.x).scale(-1); //calculate input - 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 + ws.construct_y(); + ws.u=(KC*ws.x).scale(-1); //calculate input + send_tvc(ws.u, &ws.last_u, ws.yaw); + ws.x=ws.x+(A*ws.x+B*ws.last_u+L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation using Kalman filter } break; } @@ -204,7 +190,8 @@ void loop() // TODO: implement this block break; } + display_matrix(ws.y); } // 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 643b752..1533d27 100644 --- a/main/matrix.cpp +++ b/main/matrix.cpp @@ -114,6 +114,11 @@ void display_matrix(Matrix A) } } +void Matrix:: redefine(float* new_values) +{ + values=new_values; +} + /** for testing int main() { diff --git a/main/matrix.hh b/main/matrix.hh index 6b4335e..5fefe15 100644 --- a/main/matrix.hh +++ b/main/matrix.hh @@ -11,7 +11,7 @@ class Matrix int rows; int columns; float * values; - + Matrix(int row_number, int column_number, float * mat_values) { rows=row_number; @@ -23,6 +23,8 @@ class Matrix { return values[(row-1)*columns+(column-1)]; } + + void redefine(float*); Matrix operator+(Matrix); Matrix operator-(Matrix); Matrix scale (float); @@ -31,4 +33,4 @@ class Matrix void display_matrix(Matrix); -#endif // __MODING_HH__ \ No newline at end of file +#endif // __MODING_HH__ diff --git a/main/mission_constants.hh b/main/mission_constants.hh index a56d53b..95d1679 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -1,10 +1,8 @@ #ifndef __MISSION_CONSTANTS_HH__ #define __MISSION_CONSTANTS_HH__ -#include #include "matrix.hh" - //***************************************************************************** // CONVERSION FACTORS //***************************************************************************** @@ -34,7 +32,7 @@ enum Mode { //most modes change by time //after a predefined time period we switch to the next mode //MODE TIME PERIODS -const int STARTUP_STABLE_PERIOD=10; +const int STARTUP_STABLE_PERIOD=15; const int COUNTDOWN_PERIOD=20; const int FINAL_COUNTDOWN_PERIOD=7; const int PREP_TVC_PERIOD=3; @@ -57,61 +55,56 @@ const float LSB_LINEAR = 2048.0; // (milli-gee/count) note documentation gives */ const float LSB_ANGULAR = 131.0; // (deg/s/count) note documentation gives incorrectly inverted units -// 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 - -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 +//PINOUTS 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 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 SD_CS_PIN = 0; +const int FLASH_CS_PIN = 0; +const int IMU_1_PIN=0; +const int IMU_2_PIN=0; +const int R_LED_PIN=0; +const int B_LED_PIN=0; +const int G_LED_PIN=0; -const float SERVO_SPEED; +// Thrust-Vector Controller (TVC) //TODO: modify values +const float GEAR = 9; // gearing ratio of the servo to the TVC +const float SERVO_SPEED=.1; 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 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 // 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 +//rocket physical parameters 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 +const float MASS=1; //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, +float A_VALUES [36] = {0,THRUST/MASS,0,0,0,0, + 0,0,1,0,0,0, 0,0,0,0,0,0, + 0,0,0,-THRUST/MASS,0,0, + 0,0,0,0,0,1, 0,0,0,0,0,0}; //dynamics matrix Matrix A=Matrix(6, 6, A_VALUES); - float B_VALUES [12] = {0, 0, - 0, 0, + float B_VALUES [12] = {THRUST/MASS, 0, 0, 0, + -THRUST*MOMENT_ARM/MOMENT_INERTIA_YY, 0, + 0, -THRUST/MASS, 0, 0, - 0, 0, - 0, 0}; //input matrix + 0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix Matrix B=Matrix(6, 2, B_VALUES); float K_VALUES [12] = {0,0,0,0,0,0, 0,0,0,0,0,0}; //controller gain -Matrix K=Matrix(2, 6, K_VALUES); +Matrix KC=Matrix(2, 6, K_VALUES); float C_VALUES [24] = {1,0,0,0,0,0, 0,0,1,0,0,0, @@ -127,5 +120,4 @@ Matrix C=Matrix(4, 6, C_VALUES); 0, 0, 0, 0}; //kalman gain Matrix L=Matrix(6, 4, L_VALUES); - -#endif // __MISSION_CONSTANTS_HH__ \ No newline at end of file +#endif diff --git a/main/moding.cpp b/main/moding.cpp index be87e9c..50c4689 100644 --- a/main/moding.cpp +++ b/main/moding.cpp @@ -1,30 +1,30 @@ #include "mission_constants.hh" #include "moding.hh" -bool change_mode_coundown(Mode current_mode, bool cond) +bool change_mode_to_countdown(bool cond) { bool ret = false; - if (current_mode == STARTUP_STABLE && cond) + if (cond) { ret = true; } return ret; } -bool change_mode_to_final_countdown(Mode current_mode, bool cond) +bool change_mode_to_final_countdown(bool cond) { bool ret = false; - if (current_mode == COUNTDOWN && cond) + if (cond) { ret = true; } return ret; } -bool change_mode_to_prep_tvc(Mode current_mode, bool cond) +bool change_mode_to_prep_tvc(bool cond) { bool ret = false; - if (current_mode == FINAL_COUNTDOWN && cond) + if (cond) { ret = true; } @@ -39,10 +39,10 @@ bool change_mode_to_prep_tvc(Mode current_mode, bool cond) * - current mode is NAVIGATION * - TODO: determine any remaining conditions */ -bool change_mode_to_burn_baby_burn(Mode current_mode, bool cond) +bool change_mode_to_burn_baby_burn(bool cond) { bool ret = false; - if (current_mode == PREP_TVC && cond) + if (cond) { ret = true; } @@ -58,10 +58,10 @@ bool change_mode_to_burn_baby_burn(Mode current_mode, bool cond) * - current mode is BURN_BABY_BURN * - TODO: determine any remaining conditions */ -bool change_mode_to_shutdown_stable(Mode current_mode, bool cond) +bool change_mode_to_shutdown_stable(bool cond) { bool ret = false; - if (current_mode == BURN_BABY_BURN && cond) + if (cond) { ret = true; } @@ -74,19 +74,16 @@ void transition_to_countdown() { //TODO : change led //reset navigation - //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 } @@ -97,7 +94,6 @@ void transition_to_prep_tvc() */ void transition_to_burn_baby_burn() { - //set stop time //TODO: change LED } @@ -111,4 +107,4 @@ void transition_to_shutdown_stable() //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 3267380..45d690d 100644 --- a/main/moding.hh +++ b/main/moding.hh @@ -17,4 +17,4 @@ void transition_to_prep_tvc(); void transition_to_burn_baby_burn(); void transition_to_shutdown_stable(); -#endif // __MODING_HH__ \ No newline at end of file +#endif // __MODING_HH__ diff --git a/main/workspace.hh b/main/workspace.hh index d8bfb2d..9fdd8a5 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -4,8 +4,9 @@ #include #include "mission_constants.hh" #include "moding.hh" -//#include // TODO: what's this? -//#include "Wire.h" // Arduino library +#include "matrix.hh" +#include // TODO: what's this? +#include "Wire.h" // Arduino library /* Workspace @@ -23,8 +24,8 @@ class Workspace //***************************************************************************** // HARDWARE SETUP //***************************************************************************** - MPU6050 imu_0(0x68); // (--) first IMU to read data from - MPU6050 imu_1(0x69); // (--) second IMU to read data from + struct bno055_t myBNO; + struct bno055_euler myEulerData; //***************************************************************************** // LOOP VARIABLES @@ -34,28 +35,30 @@ class Workspace 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 - //event times - unsigned long drop_time=0; - unsigned long fire_time=0; - // 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 + float theta_0[3] = {0.0, 0.0, 0.0}; // (rad/s) angular velocity, used for storing sensor measurements + float theta_1[3] = {0.0, 0.0, 0.0}; // (rad/s) angular velocity, used for storing sensor measurements // flight mode Mode mode = STARTUP_STABLE; + int next_mode_time=0; // Servos Servo tvc_x; // servo that actuates TVC around x body axis Servo tvc_y; // servo that actuates TVC around x body axis //control vectors - 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 - Matrix last_u; + // set up controller + float initialize_6 [6]={0, 0, 0, 0, 0, 0}; + float initialize_4 [4]={0, 0, 0, 0}; + float initialize_2 [2]={0, 0}; + Matrix x=Matrix(6, 1, initialize_6); + Matrix y=Matrix(4, 1, initialize_4); + Matrix u=Matrix(2, 1, initialize_2); + Matrix last_u=Matrix(2, 1, initialize_2); + float y_values [4]= {0, 0, 0, 0}; float yaw; // state @@ -63,6 +66,15 @@ class Workspace 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 float qr_body_wrt_inrt[4] = {1.0, 0.0, 0.0, 0.0}; // (--) right, scalar-first, Hamiltonian quaternion transforming body frame into inertial frame float qr_inrt_wrt_body[4] = {1.0, 0.0, 0.0, 0.0}; // (--) right, scalar-first, Hamiltonian quaternion transforming inertial frame into body frame + + void construct_y() + { + y_values[0]=0; + y_values[1]=(theta_0[1]+theta_1[1])/2; + y_values[2]= 0; + y_values[3]= (theta_0[0]+theta_1[0])/2; + y.redefine(y_values); + } }; -#endif // __WORKSPACE_HH__ \ No newline at end of file +#endif // __WORKSPACE_HH__