From 9eb9bce77c37a3cce71360d0afc2ec15431fb7b2 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Tue, 21 Sep 2021 21:42:18 -0700 Subject: [PATCH] Cleaned up PID --- include/Vehicle.h | 2 +- include/sim.h | 61 ++++++++++++++++++++--------------------------- input.csv | 10 ++++---- src/main.cpp | 7 +++--- 4 files changed, 36 insertions(+), 44 deletions(-) diff --git a/include/Vehicle.h b/include/Vehicle.h index 67cbff4..0c266dc 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -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; diff --git a/include/sim.h b/include/sim.h index 93ed77b..cf611b7 100644 --- a/include/sim.h +++ b/include/sim.h @@ -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) { diff --git a/input.csv b/input.csv index 2d989fe..d4c67d6 100644 --- a/input.csv +++ b/input.csv @@ -1,8 +1,8 @@ vx,0,m/s vy,0,m/s vz,0,m/s -yaw,5,degs -pitch,10,degs +yaw,10,degs +pitch,0,degs roll,0,degs yawdot,0,degs/s pitchdot,0,degs/s @@ -15,6 +15,6 @@ Vehicle Height,0.53,m Vehicle Radius,0.05,m Moment Arm,0.15,m Sim Step Size,1,ms -Kp,-0.1,x -Ki,0, x -Kd,0, x +Kp,-0.25,x +Ki,0.4,x +Kd,1,x diff --git a/src/main.cpp b/src/main.cpp index d618714..6ad6716 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,7 +24,7 @@ int main() { if (!inFile.is_open()) throw std::runtime_error("Could not open file"); - std::vector varValueVec = std::vector(17, 0.0); + std::vector varValueVec = std::vector(20, 0.0); std::string varName, varValue, varUnits; for (int i; i < 20; i++) { @@ -66,6 +66,8 @@ int main() { State.Ki = varValueVec[18]; State.Kd = varValueVec[19]; + std::cout << State.Kp << "\n"; + std::cout << State.Ki << "\n"; std::cout << State.Kd << "\n"; // Other Properties @@ -79,8 +81,7 @@ int main() { bool outcome = sim(State, PrevState); - std::cout << "Finished" - << "\n"; + std::cout << "Finished\n"; if (outcome == 1) { std::cout << "Sim Result = Success!";