mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
Merge branch '12-pid-implementation'
This commit is contained in:
commit
15a4c83ea5
5
.vscode/settings.json
vendored
5
.vscode/settings.json
vendored
@ -39,7 +39,10 @@
|
|||||||
"xstring": "cpp",
|
"xstring": "cpp",
|
||||||
"xtr1common": "cpp",
|
"xtr1common": "cpp",
|
||||||
"xutility": "cpp",
|
"xutility": "cpp",
|
||||||
"vector": "cpp"
|
"vector": "cpp",
|
||||||
|
"cctype": "cpp",
|
||||||
|
"sstream": "cpp",
|
||||||
|
"string": "cpp"
|
||||||
},
|
},
|
||||||
"C_Cpp.clang_format_fallbackStyle": "LLVM",
|
"C_Cpp.clang_format_fallbackStyle": "LLVM",
|
||||||
"editor.formatOnSave": true,
|
"editor.formatOnSave": true,
|
||||||
|
@ -21,9 +21,8 @@ struct Vehicle {
|
|||||||
double burnVelocity;
|
double burnVelocity;
|
||||||
double thrust, burnElapsed, burnStart;
|
double thrust, burnElapsed, burnStart;
|
||||||
bool thrustFiring = false;
|
bool thrustFiring = false;
|
||||||
;
|
|
||||||
|
|
||||||
double LQRx, LQRy, Fx, Fy, Fz;
|
double PIDx, PIDy, Fx, Fy, Fz;
|
||||||
double momentX, momentY, momentZ;
|
double momentX, momentY, momentZ;
|
||||||
|
|
||||||
double I11, I22, I33;
|
double I11, I22, I33;
|
||||||
@ -32,6 +31,12 @@ struct Vehicle {
|
|||||||
int maxServo;
|
int maxServo;
|
||||||
double xServoDegs, yServoDegs;
|
double xServoDegs, yServoDegs;
|
||||||
|
|
||||||
|
double Kp, Ki, Kd;
|
||||||
|
double yError, yPrevError;
|
||||||
|
double pError, pPrevError;
|
||||||
|
double i_yError, i_pError = 0;
|
||||||
|
double d_yError, d_pError;
|
||||||
|
|
||||||
double simTime;
|
double simTime;
|
||||||
int stepSize;
|
int stepSize;
|
||||||
};
|
};
|
||||||
|
@ -4,7 +4,7 @@
|
|||||||
#define OUTVECTOR_H
|
#define OUTVECTOR_H
|
||||||
|
|
||||||
struct outVector {
|
struct outVector {
|
||||||
int length = 10000; // current sim runs ~5000 steps, x2 just in case
|
int length = 100000; // current sim runs ~5000 steps, x2 just in case
|
||||||
|
|
||||||
std::vector<double> x = std::vector<double>(length, 0.0);
|
std::vector<double> x = std::vector<double>(length, 0.0);
|
||||||
std::vector<double> y = std::vector<double>(length, 0.0);
|
std::vector<double> y = std::vector<double>(length, 0.0);
|
||||||
@ -30,6 +30,9 @@ struct outVector {
|
|||||||
std::vector<double> servo2 = std::vector<double>(length, 0.0);
|
std::vector<double> servo2 = std::vector<double>(length, 0.0);
|
||||||
|
|
||||||
std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
|
std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
|
||||||
|
|
||||||
|
std::vector<double> PIDx = std::vector<double>(length, 0.0);
|
||||||
|
std::vector<double> PIDy = std::vector<double>(length, 0.0);
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
300
include/sim.h
300
include/sim.h
@ -3,17 +3,17 @@
|
|||||||
|
|
||||||
void burnStartTimeCalc(struct Vehicle &);
|
void burnStartTimeCalc(struct Vehicle &);
|
||||||
void thrustSelection(struct Vehicle &, int t);
|
void thrustSelection(struct Vehicle &, int t);
|
||||||
void lqrCalc(struct Vehicle &);
|
void pidController(struct Vehicle &, struct Vehicle &);
|
||||||
void TVC(struct Vehicle &);
|
void TVC(struct Vehicle &);
|
||||||
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
|
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
|
||||||
void state2vec(struct Vehicle &, struct outVector &, int t);
|
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
|
||||||
void write2CSV(struct outVector &, struct Vehicle &);
|
void write2CSV(struct outVector &, struct Vehicle &);
|
||||||
double derivative(double x2, double x1, double dt);
|
double derivative(double x2, double x1, double dt);
|
||||||
double integral(double x2, double x1, double dt);
|
double integral(double x2, double x1, double dt);
|
||||||
|
|
||||||
// Any parameters that are constants should be declared here instead of buried
|
// Any parameters that are constants should be declared here instead of buried
|
||||||
// in code
|
// in code
|
||||||
double const dt = 0.001;
|
double const dt = 0.01;
|
||||||
double const g = -9.81;
|
double const g = -9.81;
|
||||||
|
|
||||||
bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
|
bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
|
||||||
@ -27,14 +27,14 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
|
|||||||
|
|
||||||
// Start Sim
|
// Start Sim
|
||||||
do {
|
do {
|
||||||
thrustSelection(State, t);
|
|
||||||
lqrCalc(State);
|
|
||||||
TVC(State);
|
|
||||||
vehicleDynamics(State, PrevState, t);
|
vehicleDynamics(State, PrevState, t);
|
||||||
state2vec(State, stateVector, t);
|
thrustSelection(State, t);
|
||||||
|
pidController(State, PrevState);
|
||||||
|
TVC(State);
|
||||||
|
state2vec(State, PrevState, stateVector, t);
|
||||||
|
|
||||||
t++;
|
t += State.stepSize;
|
||||||
} while ((State.z > 0.0) || (State.thrust > 1.0));
|
} while ((State.z > 0.0) || (State.thrust > 0.1));
|
||||||
|
|
||||||
write2CSV(stateVector, State);
|
write2CSV(stateVector, State);
|
||||||
|
|
||||||
@ -57,7 +57,7 @@ void burnStartTimeCalc(Vehicle &State) {
|
|||||||
double velocity = State.vz;
|
double velocity = State.vz;
|
||||||
double h = 0;
|
double h = 0;
|
||||||
|
|
||||||
double a, j, mass, thrust;
|
double mass, thrust;
|
||||||
|
|
||||||
// Piecewise functions for F15 thrust curve
|
// Piecewise functions for F15 thrust curve
|
||||||
for (double i = 0.148; i < 3.450; i = i + dt) {
|
for (double i = 0.148; i < 3.450; i = i + dt) {
|
||||||
@ -84,6 +84,84 @@ void burnStartTimeCalc(Vehicle &State) {
|
|||||||
State.simTime = (State.burntime + burnStartTime) * 1000;
|
State.simTime = (State.burntime + burnStartTime) * 1000;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
|
||||||
|
// Moment of Inertia
|
||||||
|
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);
|
||||||
|
|
||||||
|
// Idot
|
||||||
|
if (t < 0.1) {
|
||||||
|
State.I11dot = 0;
|
||||||
|
State.I22dot = 0;
|
||||||
|
State.I33dot = 0;
|
||||||
|
|
||||||
|
State.x = 0;
|
||||||
|
State.y = 0;
|
||||||
|
|
||||||
|
State.ax = 0;
|
||||||
|
State.ay = 0;
|
||||||
|
State.az = State.Fz / State.massInitial;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
|
||||||
|
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
|
||||||
|
State.I33dot = derivative(State.I33, PrevState.I33, State.stepSize);
|
||||||
|
|
||||||
|
// pdot, qdot, rdot
|
||||||
|
State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
|
||||||
|
State.I22 * PrevState.pitchdot * PrevState.rolldot -
|
||||||
|
State.I33 * PrevState.pitchdot * PrevState.rolldot) /
|
||||||
|
State.I11;
|
||||||
|
State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
|
||||||
|
State.I11 * PrevState.rolldot * PrevState.yawdot +
|
||||||
|
State.I33 * PrevState.rolldot * PrevState.yawdot) /
|
||||||
|
State.I22;
|
||||||
|
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
|
||||||
|
State.I11 * PrevState.pitchdot * PrevState.yawdot -
|
||||||
|
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
|
||||||
|
State.I33;
|
||||||
|
|
||||||
|
// p, q, r
|
||||||
|
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
|
||||||
|
State.pitchdot =
|
||||||
|
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
|
||||||
|
State.rolldot = integral(State.rollddot, PrevState.rolldot, State.stepSize);
|
||||||
|
|
||||||
|
// Euler Angles
|
||||||
|
State.phidot =
|
||||||
|
State.yawdot + (sin(State.pitch) * (State.rolldot * cos(State.yaw) +
|
||||||
|
State.pitchdot * sin(State.yaw))) /
|
||||||
|
cos(State.pitch);
|
||||||
|
State.thetadot =
|
||||||
|
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
|
||||||
|
State.psidot =
|
||||||
|
(State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
|
||||||
|
cos(State.pitch);
|
||||||
|
|
||||||
|
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
|
||||||
|
State.pitch = integral(State.thetadot, PrevState.pitch, State.stepSize);
|
||||||
|
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
|
||||||
|
|
||||||
|
// ax ay az
|
||||||
|
State.ax = (State.Fx / State.mass);
|
||||||
|
State.ay = (State.Fy / State.mass);
|
||||||
|
State.az = (State.Fz / State.mass);
|
||||||
|
|
||||||
|
// vx vy vz in Earth frame
|
||||||
|
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
|
||||||
|
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
|
||||||
|
State.vz = integral(State.az, PrevState.vz, State.stepSize);
|
||||||
|
|
||||||
|
// Xe
|
||||||
|
State.x = integral(State.vx, PrevState.x, State.stepSize);
|
||||||
|
State.y = integral(State.vy, PrevState.y, State.stepSize);
|
||||||
|
State.z = integral(State.vz, PrevState.z, State.stepSize);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void thrustSelection(Vehicle &State, int t) {
|
void thrustSelection(Vehicle &State, int t) {
|
||||||
|
|
||||||
if (State.burnElapsed != 2000) {
|
if (State.burnElapsed != 2000) {
|
||||||
@ -93,7 +171,7 @@ void thrustSelection(Vehicle &State, int t) {
|
|||||||
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
||||||
}
|
}
|
||||||
|
|
||||||
else if (abs(State.burnVelocity + State.vz) < .001) {
|
else if (abs(State.burnVelocity + State.vz) < 0.001) {
|
||||||
// Start burn
|
// Start burn
|
||||||
State.burnStart = t;
|
State.burnStart = t;
|
||||||
State.burnElapsed = 0;
|
State.burnElapsed = 0;
|
||||||
@ -105,6 +183,7 @@ void thrustSelection(Vehicle &State, int t) {
|
|||||||
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
|
if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
|
||||||
State.thrustFiring = true;
|
State.thrustFiring = true;
|
||||||
State.thrust = 65.165 * State.burnElapsed - 2.3921;
|
State.thrust = 65.165 * State.burnElapsed - 2.3921;
|
||||||
|
|
||||||
} else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
|
} else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
|
||||||
State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
|
State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
|
||||||
11.609 * pow(State.burnElapsed, 5) +
|
11.609 * pow(State.burnElapsed, 5) +
|
||||||
@ -114,71 +193,58 @@ void thrustSelection(Vehicle &State, int t) {
|
|||||||
174.43 * State.burnElapsed + 67.17;
|
174.43 * State.burnElapsed + 67.17;
|
||||||
|
|
||||||
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
|
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
|
||||||
State.thrust = -195.78 * State.burnElapsed + 675.11;
|
State.thrust = -195.78 * State.burnElapsed - 675.11;
|
||||||
|
|
||||||
if (State.burnElapsed > 3.45)
|
if (State.burnElapsed > 3.45) {
|
||||||
State.thrustFiring = false;
|
State.thrustFiring = false;
|
||||||
|
State.thrust = 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void lqrCalc(Vehicle &State) {
|
void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
||||||
|
// Make sure we start reacting when we start burning
|
||||||
|
if (State.thrust > 0.01) {
|
||||||
|
|
||||||
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
|
State.yError = State.yaw;
|
||||||
pow(State.vehicleRadius, 2) / 4);
|
State.pError = State.pitch;
|
||||||
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
|
// Integral of Error
|
||||||
double K11 = 39.54316;
|
State.i_yError = integral(State.yError, State.i_yError, State.stepSize);
|
||||||
double K12 = 0.00000;
|
State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
|
||||||
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
|
// Derivative of Error
|
||||||
double gain = 0.25 * pow(10, -4);
|
State.d_yError = derivative(State.yError, PrevState.yError, State.stepSize);
|
||||||
|
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
|
||||||
|
|
||||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment
|
// TVC block properly
|
||||||
// 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
|
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
||||||
if (State.LQRx > State.thrust)
|
State.Kd * State.d_yError) /
|
||||||
State.LQRx = State.thrust;
|
State.momentArm;
|
||||||
else if (State.LQRx < -1 * State.thrust)
|
State.PIDy = (State.Kp * State.pError + State.Ki * State.i_pError +
|
||||||
State.LQRx = -1 * State.thrust;
|
State.Kd * State.d_pError) /
|
||||||
|
State.momentArm;
|
||||||
|
|
||||||
// LQR Force limiter Y
|
} else {
|
||||||
if (State.LQRy > State.thrust)
|
State.PIDx = 0;
|
||||||
State.LQRy = State.thrust;
|
State.PIDy = 0;
|
||||||
else if (State.LQRy < -1 * State.thrust)
|
}
|
||||||
State.LQRy = -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;
|
||||||
|
|
||||||
|
// 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) {
|
void TVC(Vehicle &State) {
|
||||||
if (State.thrust < 1) {
|
if (State.thrust < 0.1) {
|
||||||
// Define forces and moments for t = 0
|
// Define forces and moments for t = 0
|
||||||
State.Fx = 0;
|
State.Fx = 0;
|
||||||
State.Fy = 0;
|
State.Fy = 0;
|
||||||
@ -187,9 +253,10 @@ void TVC(Vehicle &State) {
|
|||||||
State.momentX = 0;
|
State.momentX = 0;
|
||||||
State.momentY = 0;
|
State.momentY = 0;
|
||||||
State.momentZ = 0;
|
State.momentZ = 0;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// Convert servo position to degrees for comparison to max allowable
|
// 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
|
// Servo position limiter
|
||||||
if (State.xServoDegs > State.maxServo)
|
if (State.xServoDegs > State.maxServo)
|
||||||
@ -198,7 +265,7 @@ void TVC(Vehicle &State) {
|
|||||||
State.xServoDegs = -1 * State.maxServo;
|
State.xServoDegs = -1 * State.maxServo;
|
||||||
|
|
||||||
// Convert servo position to degrees for comparison to max allowable
|
// 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
|
// Servo position limiter
|
||||||
if (State.yServoDegs > State.maxServo)
|
if (State.yServoDegs > State.maxServo)
|
||||||
@ -210,7 +277,7 @@ void TVC(Vehicle &State) {
|
|||||||
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
|
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
|
||||||
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
|
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
|
||||||
State.Fz =
|
State.Fz =
|
||||||
sqrt(pow(State.thrust, 2) - (pow(State.Fx, 2) + pow(State.Fy, 2))) +
|
sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
|
||||||
(State.mass * g);
|
(State.mass * g);
|
||||||
|
|
||||||
// Calculate moment created by Fx and Fy
|
// Calculate moment created by Fx and Fy
|
||||||
@ -220,86 +287,8 @@ void TVC(Vehicle &State) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
|
void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
|
||||||
// Idot
|
int t) {
|
||||||
if (t < 1) {
|
|
||||||
State.I11dot = 0;
|
|
||||||
State.I22dot = 0;
|
|
||||||
State.I33dot = 0;
|
|
||||||
} else {
|
|
||||||
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
|
|
||||||
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
|
|
||||||
State.I33dot = derivative(State.I33, PrevState.I33, State.stepSize);
|
|
||||||
}
|
|
||||||
|
|
||||||
// pdot, qdot, rdot
|
|
||||||
State.yawddot = (State.momentX - State.I11dot * State.yawdot +
|
|
||||||
State.I22 * State.pitchdot * State.rolldot -
|
|
||||||
State.I33 * State.pitchdot * State.rolldot) /
|
|
||||||
State.I11;
|
|
||||||
State.pitchddot = (State.momentY - State.I22dot * State.pitchdot -
|
|
||||||
State.I11 * State.rolldot * State.yawdot +
|
|
||||||
State.I33 * State.rolldot * State.yawdot) /
|
|
||||||
State.I22;
|
|
||||||
State.rollddot = (State.momentZ - State.I33dot * State.rolldot +
|
|
||||||
State.I11 * State.pitchdot * State.yawdot -
|
|
||||||
State.I22 * State.pitchdot * State.yawdot) /
|
|
||||||
State.I33;
|
|
||||||
|
|
||||||
if (t < 1) {
|
|
||||||
State.x = 0;
|
|
||||||
State.y = 0;
|
|
||||||
|
|
||||||
State.ax = 0;
|
|
||||||
State.ay = 0;
|
|
||||||
State.az = State.Fz / State.massInitial;
|
|
||||||
}
|
|
||||||
|
|
||||||
else {
|
|
||||||
// p, q, r
|
|
||||||
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
|
|
||||||
State.pitchdot =
|
|
||||||
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
|
|
||||||
State.rolldot = integral(State.rollddot, PrevState.rolldot, State.stepSize);
|
|
||||||
|
|
||||||
// ax ay az
|
|
||||||
State.ax = (State.Fx / State.mass) +
|
|
||||||
(State.pitchdot * State.vz - State.rolldot * State.vy);
|
|
||||||
State.ay = (State.Fy / State.mass) +
|
|
||||||
(State.rolldot * State.vx - State.vz * State.yawdot);
|
|
||||||
State.az = (State.Fz / State.mass) +
|
|
||||||
(State.vy * State.yawdot - State.pitchdot * State.vx);
|
|
||||||
|
|
||||||
// vx vy vz in Body frame
|
|
||||||
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
|
|
||||||
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
|
|
||||||
State.vz = integral(State.az, PrevState.vz, State.stepSize);
|
|
||||||
|
|
||||||
// Xe
|
|
||||||
State.x = integral(State.vx, PrevState.x, State.stepSize);
|
|
||||||
State.y = integral(State.vy, PrevState.y, State.stepSize);
|
|
||||||
State.z = integral(State.vz, PrevState.z, State.stepSize);
|
|
||||||
|
|
||||||
// Euler Angles
|
|
||||||
State.phidot = State.yawdot + (State.pitchdot * sin(State.yaw) +
|
|
||||||
State.rolldot * cos(State.yaw)) *
|
|
||||||
(sin(State.pitch) / cos(State.pitch));
|
|
||||||
State.thetadot =
|
|
||||||
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.pitch);
|
|
||||||
State.psidot =
|
|
||||||
(State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) /
|
|
||||||
cos(State.pitch);
|
|
||||||
|
|
||||||
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
|
|
||||||
State.pitch = integral(State.thetadot, PrevState.pitch, State.stepSize);
|
|
||||||
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set "prev" values for next timestep
|
|
||||||
PrevState = State;
|
|
||||||
}
|
|
||||||
|
|
||||||
void state2vec(Vehicle &State, outVector &stateVector, int t) {
|
|
||||||
stateVector.x[t] = State.x;
|
stateVector.x[t] = State.x;
|
||||||
stateVector.y[t] = State.y;
|
stateVector.y[t] = State.y;
|
||||||
stateVector.z[t] = State.z;
|
stateVector.z[t] = State.z;
|
||||||
@ -324,6 +313,12 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
|
|||||||
stateVector.servo2[t] = State.yServoDegs;
|
stateVector.servo2[t] = State.yServoDegs;
|
||||||
|
|
||||||
stateVector.thrustFiring[t] = State.thrustFiring;
|
stateVector.thrustFiring[t] = State.thrustFiring;
|
||||||
|
|
||||||
|
stateVector.PIDx[t] = State.PIDx;
|
||||||
|
stateVector.PIDy[t] = State.PIDy;
|
||||||
|
|
||||||
|
// Set "prev" values for next timestep
|
||||||
|
PrevState = State;
|
||||||
}
|
}
|
||||||
|
|
||||||
void write2CSV(outVector &stateVector, Vehicle &State) {
|
void write2CSV(outVector &stateVector, Vehicle &State) {
|
||||||
@ -341,13 +336,14 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
|
|||||||
// Output file header. These are the variables that we output - useful for
|
// Output file header. These are the variables that we output - useful for
|
||||||
// debugging
|
// debugging
|
||||||
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
|
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
|
||||||
"pitchdot, rolldot, Servo1, Servo2, thrustFiring"
|
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, "
|
||||||
|
"thrust, deriv"
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
|
|
||||||
std::cout << "Writing to csv...\n";
|
std::cout << "Writing to csv...\n";
|
||||||
|
|
||||||
// writing to output file
|
// writing to output file
|
||||||
for (int t = 0; t < State.simTime; t++) {
|
for (int t = 0; t < State.simTime; t += State.stepSize) {
|
||||||
outfile << t << ", ";
|
outfile << t << ", ";
|
||||||
|
|
||||||
outfile << stateVector.x[t] << ", ";
|
outfile << stateVector.x[t] << ", ";
|
||||||
@ -373,14 +369,18 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
|
|||||||
outfile << stateVector.servo1[t] << ", ";
|
outfile << stateVector.servo1[t] << ", ";
|
||||||
outfile << stateVector.servo2[t] << ", ";
|
outfile << stateVector.servo2[t] << ", ";
|
||||||
|
|
||||||
outfile << stateVector.thrustFiring[t] << std::endl;
|
outfile << stateVector.thrustFiring[t] << ", ";
|
||||||
|
|
||||||
|
outfile << stateVector.PIDx[t] << ", ";
|
||||||
|
outfile << stateVector.PIDy[t] << std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
outfile.close();
|
outfile.close();
|
||||||
|
std::cout << "Output File Closed\n";
|
||||||
}
|
}
|
||||||
|
|
||||||
double derivative(double current, double previous, double step) {
|
double derivative(double current, double previous, double step) {
|
||||||
double dxdt = (previous - current) / (step / 1000);
|
double dxdt = (current - previous) / (step / 1000);
|
||||||
return dxdt;
|
return dxdt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
21
input.csv
21
input.csv
@ -1,17 +1,20 @@
|
|||||||
vx,0,m/s
|
vx,0,m/s
|
||||||
vy,0,m/s
|
vy,0,m/s
|
||||||
vz,0,m/s
|
vz,0,m/s
|
||||||
yaw,5,degs
|
yaw,75,degs
|
||||||
pitch,10,degs
|
pitch,30,degs
|
||||||
roll,0,degs
|
roll,0,degs
|
||||||
yawdot,1,degs/s
|
yawdot,0,degs/s
|
||||||
pitchdot,-1,degs/s
|
pitchdot,0,degs/s
|
||||||
rolldot,0,degs/s
|
rolldot,0,degs/s
|
||||||
Max Servo Rotation,7.5,degs
|
Max Servo Rotation,7,degs
|
||||||
Initial Mass,1.2,kg
|
Initial Mass,1.2,kg
|
||||||
Propellant Mass,0.06,kg
|
Propellant Mass,0.06,kg
|
||||||
Burn Time,3.3,s
|
Burn Time,3.302,s
|
||||||
Vehicle Height,0.53,m
|
Vehicle Height,0.5318,m
|
||||||
Vehicle Radius,0.05,m
|
Vehicle Radius,0.05105,m
|
||||||
Moment Arm,0.15,m
|
Moment Arm,0.145,m
|
||||||
Sim Step Size,1,ms
|
Sim Step Size,1,ms
|
||||||
|
Kp,-6.8699,x
|
||||||
|
Ki,0,x
|
||||||
|
Kd,-0.775,x
|
||||||
|
|
@ -36,7 +36,7 @@ S = icare(A, B, Q, R);
|
|||||||
K = Rinv * B' * S;
|
K = Rinv * B' * S;
|
||||||
|
|
||||||
%% Outputs
|
%% Outputs
|
||||||
% Copy results in command window to LQRcalc function in C++
|
% Copy results in command window to PIDcalc function in C++
|
||||||
fprintf("double K11 = %3.5f;\n", K(1, 1))
|
fprintf("double K11 = %3.5f;\n", K(1, 1))
|
||||||
fprintf("double K12 = %3.5f;\n", K(1, 2))
|
fprintf("double K12 = %3.5f;\n", K(1, 2))
|
||||||
fprintf("double K13 = %3.5f;\n", K(1, 3))
|
fprintf("double K13 = %3.5f;\n", K(1, 3))
|
||||||
|
@ -27,6 +27,9 @@ rolldot = T(:, 16);
|
|||||||
Servo1 = T(:, 17);
|
Servo1 = T(:, 17);
|
||||||
Servo2 = T(:, 18);
|
Servo2 = T(:, 18);
|
||||||
|
|
||||||
|
PIDx = T(:, 20);
|
||||||
|
PIDy = T(:, 21);
|
||||||
|
|
||||||
% Acceleration
|
% Acceleration
|
||||||
subplot(3, 1, 1)
|
subplot(3, 1, 1)
|
||||||
plot(t, az)
|
plot(t, az)
|
||||||
@ -91,3 +94,20 @@ title('Servo 2 Position vs Time')
|
|||||||
xlabel('Time (ms)')
|
xlabel('Time (ms)')
|
||||||
ylabel('Servo 2 Position (rad)')
|
ylabel('Servo 2 Position (rad)')
|
||||||
%saveas(gcf,'outputs/Servo Position vs Time.png')
|
%saveas(gcf,'outputs/Servo Position vs Time.png')
|
||||||
|
|
||||||
|
figure(4)
|
||||||
|
|
||||||
|
% Servo 1 Position
|
||||||
|
subplot(2, 1, 1)
|
||||||
|
plot(t, PIDx)
|
||||||
|
title('PIDx vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('PIDx')
|
||||||
|
|
||||||
|
% Servo 2 Position
|
||||||
|
subplot(2, 1, 2)
|
||||||
|
plot(t, PIDy)
|
||||||
|
title('PIDy vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('PIDy')
|
||||||
|
%saveas(gcf,'outputs/Servo Position vs Time.png')
|
||||||
|
18
src/main.cpp
18
src/main.cpp
@ -3,9 +3,9 @@
|
|||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <string>
|
|
||||||
#include <stdexcept> // std::runtime_error
|
#include <stdexcept> // std::runtime_error
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "Vehicle.h"
|
#include "Vehicle.h"
|
||||||
@ -24,10 +24,10 @@ int main() {
|
|||||||
if (!inFile.is_open())
|
if (!inFile.is_open())
|
||||||
throw std::runtime_error("Could not open file");
|
throw std::runtime_error("Could not open file");
|
||||||
|
|
||||||
std::vector<double> varValueVec = std::vector<double>(17, 0.0);
|
std::vector<double> varValueVec = std::vector<double>(20, 0.0);
|
||||||
std::string varName, varValue, varUnits;
|
std::string varName, varValue, varUnits;
|
||||||
|
|
||||||
for (int i; i < 17; i++) {
|
for (int i; i < 20; i++) {
|
||||||
std::getline(inFile, varName, ',');
|
std::getline(inFile, varName, ',');
|
||||||
std::getline(inFile, varValue, ',');
|
std::getline(inFile, varValue, ',');
|
||||||
varValueVec[i] = stod(varValue);
|
varValueVec[i] = stod(varValue);
|
||||||
@ -61,6 +61,11 @@ int main() {
|
|||||||
// Sim Step Size
|
// Sim Step Size
|
||||||
State.stepSize = varValueVec[16]; // [ms]
|
State.stepSize = varValueVec[16]; // [ms]
|
||||||
|
|
||||||
|
// PID Gains
|
||||||
|
State.Kp = varValueVec[17];
|
||||||
|
State.Ki = varValueVec[18];
|
||||||
|
State.Kd = varValueVec[19];
|
||||||
|
|
||||||
// Other Properties
|
// Other Properties
|
||||||
State.burntime = varValueVec[12]; // [s]
|
State.burntime = varValueVec[12]; // [s]
|
||||||
State.massPropellant = varValueVec[11]; // [kg]
|
State.massPropellant = varValueVec[11]; // [kg]
|
||||||
@ -70,16 +75,19 @@ int main() {
|
|||||||
State.burnElapsed = 2000; // [s]
|
State.burnElapsed = 2000; // [s]
|
||||||
PrevState.thrust = 0; // [N]
|
PrevState.thrust = 0; // [N]
|
||||||
|
|
||||||
|
PrevState = State;
|
||||||
|
|
||||||
|
std::cout << "START\n";
|
||||||
bool outcome = sim(State, PrevState);
|
bool outcome = sim(State, PrevState);
|
||||||
|
|
||||||
std::cout << "Finished"
|
std::cout << "Finished\n";
|
||||||
<< "\n";
|
|
||||||
|
|
||||||
if (outcome == 1) {
|
if (outcome == 1) {
|
||||||
std::cout << "Sim Result = Success!";
|
std::cout << "Sim Result = Success!";
|
||||||
return 0;
|
return 0;
|
||||||
} else if (outcome == 0) {
|
} else if (outcome == 0) {
|
||||||
std::cout << "Sim Result = Failed!";
|
std::cout << "Sim Result = Failed!";
|
||||||
|
|
||||||
// return 1; Until I figure out how to make CI/CD continue even when run
|
// return 1; Until I figure out how to make CI/CD continue even when run
|
||||||
// fails.
|
// fails.
|
||||||
return 0;
|
return 0;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user