From af4a5d16d5b91e15f9f82bdedd228f641a34f28a Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 14 Oct 2021 17:39:17 -0700 Subject: [PATCH] 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)