From 768bcb79645affc2d76b97a989ff237caae45797 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 20 Sep 2021 10:34:20 -0700 Subject: [PATCH] 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;