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')