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

Cleaned up PID

Browse Source
This commit is contained in:
bpmcgeeney
2021-09-21 21:42:18 -07:00
parent 382566a8da
commit 9eb9bce77c
4 changed files with 36 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/Vehicle.h
View File

@@ -34,7 +34,7 @@ struct Vehicle {
double Kp, Ki, Kd;
double yError, yPrevError;
double pError, pPrevError;
double y_pidRefeed, p_pidRefeed, i_yError, i_pError = 0;
double i_yError, i_pError = 0;
double simTime;
int stepSize;

61
include/sim.h
View File

@@ -3,8 +3,8 @@
void burnStartTimeCalc(struct Vehicle &);
void thrustSelection(struct Vehicle &, int t);
void lqrCalc(struct Vehicle &);
void pidController(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);
@@ -30,7 +30,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
do {
thrustSelection(State, t);
// lqrCalc(State);
pidController(State);
pidController(State, PrevState);
TVC(State);
vehicleDynamics(State, PrevState, t);
state2vec(State, stateVector, t);
@@ -179,36 +179,32 @@ void lqrCalc(Vehicle &State) {
State.LQRy = -1 * State.thrust;
}
void pidController(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);
if (State.thrust > 1) {
State.yError = State.yaw; // - State.y_pidRefeed;
State.pError = State.pitch; // - State.p_pidRefeed;
// Make sure we start reacting when we start burning
if (State.thrust > 0.01) {
// Integral of Error
State.i_yError = integral(State.yaw, State.i_yError, State.stepSize);
State.i_pError = integral(State.pitch, State.i_pError, State.stepSize);
State.i_yError += State.yError * State.stepSize / 1000;
State.i_pError += State.pError * State.stepSize / 1000;
// Derivative of Error
double d_yError = derivative(State.yaw, PrevState.yaw, State.stepSize);
double d_pError = derivative(State.pitch, PrevState.pitch, State.stepSize);
double d_yError =
(State.yError - State.yPrevError) / (State.stepSize / 1000);
double d_pError =
(State.pError - State.pPrevError) / (State.stepSize / 1000);
// PID Function
State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
// PID Function - it says LQR but this is just so that it gets passed to the
// TVC block properly
State.LQRx = (State.Kp * State.yaw + State.Ki * State.i_yError +
State.Kd * d_yError);
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.LQRy = (State.Kp * State.pitch + State.Ki * State.i_pError +
State.Kd * d_pError);
// std::cout << State.LQRx << ", ";
State.yPrevError = State.yError;
State.pPrevError = State.pError;
} else {
State.LQRx = 0;
State.LQRy = 0;
@@ -228,7 +224,7 @@ void pidController(Vehicle &State) {
}
void TVC(Vehicle &State) {
if (State.thrust < 1) {
if (State.thrust < 0.1) {
// Define forces and moments for t = 0
State.Fx = 0;
State.Fy = 0;
@@ -313,18 +309,12 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
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);
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);
@@ -439,6 +429,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
}
outfile.close();
std::cout << "Output File Closed\n";
}
double derivative(double current, double previous, double step) {