Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-07-25 23:51:35 +00:00
Code Releases Activity

fixed LQR

Browse Source
This commit is contained in:
bpmcgeeney
2021-09-12 14:18:22 -07:00
parent 90311ff17e
commit c6b1f0fc2c
14 changed files with 8786 additions and 4339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/main.cpp
View File

@@ -14,22 +14,23 @@ int main()
Vars.vz = 0; // [m/s]
// Initial YPR
Vars.yaw = 0; // [rad]
Vars.pitch = 0; // [rad]
Vars.roll = 0; // [rad]
Vars.yaw = 15 * 3.1416 / 180; // [rad]
Vars.pitch = 0 * 3.1416 / 180; // [rad]
Vars.roll = 0 * 3.1416 / 180; // [rad]
// Initial YPRdot
Vars.yawdot = 0; // [rad/s]
Vars.pitchdot = 0; // [rad/s]
Vars.rolldot = 0; // [rad/s]
Vars.yawdot = 0 * 3.1416 / 180; // [rad/s]
Vars.pitchdot = 0 * 3.1416 / 180; // [rad/s]
Vars.rolldot = 0 * 3.1416 / 180; // [rad/s]
// Servo Limitation
Vars.maxServo = 0; // [degs]
Vars.maxServo = 15; // [degs]
// Vehicle Properties
Vars.m0 = 1.2; // [kg]
Vars.vehicleHeight = 0.5318; // [m]
Vars.vehicleRadius = 0.05105; // [m]
Vars.momentArm = 0.145; // [m]
// Sim Step Size
Vars.stepSize = 1; // [ms]
@@ -43,6 +44,7 @@ int main()
sim(Vars);
std::cout << "Finished";
std::cin.get();
return 0;
}

70
src/sim.cpp
View File

@@ -28,7 +28,7 @@ void sim(struct sVars &Vars)
// Output file header. These are the variables that we output - useful for
// debugging
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
"pitchdot, rolldot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
"pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
"LQRx, LQRy"
<< std::endl;
@@ -108,8 +108,7 @@ void thrustSelection(struct sVars &Vars, int t)
}
else
Vars.burnElapsed =
2000; // 2000 just an arbitrary number to make sure we don't burn
Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
bool b_thrustSelect;
@@ -135,12 +134,15 @@ void thrustSelection(struct sVars &Vars, int t)
void lqrCalc(struct sVars &Vars)
{
Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
pow(Vars.vehicleRadius, 2) / 4);
Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
pow(Vars.vehicleRadius, 2) / 4);
Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2);
/*
double n = sqrt(398600 / pow(6678, 3));
double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
@@ -164,6 +166,7 @@ void lqrCalc(struct sVars &Vars)
Algorithm taken from LQR wikipedia page:
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
*/
/*
double gain = 0.25;
double K11 =
@@ -190,9 +193,47 @@ void lqrCalc(struct sVars &Vars)
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
double momentArm = 0.145;
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / momentArm;
Vars.LQRy = (K22 * Vars.pitch / 2) / momentArm;
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm;
*/
double K11 = 39.54316;
double K12 = 0.00000;
double K13 = -0.00000;
double K14 = 39.55769;
double K15 = 0.00000;
double K16 = 0.00000;
double K21 = 0.00000;
double K22 = 39.54316;
double K23 = 0.00000;
double K24 = 0.00000;
double K25 = 39.55769;
double K26 = 0.00000;
double K31 = 0.00000;
double K32 = 0.00000;
double K33 = 39.54316;
double K34 = 0.00000;
double K35 = 0.00000;
double K36 = 39.54394;
double gain = 0.25 * pow(10, -4);
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
Vars.LQRx = gain * ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 + K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) / -Vars.momentArm;
Vars.LQRy = gain * ((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 + K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) / -Vars.momentArm;
// LQR Force limiter X
if (Vars.LQRx > Vars.thrust)
Vars.LQRx = Vars.thrust;
else if (Vars.LQRx < -1 * Vars.thrust)
Vars.LQRx = -1 * Vars.thrust;
// LQR Force limiter Y
if (Vars.LQRy > Vars.thrust)
Vars.LQRy = Vars.thrust;
else if (Vars.LQRy < -1 * Vars.thrust)
Vars.LQRy = -1 * Vars.thrust;
}
void TVC(struct sVars &Vars, double g)
@@ -236,9 +277,8 @@ void TVC(struct sVars &Vars, double g)
(Vars.m * g);
// Calculate moment created by Fx and Fy
double momentArm = 0.145;
Vars.momentX = Vars.Fx * momentArm;
Vars.momentY = Vars.Fy * momentArm;
Vars.momentX = Vars.Fx * Vars.momentArm;
Vars.momentY = Vars.Fy * Vars.momentArm;
Vars.momentZ = 0;
}
}
@@ -378,6 +418,18 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t)
outfile << Vars.pitchdot * 180 / 3.1416 << ", ";
outfile << Vars.rolldot * 180 / 3.1416 << ", ";
outfile << Vars.yawddot * 180 / 3.1416 << ", ";
outfile << Vars.pitchddot * 180 / 3.1416 << ", ";
outfile << Vars.rollddot * 180 / 3.1416 << ", ";
outfile << Vars.I11 << ", ";
outfile << Vars.I22 << ", ";
outfile << Vars.I33 << ", ";
outfile << Vars.I11dot << ", ";
outfile << Vars.I22dot << ", ";
outfile << Vars.I33dot << ", ";
outfile << Vars.xServoDegs << ", ";
outfile << Vars.yServoDegs << ", ";