changed all LQR references to PID

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;

include/outVector.h

@@ -31,8 +31,8 @@ struct outVector {
 
   std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
 
-  std::vector<double> LQRx = std::vector<double>(length, 0.0);
+  std::vector<double> PIDx = std::vector<double>(length, 0.0);
-  std::vector<double> LQRy = std::vector<double>(length, 0.0);
+  std::vector<double> PIDy = std::vector<double>(length, 0.0);
 };
 
 #endif

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.PIDx = 0;
-    State.LQRy = 0;
+    State.PIDy = 0;
   }
 
-  // LQR Force limiter X
+  // PID Force limiter X
-  if (State.LQRx > State.thrust)
+  if (State.PIDx > State.thrust)
-    State.LQRx = State.thrust;
+    State.PIDx = State.thrust;
-  else if (State.LQRx < -1 * State.thrust)
+  else if (State.PIDx < -1 * State.thrust)
-    State.LQRx = -1 * State.thrust;
+    State.PIDx = -1 * State.thrust;
 
-  // LQR Force limiter Y
+  // PID Force limiter Y
-  if (State.LQRy > State.thrust)
+  if (State.PIDy > State.thrust)
-    State.LQRy = State.thrust;
+    State.PIDy = State.thrust;
-  else if (State.LQRy < -1 * State.thrust)
+  else if (State.PIDy < -1 * State.thrust)
-    State.LQRy = -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.PIDx[t] = State.PIDx;
-  stateVector.LQRy[t] = State.LQRy;
+  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.PIDx[t] << ", ";
-    outfile << stateVector.LQRy[t] << std::endl;
+    outfile << stateVector.PIDy[t] << std::endl;
   }
 
   outfile.close();

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

matlabHelpers/simPlot.m

@@ -27,8 +27,8 @@ rolldot     = T(:, 16);
 Servo1      = T(:, 17);
 Servo2      = T(:, 18);
 
-LQRx      = T(:, 20);
+PIDx      = T(:, 20);
-LQRy      = T(:, 21);
+PIDy      = T(:, 21);
 
 % Acceleration
 subplot(3, 1, 1)
@@ -99,15 +99,15 @@ figure(4)
 
 % Servo 1 Position
 subplot(2, 1, 1)
-plot(t, LQRx)
+plot(t, PIDx)
-title('LQRx vs Time')
+title('PIDx vs Time')
 xlabel('Time (ms)')
-ylabel('LQRx')
+ylabel('PIDx')
 
 % Servo 2 Position
 subplot(2, 1, 2)
-plot(t, LQRy)
+plot(t, PIDy)
-title('LQRy vs Time')
+title('PIDy vs Time')
 xlabel('Time (ms)')
-ylabel('LQRy')
+ylabel('PIDy')
 %saveas(gcf,'outputs/Servo Position vs Time.png')