diff --git a/src/main.ino b/src/main.ino index c5fdc13..7a0ce12 100644 --- a/src/main.ino +++ b/src/main.ino @@ -1,20 +1,20 @@ #include // C STD Int library, might not be needed -// #include // TODO: what's this and why is it commented-out? -#include // TODO: what's this? +// #include // another of version of Eigen313.h +#include // linear algebra library #include "I2Cdev.h" -#include // TODO: what's this? +#include // needed for Eigen313.h to work #include "mission_constants.hh" #include "moding.hh" #include "MPU6050.h" // MPU 6050 IMU Library #include // TODO: what's this? -#include // TODO: what's this? +#include // 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 } \ No newline at end of file diff --git a/src/mission_constants.hh b/src/mission_constants.hh index 6fe4ddc..f8c86c5 100644 --- a/src/mission_constants.hh +++ b/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 diff --git a/src/workspace.hh b/src/workspace.hh index bf93ff1..594796e 100644 --- a/src/workspace.hh +++ b/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