Skip to content

Commit

Permalink
tvc update
Browse files Browse the repository at this point in the history
  • Loading branch information
LiamSmego committed Mar 25, 2021
1 parent 9d03cb7 commit f313f98
Show file tree
Hide file tree
Showing 3 changed files with 89 additions and 43 deletions.
74 changes: 48 additions & 26 deletions main/main.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,32 +35,50 @@ void main_display_matrix(Matrix param, float scale)
Matrix send_tvc(Matrix u, Matrix last_u, float yaw)
{
//scale down angle to physical tvc limit
float input_magnitude=powf((powf(u.select(1, 1), 2)+powf(u.select(2, 1), 2)), 0.5);
float input_magnitude=sqrtf(u.values[0]*u.values[0]+u.values[1]*u.values[1]);
if (input_magnitude>MAX_U)
{
u.scale(MAX_U/input_magnitude);
}
main_display_matrix(u, RAD_2_DEG);
//send
float travel_magnitude=powf((powf(u.select(1, 1)-last_u.select(1, 1), 2)+powf(u.select(2, 1)-last_u.select(2, 1), 2)), 0.5);
if (travel_magnitude>SERVO_SPEED*ws.dt/MEGA)
//main_display_matrix(u, RAD_2_DEG);

if(NULL_U_X_AXIS){ u.values[0]=0; }
if(NULL_U_Y_AXIS){ u.values[1]=0; }
//main_display_matrix(u, RAD_2_DEG);

vector<float> rotation_values {cos(yaw), -sin(yaw), sin(yaw), cos(yaw)};
Matrix R=Matrix(2, 2, rotation_values);
u=R*u;
//main_display_matrix(u, RAD_2_DEG);


Matrix travel=u-last_u;
float travel_magnitude=sqrtf(travel.values[0]*travel.values[0]+travel.values[1]*travel.values[1]);
/*
if (travel_magnitude<MIN_STEP)
{
u=last_u;
}
else if (travel_magnitude>SERVO_SPEED*ws.dt)
{
u=last_u+(u-last_u).scale(SERVO_SPEED*ws.dt/MEGA/travel_magnitude);
u=last_u+travel.scale(SERVO_SPEED*ws.dt/travel_magnitude);
}
main_display_matrix(u, RAD_2_DEG);
*/
//main_display_matrix(u, RAD_2_DEG);




//rotate input are body z axis
float gamma=yaw+BETA;
vector<float> rotation_values {cos(gamma), -sin(gamma), sin(gamma), cos(gamma)};
Matrix R=Matrix(2, 2, rotation_values);
rotation_values = {cos(BETA), -sin(BETA), sin(BETA), cos(BETA)};
R.values=rotation_values;
Matrix new_u=R*u;
main_display_matrix(u, RAD_2_DEG);
Serial.println(" ");

//convert to degrees, gear the angle, and round
if (!NULL_U_X_AXIS) {ws.tvc_x.write(round(GEAR*RAD_2_DEG*new_u.select(1, 1))+TVC_X_OFFSET);}
if (!NULL_U_Y_AXIS) {ws.tvc_y.write(round(GEAR*RAD_2_DEG*new_u.select(2, 1))+TVC_Y_OFFSET);}

ws.tvc_x.write(round(GEAR*RAD_2_DEG*new_u.values[0])+TVC_X_OFFSET);
ws.tvc_y.write(round(GEAR*RAD_2_DEG*new_u.values[1])+TVC_Y_OFFSET);
delay(1);

return u;
}
Expand All @@ -78,6 +96,9 @@ void setup()

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

pinMode(G_LED_PIN, OUTPUT);
digitalWrite(G_LED_PIN, HIGH);

//TODO flash setup
//flash.begin(9600); // begins flash chip at specified baud rate
Expand Down Expand Up @@ -125,8 +146,9 @@ void setup()
void loop()
{
// update the clock
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
unsigned long current_time=micros();
ws.dt = float((current_time - ws.t_prev_cycle)/MEGA); // (us) time step since previous loop
ws.t_prev_cycle = current_time; // (us) update time for next loop

bno055_read_euler_hrp(&ws.euler_1);
bno055_read_euler_hrp(&ws.euler_2);
Expand All @@ -144,7 +166,8 @@ void loop()
}
else
{

ws.construct_y();
ws.construct_x(false);
}
break;
}
Expand All @@ -159,25 +182,22 @@ void loop()
else
{
ws.construct_y();
ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt/MEGA); //state estimation only using sensors
ws.construct_x(false);
}
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/MEGA); //state estimation only using sensors
ws.construct_x(false);
}
break;
}
Expand All @@ -188,13 +208,14 @@ void loop()
transition_to_burn_baby_burn();
ws.next_mode_time=millis()+BURN_BABY_BURN_PERIOD*KILO_I;
ws.mode = BURN_BABY_BURN;
digitalWrite(G_LED_PIN, LOW);
}
else
{
ws.construct_y();
ws.u=(KC*ws.x).scale(-1);//calculate input
ws.last_u=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
ws.construct_x(false);
}
break;
}
Expand All @@ -210,19 +231,20 @@ void loop()
ws.construct_y();
ws.u=(KC*ws.x).scale(-1); //calculate input
ws.last_u=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
ws.construct_x(KALMAN_ENABLED);
}
break;
}
case (SHUTDOWN_STABLE):
{
// TODO: implement this block
ws.construct_y();
ws.construct_x(false);
break;
}

}
Serial.println(ws.mode);

main_display_matrix(ws.x, 1);
//Serial.print(millis());
//main_display_matrix(ws.last_u, RAD_2_DEG);
// TODO: add data record
Expand Down
22 changes: 12 additions & 10 deletions main/mission_constants.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ enum Mode {
SHUTDOWN_STABLE = 5
};

const bool KALMAN_ENABLED=false;
const bool NULL_U_X_AXIS=false;
const bool NULL_U_Y_AXIS=false;

Expand All @@ -41,7 +42,7 @@ const int STARTUP_STABLE_PERIOD=2; //15
const int COUNTDOWN_PERIOD=2; //20
const int FINAL_COUNTDOWN_PERIOD=2;
const int PREP_TVC_PERIOD=3;
const int BURN_BABY_BURN_PERIOD=120; //15
const int BURN_BABY_BURN_PERIOD=300; //15

//*****************************************************************************
// HARDWARE
Expand Down Expand Up @@ -69,11 +70,12 @@ 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 int G_LED_PIN=4;

// Thrust-Vector Controller (TVC) //TODO: modify values
const float GEAR = 6; // gearing ratio of the servo to the TVC //9
const float SERVO_SPEED=1;
const float GEAR = 9; // gearing ratio of the servo to the TVC //9
const float SERVO_SPEED=3;
const float MIN_STEP=DEG_2_RAD/GEAR;
const float TVC_X_OFFSET = 100; // TODO: add description
const float TVC_Y_OFFSET = 55; // TODO: add description
const float MAX_U= 5*DEG_2_RAD; //maximum gimbal angle
Expand All @@ -89,10 +91,10 @@ const float EULER_Y_OFFSET;

//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;
const float MOMENT_ARM=.2413; //TODO: make exact
const float MOMENT_INERTIA_XX=.01984; //TODO: make exact
const float MOMENT_INERTIA_YY=.01669;//TODO: make exact
const float MASS=.611;

//control constants TODO: fill these out
const vector<float> A_VALUES {0,THRUST/MASS,0,0,0,0,
Expand All @@ -111,8 +113,8 @@ const vector<float> B_VALUES {THRUST/MASS, 0,
0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix
const Matrix B=Matrix(6, 2, B_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 vector<float> K_VALUES {-.6325,-5.5608,-1.4869,0,0,0,
0,0,0,.6325,-5.5608,-1.4869}; //controller gain
const Matrix KC=Matrix(2, 6, K_VALUES);

const vector<float> C_VALUES {1,0,0,0,0,0,
Expand Down
36 changes: 29 additions & 7 deletions main/workspace.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,11 +60,11 @@ class Workspace
vector<float> initialize_4 {0, 0, 0, 0};
vector<float> initialize_2 {0, 0};
Matrix x=Matrix(6, 1, initialize_6);
Matrix x_raw=Matrix(6, 1, initialize_6);
Matrix y=Matrix(4, 1, initialize_4);
Matrix last_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 @@ -75,19 +75,41 @@ class Workspace

void construct_y()
{
last_y.values=y.values;

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.0*DEG_2_RAD;
y_values[2]= 0;
y_values[3]= (theta_0[0]+theta_1[0])/2.0*DEG_2_RAD;
y.values[0]=0;
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.0*DEG_2_RAD;
yaw=(theta_0[2]+theta_1[2])/2.0*DEG_2_RAD;
y.redefine(y_values);
}

void construct_x(bool kalman)
{
x_raw.values[0]=0;
x_raw.values[1]=y.values[1];
x_raw.values[2]=(y.values[1]-last_y.values[1])/dt;
x_raw.values[3]=0;
x_raw.values[4]=y.values[3];
x_raw.values[5]=(y.values[3]-last_y.values[3])/dt;

if (kalman)
{
x=x+(A*x+B*last_u+L*(y-(C*x))).scale(dt);
x.values[0]=0;
x.values[3]=0;
}
else
{
x.values=x_raw.values;
}
}
};

Expand Down

0 comments on commit f313f98

Please sign in to comment.