Skip to content

Commit

Permalink
edits_before_size_testing
Browse files Browse the repository at this point in the history
  • Loading branch information
LiamSmego committed Mar 21, 2021
1 parent 3f3a69f commit e821600
Show file tree
Hide file tree
Showing 7 changed files with 124 additions and 130 deletions.
115 changes: 51 additions & 64 deletions main/main.ino
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,8 @@
#include <cstdint> // C STD Int library, might not be needed
#include "I2Cdev.h"
#include "moding.hh"
#include <SH.h> // TODO: what's this?
#include <SPIFlash.h> // flash chip library
#include "Wire.h" // Arduino library
//#include <SPIFlash.h> // flash chip library
#include "BNO055_support.h" //Contains the bridge code between the API and Arduino
#include <Wire.h>
#include "workspace.hh" // variable storage
#include <Servo.h>
#include "matrix.hh"
Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -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;
Expand All @@ -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;
}


Expand All @@ -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;
}
Expand All @@ -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
}
}
5 changes: 5 additions & 0 deletions main/matrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,11 @@ void display_matrix(Matrix A)
}
}

void Matrix:: redefine(float* new_values)
{
values=new_values;
}

/** for testing
int main()
{
Expand Down
6 changes: 4 additions & 2 deletions main/matrix.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -31,4 +33,4 @@ class Matrix

void display_matrix(Matrix);

#endif // __MODING_HH__
#endif // __MODING_HH__
58 changes: 25 additions & 33 deletions main/mission_constants.hh
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,8 @@
#ifndef __MISSION_CONSTANTS_HH__
#define __MISSION_CONSTANTS_HH__

#include <cstdint>
#include "matrix.hh"


//*****************************************************************************
// CONVERSION FACTORS
//*****************************************************************************
Expand Down Expand Up @@ -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;
Expand All @@ -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,
Expand All @@ -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__
#endif
Loading

0 comments on commit e821600

Please sign in to comment.