mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-07-23 14:41:25 +00:00
changed all LQR references to PID
This commit is contained in:
@@ -22,7 +22,7 @@ struct Vehicle {
|
||||
double thrust, burnElapsed, burnStart;
|
||||
bool thrustFiring = false;
|
||||
|
||||
double LQRx, LQRy, Fx, Fy, Fz;
|
||||
double PIDx, PIDy, Fx, Fy, Fz;
|
||||
double momentX, momentY, momentZ;
|
||||
|
||||
double I11, I22, I33;
|
||||
|
||||
@@ -31,8 +31,8 @@ struct outVector {
|
||||
|
||||
std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
|
||||
|
||||
std::vector<double> LQRx = std::vector<double>(length, 0.0);
|
||||
std::vector<double> LQRy = std::vector<double>(length, 0.0);
|
||||
std::vector<double> PIDx = std::vector<double>(length, 0.0);
|
||||
std::vector<double> PIDy = std::vector<double>(length, 0.0);
|
||||
};
|
||||
|
||||
#endif
|
||||
102
include/sim.h
102
include/sim.h
@@ -3,7 +3,6 @@
|
||||
|
||||
void burnStartTimeCalc(struct Vehicle &);
|
||||
void thrustSelection(struct Vehicle &, int t);
|
||||
void lqrCalc(struct Vehicle &);
|
||||
void pidController(struct Vehicle &, struct Vehicle &);
|
||||
void TVC(struct Vehicle &);
|
||||
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
|
||||
@@ -31,7 +30,6 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
|
||||
vehicleDynamics(State, PrevState, t);
|
||||
thrustSelection(State, t);
|
||||
pidController(State, PrevState);
|
||||
// lqrCalc(State);
|
||||
TVC(State);
|
||||
state2vec(State, PrevState, stateVector, t);
|
||||
|
||||
@@ -203,63 +201,6 @@ void thrustSelection(Vehicle &State, int t) {
|
||||
}
|
||||
}
|
||||
|
||||
void lqrCalc(Vehicle &State) {
|
||||
|
||||
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
|
||||
pow(State.vehicleRadius, 2) / 4);
|
||||
State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
|
||||
pow(State.vehicleRadius, 2) / 4);
|
||||
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
|
||||
|
||||
// Paste in Values from gainCalc.m
|
||||
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;
|
||||
|
||||
// changing gain exponent drastically changes results of LQR
|
||||
double gain = 0.25 * pow(10, -4);
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment
|
||||
// arm
|
||||
State.LQRx =
|
||||
gain *
|
||||
((K12 * State.pitch) / 2 + K15 * State.pitchdot + (K13 * State.roll) / 2 +
|
||||
K16 * State.rolldot + (K11 * State.yaw) / 2 + K14 * State.yawdot) /
|
||||
-State.momentArm;
|
||||
State.LQRy =
|
||||
gain *
|
||||
((K22 * State.pitch) / 2 + K25 * State.pitchdot + (K23 * State.roll) / 2 +
|
||||
K26 * State.rolldot + (K21 * State.yaw) / 2 + K24 * State.yawdot) /
|
||||
-State.momentArm;
|
||||
|
||||
// LQR Force limiter X
|
||||
if (State.LQRx > State.thrust)
|
||||
State.LQRx = State.thrust;
|
||||
else if (State.LQRx < -1 * State.thrust)
|
||||
State.LQRx = -1 * State.thrust;
|
||||
|
||||
// LQR Force limiter Y
|
||||
if (State.LQRy > State.thrust)
|
||||
State.LQRy = State.thrust;
|
||||
else if (State.LQRy < -1 * State.thrust)
|
||||
State.LQRy = -1 * State.thrust;
|
||||
}
|
||||
|
||||
void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
||||
// Make sure we start reacting when we start burning
|
||||
if (State.thrust > 0.01) {
|
||||
@@ -275,32 +216,31 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
||||
State.d_yError = derivative(State.yError, PrevState.yError, State.stepSize);
|
||||
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
|
||||
|
||||
// PID Function - it says LQR but this is just so that it gets passed to the
|
||||
// TVC block properly
|
||||
|
||||
State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
||||
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
||||
State.Kd * State.d_yError) /
|
||||
State.momentArm;
|
||||
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
|
||||
State.PIDy = (State.Kp * State.pError + State.Ki * State.i_pError +
|
||||
State.Kd * State.d_pError) /
|
||||
State.momentArm;
|
||||
|
||||
} else {
|
||||
State.LQRx = 0;
|
||||
State.LQRy = 0;
|
||||
State.PIDx = 0;
|
||||
State.PIDy = 0;
|
||||
}
|
||||
|
||||
// LQR Force limiter X
|
||||
if (State.LQRx > State.thrust)
|
||||
State.LQRx = State.thrust;
|
||||
else if (State.LQRx < -1 * State.thrust)
|
||||
State.LQRx = -1 * State.thrust;
|
||||
// PID Force limiter X
|
||||
if (State.PIDx > State.thrust)
|
||||
State.PIDx = State.thrust;
|
||||
else if (State.PIDx < -1 * State.thrust)
|
||||
State.PIDx = -1 * State.thrust;
|
||||
|
||||
// LQR Force limiter Y
|
||||
if (State.LQRy > State.thrust)
|
||||
State.LQRy = State.thrust;
|
||||
else if (State.LQRy < -1 * State.thrust)
|
||||
State.LQRy = -1 * State.thrust;
|
||||
// PID Force limiter Y
|
||||
if (State.PIDy > State.thrust)
|
||||
State.PIDy = State.thrust;
|
||||
else if (State.PIDy < -1 * State.thrust)
|
||||
State.PIDy = -1 * State.thrust;
|
||||
}
|
||||
|
||||
void TVC(Vehicle &State) {
|
||||
@@ -316,7 +256,7 @@ void TVC(Vehicle &State) {
|
||||
|
||||
} else {
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
State.xServoDegs = (180 / M_PI) * asin(State.LQRx / State.thrust);
|
||||
State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
|
||||
|
||||
// Servo position limiter
|
||||
if (State.xServoDegs > State.maxServo)
|
||||
@@ -325,7 +265,7 @@ void TVC(Vehicle &State) {
|
||||
State.xServoDegs = -1 * State.maxServo;
|
||||
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
State.yServoDegs = (180 / M_PI) * asin(State.LQRy / State.thrust);
|
||||
State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
|
||||
|
||||
// Servo position limiter
|
||||
if (State.yServoDegs > State.maxServo)
|
||||
@@ -374,8 +314,8 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
|
||||
|
||||
stateVector.thrustFiring[t] = State.thrustFiring;
|
||||
|
||||
stateVector.LQRx[t] = State.LQRx;
|
||||
stateVector.LQRy[t] = State.LQRy;
|
||||
stateVector.PIDx[t] = State.PIDx;
|
||||
stateVector.PIDy[t] = State.PIDy;
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
PrevState = State;
|
||||
@@ -396,7 +336,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
|
||||
// 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, thrustFiring, LQRx, LQRy, "
|
||||
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, "
|
||||
"thrust, deriv"
|
||||
<< std::endl;
|
||||
|
||||
@@ -431,8 +371,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
|
||||
|
||||
outfile << stateVector.thrustFiring[t] << ", ";
|
||||
|
||||
outfile << stateVector.LQRx[t] << ", ";
|
||||
outfile << stateVector.LQRy[t] << std::endl;
|
||||
outfile << stateVector.PIDx[t] << ", ";
|
||||
outfile << stateVector.PIDy[t] << std::endl;
|
||||
}
|
||||
|
||||
outfile.close();
|
||||
|
||||
Reference in New Issue
Block a user