1
0
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:
Anson Biggs 2021-10-14 18:08:17 -07:00
commit 15a4c83ea5
8 changed files with 244 additions and 202 deletions

View File

@ -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,

View File

@ -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;
}; };

View File

@ -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

View File

@ -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;
} }

View File

@ -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

1 vx 0 m/s
2 vy 0 m/s
3 vz 0 m/s
4 yaw 5 75 degs
5 pitch 10 30 degs
6 roll 0 degs
7 yawdot 1 0 degs/s
8 pitchdot -1 0 degs/s
9 rolldot 0 degs/s
10 Max Servo Rotation 7.5 7 degs
11 Initial Mass 1.2 kg
12 Propellant Mass 0.06 kg
13 Burn Time 3.3 3.302 s
14 Vehicle Height 0.53 0.5318 m
15 Vehicle Radius 0.05 0.05105 m
16 Moment Arm 0.15 0.145 m
17 Sim Step Size 1 ms
18 Kp -6.8699 x
19 Ki 0 x
20 Kd -0.775 x

View File

@ -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))

View File

@ -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')

View File

@ -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;