From 768bcb79645affc2d76b97a989ff237caae45797 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 20 Sep 2021 10:34:20 -0700 Subject: [PATCH 1/8] Initial Pass, still lots of bugs to fix --- .vscode/settings.json | 5 ++- include/Vehicle.h | 6 +++- include/outVector.h | 3 ++ include/sim.h | 62 ++++++++++++++++++++++++++++++-- input.csv | 7 ++-- src/main.cpp | 82 ++++++++++++++++++++++++------------------- 6 files changed, 121 insertions(+), 44 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index a124363..0f4f1c9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -39,7 +39,10 @@ "xstring": "cpp", "xtr1common": "cpp", "xutility": "cpp", - "vector": "cpp" + "vector": "cpp", + "cctype": "cpp", + "sstream": "cpp", + "string": "cpp" }, "C_Cpp.clang_format_fallbackStyle": "LLVM", "editor.formatOnSave": true, diff --git a/include/Vehicle.h b/include/Vehicle.h index d4e0342..67cbff4 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -21,7 +21,6 @@ struct Vehicle { double burnVelocity; double thrust, burnElapsed, burnStart; bool thrustFiring = false; - ; double LQRx, LQRy, Fx, Fy, Fz; double momentX, momentY, momentZ; @@ -32,6 +31,11 @@ struct Vehicle { int maxServo; double xServoDegs, yServoDegs; + double Kp, Ki, Kd; + double yError, yPrevError; + double pError, pPrevError; + double y_pidRefeed, p_pidRefeed, i_yError, i_pError = 0; + double simTime; int stepSize; }; diff --git a/include/outVector.h b/include/outVector.h index 0bbbf83..686f7d8 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -30,6 +30,9 @@ struct outVector { std::vector servo2 = std::vector(length, 0.0); std::vector thrustFiring = std::vector(length, 0.0); + + std::vector LQRx = std::vector(length, 0.0); + std::vector LQRy = std::vector(length, 0.0); }; #endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index 55add14..fc06bcb 100644 --- a/include/sim.h +++ b/include/sim.h @@ -4,6 +4,7 @@ void burnStartTimeCalc(struct Vehicle &); void thrustSelection(struct Vehicle &, int t); void lqrCalc(struct Vehicle &); +void pidController(struct Vehicle &); void TVC(struct Vehicle &); void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); void state2vec(struct Vehicle &, struct outVector &, int t); @@ -28,7 +29,8 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) { // Start Sim do { thrustSelection(State, t); - lqrCalc(State); + // lqrCalc(State); + pidController(State); TVC(State); vehicleDynamics(State, PrevState, t); state2vec(State, stateVector, t); @@ -177,6 +179,54 @@ void lqrCalc(Vehicle &State) { State.LQRy = -1 * State.thrust; } +void pidController(Vehicle &State) { + 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; + + State.i_yError += State.yError * State.stepSize / 1000; + State.i_pError += State.pError * State.stepSize / 1000; + + 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 + + State.Kd * d_yError); + State.LQRy = (State.Kp * State.pError + 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; + } + + // LQR Force limiter X + if (State.LQRx > State.thrust) + State.LQRx = State.thrust; + else if (State.LQRx < -1 * State.thrust) + State.LQRx = -1 * State.thrust; + + // LQR Force limiter Y + if (State.LQRy > State.thrust) + State.LQRy = State.thrust; + else if (State.LQRy < -1 * State.thrust) + State.LQRy = -1 * State.thrust; +} + void TVC(Vehicle &State) { if (State.thrust < 1) { // Define forces and moments for t = 0 @@ -324,6 +374,9 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) { stateVector.servo2[t] = State.yServoDegs; stateVector.thrustFiring[t] = State.thrustFiring; + + stateVector.LQRx[t] = State.LQRx; + stateVector.LQRy[t] = State.LQRy; } void write2CSV(outVector &stateVector, Vehicle &State) { @@ -341,7 +394,7 @@ 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" + "pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy" << std::endl; std::cout << "Writing to csv...\n"; @@ -373,7 +426,10 @@ void write2CSV(outVector &stateVector, Vehicle &State) { outfile << stateVector.servo1[t] << ", "; outfile << stateVector.servo2[t] << ", "; - outfile << stateVector.thrustFiring[t] << std::endl; + outfile << stateVector.thrustFiring[t] << ", "; + + outfile << stateVector.LQRx[t] << ", "; + outfile << stateVector.LQRy[t] << std::endl; } outfile.close(); diff --git a/input.csv b/input.csv index 85abb23..c196115 100644 --- a/input.csv +++ b/input.csv @@ -4,8 +4,8 @@ vz,0,m/s yaw,5,degs pitch,10,degs roll,0,degs -yawdot,1,degs/s -pitchdot,-1,degs/s +yawdot,0,degs/s +pitchdot,0,degs/s rolldot,0,degs/s Max Servo Rotation,7.5,degs Initial Mass,1.2,kg @@ -15,3 +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.1, x +Kd,-0.5, x diff --git a/src/main.cpp b/src/main.cpp index c1a7562..c580c7e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,9 +3,9 @@ #include #include #include -#include #include // std::runtime_error #include +#include #include #include "Vehicle.h" @@ -27,59 +27,67 @@ int main() { std::vector varValueVec = std::vector(17, 0.0); std::string varName, varValue, varUnits; - for (int i; i < 17; i++) { + for (int i; i < 20; i++) { std::getline(inFile, varName, ','); std::getline(inFile, varValue, ','); varValueVec[i] = stod(varValue); std::getline(inFile, varUnits); - } + } // Initial Velocity - State.vx = varValueVec[0]; // [m/s] - State.vy = varValueVec[1]; // [m/s] - State.vz = varValueVec[2]; // [m/s] + State.vx = varValueVec[0]; // [m/s] + State.vy = varValueVec[1]; // [m/s] + State.vz = varValueVec[2]; // [m/s] - // Initial YPR - State.yaw = varValueVec[3] * M_PI / 180; // [rad] - State.pitch = varValueVec[4] * M_PI / 180; // [rad] - State.roll = varValueVec[5] * M_PI / 180; // [rad] + // Initial YPR + State.yaw = varValueVec[3] * M_PI / 180; // [rad] + State.pitch = varValueVec[4] * M_PI / 180; // [rad] + State.roll = varValueVec[5] * M_PI / 180; // [rad] - // Initial YPRdot - State.yawdot = varValueVec[6] * M_PI / 180; // [rad/s] - State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s] - State.rolldot = varValueVec[8] * M_PI / 180; // [rad/s] + // Initial YPRdot + State.yawdot = varValueVec[6] * M_PI / 180; // [rad/s] + State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s] + State.rolldot = varValueVec[8] * M_PI / 180; // [rad/s] - // Servo Limitation - State.maxServo = varValueVec[9]; // [degs] + // Servo Limitation + State.maxServo = varValueVec[9]; // [degs] - // Vehicle Properties - State.massInitial = varValueVec[10]; // [kg] - State.vehicleHeight = varValueVec[13]; // [m] - State.vehicleRadius = varValueVec[14]; // [m] - State.momentArm = varValueVec[15]; // [m] + // Vehicle Properties + State.massInitial = varValueVec[10]; // [kg] + State.vehicleHeight = varValueVec[13]; // [m] + State.vehicleRadius = varValueVec[14]; // [m] + State.momentArm = varValueVec[15]; // [m] - // Sim Step Size - State.stepSize = varValueVec[16]; // [ms] + // Sim Step Size + State.stepSize = varValueVec[16]; // [ms] - // Other Properties - State.burntime = varValueVec[12]; // [s] - State.massPropellant = varValueVec[11]; // [kg] - State.massBurnout = State.massInitial - State.massPropellant; // [kg] - State.mdot = State.massPropellant / State.burntime; // [kg/s] - State.mass = State.massInitial; // [kg] - State.burnElapsed = 2000; // [s] - PrevState.thrust = 0; // [N] + // PID Gains + State.Kp = varValueVec[17]; + State.Ki = varValueVec[18]; + State.Kd = -1; - bool outcome = sim(State, PrevState); + std::cout << State.Kd << "\n"; - std::cout << "Finished" - << "\n"; + // Other Properties + State.burntime = varValueVec[12]; // [s] + State.massPropellant = varValueVec[11]; // [kg] + State.massBurnout = State.massInitial - State.massPropellant; // [kg] + State.mdot = State.massPropellant / State.burntime; // [kg/s] + State.mass = State.massInitial; // [kg] + State.burnElapsed = 2000; // [s] + PrevState.thrust = 0; // [N] - if (outcome == 1) { - std::cout << "Sim Result = Success!"; - return 0; + bool outcome = sim(State, PrevState); + + std::cout << "Finished" + << "\n"; + + if (outcome == 1) { + std::cout << "Sim Result = Success!"; + return 0; } else if (outcome == 0) { std::cout << "Sim Result = Failed!"; + // return 1; Until I figure out how to make CI/CD continue even when run // fails. return 0; From 382566a8da5653d84646bfc4d68894ab9f1bc37e Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 20 Sep 2021 11:35:11 -0700 Subject: [PATCH 2/8] changed up vehicle dynamics in accordance with simulink changes --- include/sim.h | 18 ++++++++++++------ input.csv | 4 ++-- src/main.cpp | 2 +- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/include/sim.h b/include/sim.h index fc06bcb..93ed77b 100644 --- a/include/sim.h +++ b/include/sim.h @@ -313,12 +313,18 @@ 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); diff --git a/input.csv b/input.csv index c196115..2d989fe 100644 --- a/input.csv +++ b/input.csv @@ -16,5 +16,5 @@ Vehicle Radius,0.05,m Moment Arm,0.15,m Sim Step Size,1,ms Kp,-0.1,x -Ki,0.1, x -Kd,-0.5, x +Ki,0, x +Kd,0, x diff --git a/src/main.cpp b/src/main.cpp index c580c7e..d618714 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -64,7 +64,7 @@ int main() { // PID Gains State.Kp = varValueVec[17]; State.Ki = varValueVec[18]; - State.Kd = -1; + State.Kd = varValueVec[19]; std::cout << State.Kd << "\n"; From 9eb9bce77c37a3cce71360d0afc2ec15431fb7b2 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Tue, 21 Sep 2021 21:42:18 -0700 Subject: [PATCH 3/8] 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!"; From 1b13a88c7d51c76366c4482eb8a36a7679c0e411 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Tue, 21 Sep 2021 22:14:10 -0700 Subject: [PATCH 4/8] more cleanup --- include/Vehicle.h | 1 + include/sim.h | 18 ++++++++++++------ input.csv | 6 +++--- src/main.cpp | 4 ---- 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/include/Vehicle.h b/include/Vehicle.h index 0c266dc..4cc5681 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -35,6 +35,7 @@ struct Vehicle { double yError, yPrevError; double pError, pPrevError; double i_yError, i_pError = 0; + int pidTest = 0; double simTime; int stepSize; diff --git a/include/sim.h b/include/sim.h index cf611b7..abf0e14 100644 --- a/include/sim.h +++ b/include/sim.h @@ -190,19 +190,25 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) { // Make sure we start reacting when we start burning if (State.thrust > 0.01) { + + State.yError = State.yaw; + State.pError = State.pitch; + // 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 = integral(State.yError, State.i_yError, State.stepSize); + State.i_pError = integral(State.pError, State.i_pError, State.stepSize); // 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 = + derivative(State.yError, PrevState.yError, State.stepSize); + double 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.yaw + State.Ki * State.i_yError + + State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError + State.Kd * d_yError); - State.LQRy = (State.Kp * State.pitch + State.Ki * State.i_pError + + State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError + State.Kd * d_pError); } else { diff --git a/input.csv b/input.csv index d4c67d6..a085a5b 100644 --- a/input.csv +++ b/input.csv @@ -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.25,x -Ki,0.4,x -Kd,1,x +Kp,-1,x +Ki,-1,x +Kd,-1,x diff --git a/src/main.cpp b/src/main.cpp index 6ad6716..83fddd3 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -66,10 +66,6 @@ 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 State.burntime = varValueVec[12]; // [s] State.massPropellant = varValueVec[11]; // [kg] From 591ddc77e450db644aa6b42cbd17d82a48ea679e Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 11 Oct 2021 11:30:51 -0700 Subject: [PATCH 5/8] started debugging --- include/Vehicle.h | 1 - include/outVector.h | 4 ++- include/sim.h | 56 +++++++++++++++++++++-------------------- input.csv | 8 +++--- matlabHelpers/simPlot.m | 20 +++++++++++++++ src/main.cpp | 3 +++ 6 files changed, 59 insertions(+), 33 deletions(-) diff --git a/include/Vehicle.h b/include/Vehicle.h index 4cc5681..0c266dc 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -35,7 +35,6 @@ struct Vehicle { double yError, yPrevError; double pError, pPrevError; double i_yError, i_pError = 0; - int pidTest = 0; double simTime; int stepSize; diff --git a/include/outVector.h b/include/outVector.h index 686f7d8..4df7275 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -4,7 +4,7 @@ #define OUTVECTOR_H 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 x = std::vector(length, 0.0); std::vector y = std::vector(length, 0.0); @@ -33,6 +33,8 @@ struct outVector { std::vector LQRx = std::vector(length, 0.0); std::vector LQRy = std::vector(length, 0.0); + + std::vector thrust = std::vector(length, 0.0); }; #endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index abf0e14..d33c686 100644 --- a/include/sim.h +++ b/include/sim.h @@ -14,7 +14,7 @@ double integral(double x2, double x1, double dt); // Any parameters that are constants should be declared here instead of buried // in code -double const dt = 0.001; +double const dt = 0.01; double const g = -9.81; bool sim(struct Vehicle &State, struct Vehicle &PrevState) { @@ -29,14 +29,13 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) { // Start Sim do { thrustSelection(State, t); - // lqrCalc(State); - pidController(State, PrevState); + pidController(State, PrevState); // lqrCalc(State); TVC(State); vehicleDynamics(State, PrevState, t); state2vec(State, stateVector, t); - t++; - } while ((State.z > 0.0) || (State.thrust > 1.0)); + t += State.stepSize; + } while ((State.z > 0.0) || (State.thrust > 0.1)); write2CSV(stateVector, State); @@ -59,7 +58,7 @@ void burnStartTimeCalc(Vehicle &State) { double velocity = State.vz; double h = 0; - double a, j, mass, thrust; + double mass, thrust; // Piecewise functions for F15 thrust curve for (double i = 0.148; i < 3.450; i = i + dt) { @@ -107,6 +106,7 @@ void thrustSelection(Vehicle &State, int t) { if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) { State.thrustFiring = true; State.thrust = 65.165 * State.burnElapsed - 2.3921; + } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383)) State.thrust = 0.8932 * pow(State.burnElapsed, 6) - 11.609 * pow(State.burnElapsed, 5) + @@ -210,7 +210,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) { State.Kd * d_yError); State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError + State.Kd * d_pError); - } else { State.LQRx = 0; State.LQRy = 0; @@ -274,7 +273,7 @@ void TVC(Vehicle &State) { void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { // Idot - if (t < 1) { + if (t < 0.1) { State.I11dot = 0; State.I22dot = 0; State.I33dot = 0; @@ -285,20 +284,20 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { } // pdot, qdot, rdot - State.yawddot = (State.momentX - State.I11dot * State.yawdot + - State.I22 * State.pitchdot * State.rolldot - - State.I33 * State.pitchdot * State.rolldot) / + 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 * State.pitchdot - - State.I11 * State.rolldot * State.yawdot + - State.I33 * State.rolldot * State.yawdot) / + 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 * State.rolldot + - State.I11 * State.pitchdot * State.yawdot - - State.I22 * State.pitchdot * State.yawdot) / + State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot + + State.I11 * PrevState.pitchdot * PrevState.yawdot - + State.I22 * PrevState.pitchdot * PrevState.yawdot) / State.I33; - if (t < 1) { + if (t < 0.1) { State.x = 0; State.y = 0; @@ -322,7 +321,7 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { State.az = (State.Fz / State.mass) + (State.vy * State.yawdot - State.pitchdot * State.vx); - // vx vy vz in Body frame + // 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); @@ -333,13 +332,14 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { 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.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.pitch); + State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw); State.psidot = - (State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) / + (State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) / cos(State.pitch); State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize); @@ -379,6 +379,7 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) { stateVector.LQRx[t] = State.LQRx; stateVector.LQRy[t] = State.LQRy; + stateVector.thrust[t] = State.thrust; } void write2CSV(outVector &stateVector, Vehicle &State) { @@ -396,13 +397,13 @@ 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" + "pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy, thrust" << std::endl; std::cout << "Writing to csv...\n"; // 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 << stateVector.x[t] << ", "; @@ -431,7 +432,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) { outfile << stateVector.thrustFiring[t] << ", "; outfile << stateVector.LQRx[t] << ", "; - outfile << stateVector.LQRy[t] << std::endl; + outfile << stateVector.LQRy[t] << ", "; + outfile << stateVector.thrust[t] << std::endl; } outfile.close(); diff --git a/input.csv b/input.csv index a085a5b..8c3916e 100644 --- a/input.csv +++ b/input.csv @@ -1,7 +1,7 @@ vx,0,m/s vy,0,m/s vz,0,m/s -yaw,10,degs +yaw,45,degs pitch,0,degs roll,0,degs yawdot,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,-1,x -Ki,-1,x -Kd,-1,x +Kp,-6.8699,x +Ki,0,x +Kd,-0.775,x diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m index f15e1c6..e180aad 100644 --- a/matlabHelpers/simPlot.m +++ b/matlabHelpers/simPlot.m @@ -27,6 +27,9 @@ rolldot = T(:, 16); Servo1 = T(:, 17); Servo2 = T(:, 18); +LQRx = T(:, 19); +LQRy = T(:, 20); + % Acceleration subplot(3, 1, 1) plot(t, az) @@ -91,3 +94,20 @@ title('Servo 2 Position vs Time') xlabel('Time (ms)') ylabel('Servo 2 Position (rad)') %saveas(gcf,'outputs/Servo Position vs Time.png') + +figure(4) + +% Servo 1 Position +subplot(2, 1, 1) +plot(t, LQRx) +title('LQRx vs Time') +xlabel('Time (ms)') +ylabel('LQRx') + +% Servo 2 Position +subplot(2, 1, 2) +plot(t, LQRy) +title('LQRy vs Time') +xlabel('Time (ms)') +ylabel('LQRy') +%saveas(gcf,'outputs/Servo Position vs Time.png') diff --git a/src/main.cpp b/src/main.cpp index 83fddd3..304d9a2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -75,6 +75,9 @@ int main() { State.burnElapsed = 2000; // [s] PrevState.thrust = 0; // [N] + PrevState = State; + + std::cout << "START\n"; bool outcome = sim(State, PrevState); std::cout << "Finished\n"; From af4a5d16d5b91e15f9f82bdedd228f641a34f28a Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 14 Oct 2021 17:39:17 -0700 Subject: [PATCH 6/8] PID fully debugged --- include/Vehicle.h | 1 + include/outVector.h | 1 + include/sim.h | 219 +++++++++++++++++++------------------ input.csv | 14 +-- matlabHelpers/Untitled.asv | 13 +++ matlabHelpers/Untitled.m | 24 ++++ matlabHelpers/simPlot.m | 4 +- 7 files changed, 159 insertions(+), 117 deletions(-) create mode 100644 matlabHelpers/Untitled.asv create mode 100644 matlabHelpers/Untitled.m diff --git a/include/Vehicle.h b/include/Vehicle.h index 0c266dc..f19f440 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -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; diff --git a/include/outVector.h b/include/outVector.h index 4df7275..a0d6247 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -35,6 +35,7 @@ struct outVector { std::vector LQRy = std::vector(length, 0.0); std::vector thrust = std::vector(length, 0.0); + std::vector d_yError = std::vector(length, 0.0); }; #endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index d33c686..ab883df 100644 --- a/include/sim.h +++ b/include/sim.h @@ -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; } diff --git a/input.csv b/input.csv index 8c3916e..4585dcf 100644 --- a/input.csv +++ b/input.csv @@ -1,19 +1,19 @@ vx,0,m/s vy,0,m/s vz,0,m/s -yaw,45,degs -pitch,0,degs +yaw,75,degs +pitch,30,degs roll,0,degs yawdot,0,degs/s pitchdot,0,degs/s rolldot,0,degs/s -Max Servo Rotation,7.5,degs +Max Servo Rotation,7,degs Initial Mass,1.2,kg Propellant Mass,0.06,kg -Burn Time,3.3,s -Vehicle Height,0.53,m -Vehicle Radius,0.05,m -Moment Arm,0.15,m +Burn Time,3.302,s +Vehicle Height,0.5318,m +Vehicle Radius,0.05105,m +Moment Arm,0.145,m Sim Step Size,1,ms Kp,-6.8699,x Ki,0,x diff --git a/matlabHelpers/Untitled.asv b/matlabHelpers/Untitled.asv new file mode 100644 index 0000000..268daf9 --- /dev/null +++ b/matlabHelpers/Untitled.asv @@ -0,0 +1,13 @@ +clear all; close all; clc; + +syms I11 I22 I33 I11dot I22dot I33dot + +I = [I11 0 0; 0 I22 0; 0 0 I33]; +Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot]; + +syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot + +w = + +I * + diff --git a/matlabHelpers/Untitled.m b/matlabHelpers/Untitled.m new file mode 100644 index 0000000..8842322 --- /dev/null +++ b/matlabHelpers/Untitled.m @@ -0,0 +1,24 @@ +clear all; close all; clc; + +syms I11 I22 I33 I11dot I22dot I33dot + +I = [I11 0 0; 0 I22 0; 0 0 I33]; +Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot]; + +syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot + +w = [yawdot; pitchdot; rolldot]; + +Iw = I * w; +Idw = Idot * w; + +C = cross(w, Iw); + +syms momentX momentY momentZ + +M = [momentX; momentY; momentZ]; +A = M - Idw - C; +B = [A(1) A(2) A(3)]; + +X = B*inv(I); +E = [X(1); X(2); X(3)] \ No newline at end of file diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m index e180aad..406a602 100644 --- a/matlabHelpers/simPlot.m +++ b/matlabHelpers/simPlot.m @@ -27,8 +27,8 @@ rolldot = T(:, 16); Servo1 = T(:, 17); Servo2 = T(:, 18); -LQRx = T(:, 19); -LQRy = T(:, 20); +LQRx = T(:, 20); +LQRy = T(:, 21); % Acceleration subplot(3, 1, 1) From ee9fa3a3d161ceec60cf8fd3ad135c2afd0da50d Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 14 Oct 2021 17:45:23 -0700 Subject: [PATCH 7/8] cleanup from debugging --- include/outVector.h | 3 --- include/sim.h | 6 +----- matlabHelpers/Untitled.asv | 13 ------------- matlabHelpers/Untitled.m | 24 ------------------------ 4 files changed, 1 insertion(+), 45 deletions(-) delete mode 100644 matlabHelpers/Untitled.asv delete mode 100644 matlabHelpers/Untitled.m diff --git a/include/outVector.h b/include/outVector.h index a0d6247..1e08f4b 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -33,9 +33,6 @@ struct outVector { std::vector LQRx = std::vector(length, 0.0); std::vector LQRy = std::vector(length, 0.0); - - std::vector thrust = std::vector(length, 0.0); - std::vector d_yError = std::vector(length, 0.0); }; #endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index ab883df..0f10021 100644 --- a/include/sim.h +++ b/include/sim.h @@ -376,8 +376,6 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector, 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; @@ -434,9 +432,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) { outfile << stateVector.thrustFiring[t] << ", "; outfile << stateVector.LQRx[t] << ", "; - outfile << stateVector.LQRy[t] << ", "; - outfile << stateVector.thrust[t] << ", "; - outfile << stateVector.d_yError[t] << std::endl; + outfile << stateVector.LQRy[t] << std::endl; } outfile.close(); diff --git a/matlabHelpers/Untitled.asv b/matlabHelpers/Untitled.asv deleted file mode 100644 index 268daf9..0000000 --- a/matlabHelpers/Untitled.asv +++ /dev/null @@ -1,13 +0,0 @@ -clear all; close all; clc; - -syms I11 I22 I33 I11dot I22dot I33dot - -I = [I11 0 0; 0 I22 0; 0 0 I33]; -Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot]; - -syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot - -w = - -I * - diff --git a/matlabHelpers/Untitled.m b/matlabHelpers/Untitled.m deleted file mode 100644 index 8842322..0000000 --- a/matlabHelpers/Untitled.m +++ /dev/null @@ -1,24 +0,0 @@ -clear all; close all; clc; - -syms I11 I22 I33 I11dot I22dot I33dot - -I = [I11 0 0; 0 I22 0; 0 0 I33]; -Idot = [I11dot 0 0; 0 I22dot 0; 0 0 I33dot]; - -syms yaw pitch roll yawdot pitchdot rolldot yawddot pitchddot rollddot - -w = [yawdot; pitchdot; rolldot]; - -Iw = I * w; -Idw = Idot * w; - -C = cross(w, Iw); - -syms momentX momentY momentZ - -M = [momentX; momentY; momentZ]; -A = M - Idw - C; -B = [A(1) A(2) A(3)]; - -X = B*inv(I); -E = [X(1); X(2); X(3)] \ No newline at end of file From a452b30fae22b7174457bb91cf18326a84df5bb4 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Thu, 14 Oct 2021 18:02:01 -0700 Subject: [PATCH 8/8] changed all LQR references to PID --- include/Vehicle.h | 2 +- include/outVector.h | 4 +- include/sim.h | 102 ++++++++------------------------------- matlabHelpers/gainCalc.m | 2 +- matlabHelpers/simPlot.m | 16 +++--- 5 files changed, 33 insertions(+), 93 deletions(-) diff --git a/include/Vehicle.h b/include/Vehicle.h index f19f440..e570cc9 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -22,7 +22,7 @@ struct Vehicle { double thrust, burnElapsed, burnStart; bool thrustFiring = false; - double LQRx, LQRy, Fx, Fy, Fz; + double PIDx, PIDy, Fx, Fy, Fz; double momentX, momentY, momentZ; double I11, I22, I33; diff --git a/include/outVector.h b/include/outVector.h index 1e08f4b..9b21427 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -31,8 +31,8 @@ struct outVector { std::vector thrustFiring = std::vector(length, 0.0); - std::vector LQRx = std::vector(length, 0.0); - std::vector LQRy = std::vector(length, 0.0); + std::vector PIDx = std::vector(length, 0.0); + std::vector PIDy = std::vector(length, 0.0); }; #endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index 0f10021..9d287a6 100644 --- a/include/sim.h +++ b/include/sim.h @@ -3,7 +3,6 @@ void burnStartTimeCalc(struct Vehicle &); void thrustSelection(struct Vehicle &, int t); -void lqrCalc(struct Vehicle &); void pidController(struct Vehicle &, struct Vehicle &); void TVC(struct Vehicle &); void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); @@ -31,7 +30,6 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) { vehicleDynamics(State, PrevState, t); thrustSelection(State, t); pidController(State, PrevState); - // lqrCalc(State); TVC(State); state2vec(State, PrevState, stateVector, t); @@ -203,63 +201,6 @@ void thrustSelection(Vehicle &State, int t) { } } -void lqrCalc(Vehicle &State) { - - 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); - - // Paste in Values from gainCalc.m - double K11 = 39.54316; - double K12 = 0.00000; - 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 - double gain = 0.25 * pow(10, -4); - - // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment - // 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 - if (State.LQRx > State.thrust) - State.LQRx = State.thrust; - else if (State.LQRx < -1 * State.thrust) - State.LQRx = -1 * State.thrust; - - // LQR Force limiter Y - if (State.LQRy > State.thrust) - State.LQRy = State.thrust; - else if (State.LQRy < -1 * State.thrust) - State.LQRy = -1 * State.thrust; -} - void pidController(Vehicle &State, struct Vehicle &PrevState) { // Make sure we start reacting when we start burning if (State.thrust > 0.01) { @@ -275,32 +216,31 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) { 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.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError + State.Kd * State.d_yError) / State.momentArm; - State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError + + State.PIDy = (State.Kp * State.pError + State.Ki * State.i_pError + State.Kd * State.d_pError) / State.momentArm; } else { - State.LQRx = 0; - State.LQRy = 0; + State.PIDx = 0; + State.PIDy = 0; } - // LQR Force limiter X - if (State.LQRx > State.thrust) - State.LQRx = State.thrust; - else if (State.LQRx < -1 * State.thrust) - State.LQRx = -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; - // LQR Force limiter Y - if (State.LQRy > State.thrust) - State.LQRy = State.thrust; - else if (State.LQRy < -1 * State.thrust) - State.LQRy = -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) { @@ -316,7 +256,7 @@ void TVC(Vehicle &State) { } else { // 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 if (State.xServoDegs > State.maxServo) @@ -325,7 +265,7 @@ void TVC(Vehicle &State) { State.xServoDegs = -1 * State.maxServo; // 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 if (State.yServoDegs > State.maxServo) @@ -374,8 +314,8 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector, stateVector.thrustFiring[t] = State.thrustFiring; - stateVector.LQRx[t] = State.LQRx; - stateVector.LQRy[t] = State.LQRy; + stateVector.PIDx[t] = State.PIDx; + stateVector.PIDy[t] = State.PIDy; // Set "prev" values for next timestep PrevState = State; @@ -396,7 +336,7 @@ 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, " + "pitchdot, rolldot, Servo1, Servo2, thrustFiring, PIDx, PIDy, " "thrust, deriv" << std::endl; @@ -431,8 +371,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) { outfile << stateVector.thrustFiring[t] << ", "; - outfile << stateVector.LQRx[t] << ", "; - outfile << stateVector.LQRy[t] << std::endl; + outfile << stateVector.PIDx[t] << ", "; + outfile << stateVector.PIDy[t] << std::endl; } outfile.close(); diff --git a/matlabHelpers/gainCalc.m b/matlabHelpers/gainCalc.m index 4301efe..387fb14 100644 --- a/matlabHelpers/gainCalc.m +++ b/matlabHelpers/gainCalc.m @@ -36,7 +36,7 @@ S = icare(A, B, Q, R); K = Rinv * B' * S; %% 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 K12 = %3.5f;\n", K(1, 2)) fprintf("double K13 = %3.5f;\n", K(1, 3)) diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m index 406a602..3f3a4cf 100644 --- a/matlabHelpers/simPlot.m +++ b/matlabHelpers/simPlot.m @@ -27,8 +27,8 @@ rolldot = T(:, 16); Servo1 = T(:, 17); Servo2 = T(:, 18); -LQRx = T(:, 20); -LQRy = T(:, 21); +PIDx = T(:, 20); +PIDy = T(:, 21); % Acceleration subplot(3, 1, 1) @@ -99,15 +99,15 @@ figure(4) % Servo 1 Position subplot(2, 1, 1) -plot(t, LQRx) -title('LQRx vs Time') +plot(t, PIDx) +title('PIDx vs Time') xlabel('Time (ms)') -ylabel('LQRx') +ylabel('PIDx') % Servo 2 Position subplot(2, 1, 2) -plot(t, LQRy) -title('LQRy vs Time') +plot(t, PIDy) +title('PIDy vs Time') xlabel('Time (ms)') -ylabel('LQRy') +ylabel('PIDy') %saveas(gcf,'outputs/Servo Position vs Time.png')