From 591ddc77e450db644aa6b42cbd17d82a48ea679e Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 11 Oct 2021 11:30:51 -0700 Subject: [PATCH] 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";