Initial Pass, still lots of bugs to fix

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;
 };

include/outVector.h

@@ -30,6 +30,9 @@ struct outVector {
   std::vector<double> servo2 = std::vector<double>(length, 0.0);
 
   std::vector<bool> thrustFiring = std::vector<bool>(length, 0.0);
+
+  std::vector<double> LQRx = std::vector<double>(length, 0.0);
+  std::vector<double> LQRy = std::vector<double>(length, 0.0);
 };
 
 #endif

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