Initial Pass, still lots of bugs to fix

.vscode/settings.json

@@ -39,7 +39,10 @@
         "xstring": "cpp",
         "xtr1common": "cpp",
         "xutility": "cpp",
-        "vector": "cpp"
+        "vector": "cpp",
+        "cctype": "cpp",
+        "sstream": "cpp",
+        "string": "cpp"
     },
     "C_Cpp.clang_format_fallbackStyle": "LLVM",
     "editor.formatOnSave": true,

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

input.csv

@@ -4,8 +4,8 @@ vz,0,m/s
 yaw,5,degs
 pitch,10,degs
 roll,0,degs
-yawdot,1,degs/s
-pitchdot,-1,degs/s
+yawdot,0,degs/s
+pitchdot,0,degs/s
 rolldot,0,degs/s
 Max Servo Rotation,7.5,degs
 Initial Mass,1.2,kg
@@ -15,3 +15,6 @@ Vehicle Height,0.53,m
 Vehicle Radius,0.05,m
 Moment Arm,0.15,m
 Sim Step Size,1,ms
+Kp,-0.1,x
+Ki,0.1, x
+Kd,-0.5, x

src/main.cpp

@@ -3,9 +3,9 @@
 #include <cmath>
 #include <fstream>
 #include <iostream>
-#include <string>
 #include <stdexcept> // std::runtime_error
 #include <stdio.h>
+#include <string>
 #include <vector>
 
 #include "Vehicle.h"
@@ -27,7 +27,7 @@ int main() {
   std::vector<double> varValueVec = std::vector<double>(17, 0.0);
   std::string varName, varValue, varUnits;
 
-  for (int i; i < 17; i++) {
+  for (int i; i < 20; i++) {
     std::getline(inFile, varName, ',');
     std::getline(inFile, varValue, ',');
     varValueVec[i] = stod(varValue);
@@ -61,6 +61,13 @@ int main() {
   // Sim Step Size
   State.stepSize = varValueVec[16]; // [ms]
 
+  // PID Gains
+  State.Kp = varValueVec[17];
+  State.Ki = varValueVec[18];
+  State.Kd = -1;
+
+  std::cout << State.Kd << "\n";
+
   // Other Properties
   State.burntime = varValueVec[12];                             // [s]
   State.massPropellant = varValueVec[11];                       // [kg]
@@ -80,6 +87,7 @@ int main() {
     return 0;
   } else if (outcome == 0) {
     std::cout << "Sim Result = Failed!";
+
     // return 1; Until I figure out how to make CI/CD continue even when run
     // fails.
     return 0;