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

PID fully debugged

Browse Source
This commit is contained in:
bpmcgeeney
2021-10-14 17:39:17 -07:00
parent 591ddc77e4
commit af4a5d16d5
7 changed files with 159 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

@@ -35,6 +35,7 @@ struct Vehicle {
double yError, yPrevError;
double pError, pPrevError;
double i_yError, i_pError = 0;
double d_yError, d_pError;
double simTime;
int stepSize;

1
include/outVector.h
View File

@@ -35,6 +35,7 @@ struct outVector {
std::vector<double> LQRy = std::vector<double>(length, 0.0);
std::vector<double> thrust = std::vector<double>(length, 0.0);
std::vector<double> d_yError = std::vector<double>(length, 0.0);
};
#endif

219
include/sim.h
View File

@@ -3,11 +3,11 @@
void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
// void lqrCalc(struct Vehicle &);
void lqrCalc(struct Vehicle &);
void pidController(struct Vehicle &, struct Vehicle &);
void TVC(struct Vehicle &);
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 &);
double derivative(double x2, double x1, double dt);
double integral(double x2, double x1, double dt);
@@ -28,11 +28,12 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
// Start Sim
do {
thrustSelection(State, t);
pidController(State, PrevState); // lqrCalc(State);
TVC(State);
vehicleDynamics(State, PrevState, t);
state2vec(State, stateVector, t);
thrustSelection(State, t);
pidController(State, PrevState);
// lqrCalc(State);
TVC(State);
state2vec(State, PrevState, stateVector, t);
t += State.stepSize;
} while ((State.z > 0.0) || (State.thrust > 0.1));
@@ -85,6 +86,84 @@ void burnStartTimeCalc(Vehicle &State) {
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) {
if (State.burnElapsed != 2000) {
@@ -94,7 +173,7 @@ void thrustSelection(Vehicle &State, int t) {
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
State.burnStart = t;
State.burnElapsed = 0;
@@ -116,10 +195,12 @@ void thrustSelection(Vehicle &State, int t) {
174.43 * State.burnElapsed + 67.17;
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.thrust = 0;
}
}
void lqrCalc(Vehicle &State) {
@@ -180,14 +261,6 @@ void lqrCalc(Vehicle &State) {
}
void pidController(Vehicle &State, struct Vehicle &PrevState) {
// Not used in this function, but it was in the LQR function so had to copy it
// over since we don't run LQR when running PID
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);
// Make sure we start reacting when we start burning
if (State.thrust > 0.01) {
@@ -199,17 +272,19 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
// Derivative of Error
double d_yError =
derivative(State.yError, PrevState.yError, State.stepSize);
double d_pError =
derivative(State.pError, PrevState.pError, State.stepSize);
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.Kd * d_yError);
State.Kd * State.d_yError) /
State.momentArm;
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError);
State.Kd * State.d_pError) /
State.momentArm;
} else {
State.LQRx = 0;
State.LQRy = 0;
@@ -238,6 +313,7 @@ void TVC(Vehicle &State) {
State.momentX = 0;
State.momentY = 0;
State.momentZ = 0;
} else {
// Convert servo position to degrees for comparison to max allowable
State.xServoDegs = (180 / M_PI) * asin(State.LQRx / State.thrust);
@@ -261,7 +337,7 @@ void TVC(Vehicle &State) {
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
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);
// Calculate moment created by Fx and Fy
@@ -271,87 +347,8 @@ void TVC(Vehicle &State) {
}
}
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
// Idot
if (t < 0.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 * 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;
if (t < 0.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 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);
// 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);
}
// Set "prev" values for next timestep
PrevState = State;
}
void state2vec(Vehicle &State, outVector &stateVector, int t) {
void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
int t) {
stateVector.x[t] = State.x;
stateVector.y[t] = State.y;
stateVector.z[t] = State.z;
@@ -380,6 +377,10 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.LQRx[t] = State.LQRx;
stateVector.LQRy[t] = State.LQRy;
stateVector.thrust[t] = State.thrust;
stateVector.d_yError[t] = State.d_yError;
// Set "prev" values for next timestep
PrevState = State;
}
void write2CSV(outVector &stateVector, Vehicle &State) {
@@ -397,7 +398,8 @@ 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, thrust"
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy, "
"thrust, deriv"
<< std::endl;
std::cout << "Writing to csv...\n";
@@ -433,7 +435,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
outfile << stateVector.LQRx[t] << ", ";
outfile << stateVector.LQRy[t] << ", ";
outfile << stateVector.thrust[t] << std::endl;
outfile << stateVector.thrust[t] << ", ";
outfile << stateVector.d_yError[t] << std::endl;
}
outfile.close();
@@ -441,7 +444,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
}
double derivative(double current, double previous, double step) {
double dxdt = (previous - current) / (step / 1000);
double dxdt = (current - previous) / (step / 1000);
return dxdt;
}