Skip to content

Commit

Permalink
axis remap and double imu implemented
Browse files Browse the repository at this point in the history
  • Loading branch information
LiamSmego committed Mar 24, 2021
1 parent 60e8bb8 commit 11b7948
Show file tree
Hide file tree
Showing 5 changed files with 78 additions and 54 deletions.
61 changes: 33 additions & 28 deletions main/main.ino
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
#include "I2Cdev.h"
#include "moding.hh"
//#include <SPIFlash.h> // flash chip 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"
#include "mission_constants.hh"
#include <cmath>

#include <vector>
using namespace std;

// declare flash chip
Expand All @@ -18,14 +18,11 @@ using namespace std;
// (--) instance of workspace class storing all the variables used in the loop
Workspace ws;

int count=0;


void main_display_matrix(Matrix param)
{
for (int i=0; i<param.rows*param.columns; i++)
{
Serial.print(param.values[i]);
Serial.print(param.values[i], 4);
Serial.print(" ");
}
Serial.println(" ");
Expand Down Expand Up @@ -71,8 +68,11 @@ void send_tvc(Matrix u, Matrix * last_u, double yaw)
void setup()
{
// set up pins (OUTPUT and LOW are defined in Arduino.h)
pinMode(15, OUTPUT);
pinMode(15, INPUT);
digitalWrite(15, HIGH);

pinMode(0, INPUT);
digitalWrite(0, LOW);

//TODO flash setup
//flash.begin(9600); // begins flash chip at specified baud rate
Expand All @@ -88,7 +88,16 @@ void setup()
Wire.begin();

//Initialization of the BNO055
BNO_Init(&ws.myBNO); //Assigning the structure to hold information about the device
BNO_Init(&ws.bno_1); //Assigning the structure to hold information about the device
BNO_Init(&ws.bno_2); //Assigning the structure to hold information about the device

bno055_set_axis_remap_value(0X06);
delay(1);
bno055_set_z_remap_sign(0x01);
delay(1);
bno055_set_z_remap_sign(0x01);
delay(1);


//Configuration to NDoF mode
bno055_set_operation_mode(OPERATION_MODE_NDOF);
Expand All @@ -108,22 +117,11 @@ void setup()
void loop()
{
// update the clock
ws.dt = micros() - ws.t_prev_cycle; // (us) time step since previous loop
ws.dt = float(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;

//Serial.print(float(ws.myEulerData.h/16.00));
//Serial.print(" ");
//Serial.print(float(ws.myEulerData.r/16.00));
//Serial.print(" ");
//Serial.println(float(ws.myEulerData.p/16.00));

ws.theta_0[0]=float(ws.myEulerData.r/16.00);
ws.theta_1[0]=float(ws.myEulerData.r/16.00);
ws.theta_0[1]=float(ws.myEulerData.p/16.00);
ws.theta_1[1]=float(ws.myEulerData.p/16.00);
bno055_read_euler_hrp(&ws.euler_1);
bno055_read_euler_hrp(&ws.euler_2);

// check for moding change conditions
switch (ws.mode)
Expand Down Expand Up @@ -153,22 +151,25 @@ void loop()
else
{
ws.construct_y();
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(FINAL_COUNTDOWN):
{
if (change_mode_to_prep_tvc(millis() > ws.next_mode_time))
{
digitalWrite(15, HIGH);
delay(1);
digitalWrite(15, HIGH);
transition_to_prep_tvc();
ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I;
ws.mode = PREP_TVC;
}
else
{
ws.construct_y();
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;
}
Expand All @@ -184,8 +185,8 @@ void loop()
{
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
//send_tvc(ws.u, &ws.last_u, ws.yaw);
ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors
}
break;
}
Expand All @@ -200,8 +201,8 @@ void loop()
{
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); //state estimation using Kalman filter
//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 @@ -212,6 +213,10 @@ void loop()
}

}
Serial.println(ws.mode);

//Serial.print(millis());
main_display_matrix(ws.x);
// TODO: add data record
// TODO: add (somewhere else) data struct
}
1 change: 0 additions & 1 deletion main/matrix.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#include "matrix.hh"
#include <string>
#include <vector>

using namespace std;

//matrix multiplication
Expand Down
1 change: 1 addition & 0 deletions main/matrix.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <cstdbool>
#include <cstdint>
#include <string>
#include <vector>
using namespace std;


Expand Down
39 changes: 22 additions & 17 deletions main/mission_constants.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@

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

using namespace std;

//*****************************************************************************
Expand Down Expand Up @@ -32,6 +31,8 @@ enum Mode {
SHUTDOWN_STABLE = 5
};

const bool RESTRICT_U_AXIS=false;

//most modes change by time
//after a predefined time period we switch to the next mode
//MODE TIME PERIODS
Expand Down Expand Up @@ -81,6 +82,10 @@ const float BETA = 0.95; // angle (rad) that corrects for misalignment between
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

//IMU parameters
const float EULER_X_OFFSET;
const float EULER_Y_OFFSET;

//rocket physical parameters
const float THRUST=10; //TODO: make exact
const float MOMENT_ARM=.265; //TODO: make exact
Expand All @@ -89,38 +94,38 @@ const float MOMENT_INERTIA_YY=1;//TODO: make exact
const float MASS=1;

//control constants TODO: fill these out
vector<float> A_VALUES {0,THRUST/MASS,0,0,0,0,
const vector<float> A_VALUES {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);
const Matrix A=Matrix(6, 6, A_VALUES);

vector<float> B_VALUES {THRUST/MASS, 0,
const vector<float> B_VALUES {THRUST/MASS, 0,
0, 0,
-THRUST*MOMENT_ARM/MOMENT_INERTIA_YY, 0,
0, -THRUST/MASS,
0, 0,
0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix
Matrix B=Matrix(6, 2, B_VALUES);
const Matrix B=Matrix(6, 2, B_VALUES);

vector<float> K_VALUES {0,0,0,0,0,0,
0,0,0,0,0,0}; //controller gain
Matrix KC=Matrix(2, 6, K_VALUES);
const vector<float> K_VALUES {-2.582,-6.9449,-.5665,0,0,0,
0,0,0,2.582,-6.9449,-.5665}; //controller gain
const Matrix KC=Matrix(2, 6, K_VALUES);

vector<float> C_VALUES {1,0,0,0,0,0,
const vector<float> C_VALUES {1,0,0,0,0,0,
0,0,1,0,0,0,
0,0,0,1,0,0,
0,0,0,0,0,1}; //sensor matrix
Matrix C=Matrix(4, 6, C_VALUES);
const Matrix C=Matrix(4, 6, C_VALUES);

vector<float> L_VALUES {0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0}; //kalman gain
Matrix L=Matrix(6, 4, L_VALUES);
const vector<float> L_VALUES {13.824, 6.9192, 0, 0,
6.9192, 8.3015, 0, 0,
20.0079, 53.3949, 0, 0,
0, 0, 13.824, -6.9192,
0, 0, -6.9192, 8.3015,
0, 0, -20.0079, 53.3949}; //kalman gain
const Matrix L=Matrix(6, 4, L_VALUES);

#endif
30 changes: 22 additions & 8 deletions main/workspace.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include "matrix.hh"
#include <Servo.h> // TODO: what's this?
#include "Wire.h" // Arduino library
#include <vector>
using namespace std;


/* Workspace
Expand All @@ -24,16 +26,19 @@ class Workspace
//*****************************************************************************
// HARDWARE SETUP
//*****************************************************************************
struct bno055_t myBNO;
struct bno055_euler myEulerData;
struct bno055_t bno_1;
struct bno055_euler euler_1;

struct bno055_t bno_2;
struct bno055_euler euler_2;

//*****************************************************************************
// LOOP VARIABLES
//*****************************************************************************
// clock time
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
float 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_0[3] = {0.0, 0.0, 0.0}; // (m/s^2) linear acceleration, used for storing sensor measurements
Expand All @@ -51,14 +56,15 @@ class Workspace

//control vectors
// 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};
vector<float> initialize_6 {0, 0, 0, 0, 0, 0};
vector<float> initialize_4 {0, 0, 0, 0};
vector<float> initialize_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);
vector<float> y_values {0, 0, 0, 0};
vector<float> last_y_values {0, 0, 0, 0};
float yaw;

// state
Expand All @@ -69,10 +75,18 @@ class Workspace

void construct_y()
{
theta_0[2]=float(euler_1.h/16.00);
theta_1[2]=float(euler_2.h/16.00);
theta_0[1]=float(euler_1.r/16.00);
theta_1[1]=float(euler_2.r/16.00);
theta_0[0]=float(euler_1.p/16.00);
theta_1[0]=float(euler_2.p/16.00);

y_values[0]=0;
y_values[1]=(theta_0[1]+theta_1[1])/2;
y_values[1]=(theta_0[1]+theta_1[1])/2.0*DEG_2_RAD;
y_values[2]= 0;
y_values[3]= (theta_0[0]+theta_1[0])/2;
y_values[3]= (theta_0[0]+theta_1[0])/2.0*DEG_2_RAD;
yaw=(theta_0[2]+theta_1[2])/2.0*DEG_2_RAD;
y.redefine(y_values);
}
};
Expand Down

0 comments on commit 11b7948

Please sign in to comment.