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
+yawdot,0,degs/s
-pitchdot,-1,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,59 +27,67 @@ 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);
     std::getline(inFile, varUnits);
-    }
+  }
 
   // Initial Velocity
-    State.vx = varValueVec[0]; // [m/s]
+  State.vx = varValueVec[0]; // [m/s]
-    State.vy = varValueVec[1]; // [m/s]
+  State.vy = varValueVec[1]; // [m/s]
-    State.vz = varValueVec[2]; // [m/s]
+  State.vz = varValueVec[2]; // [m/s]
 
-    // Initial YPR
+  // Initial YPR
-    State.yaw = varValueVec[3] * M_PI / 180; // [rad]
+  State.yaw = varValueVec[3] * M_PI / 180;   // [rad]
-    State.pitch = varValueVec[4] * M_PI / 180; // [rad]
+  State.pitch = varValueVec[4] * M_PI / 180; // [rad]
-    State.roll = varValueVec[5] * M_PI / 180;  // [rad]
+  State.roll = varValueVec[5] * M_PI / 180;  // [rad]
 
-    // Initial YPRdot
+  // Initial YPRdot
-    State.yawdot = varValueVec[6] * M_PI / 180; // [rad/s]
+  State.yawdot = varValueVec[6] * M_PI / 180;   // [rad/s]
-    State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s]
+  State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s]
-    State.rolldot = varValueVec[8] * M_PI / 180;   // [rad/s]
+  State.rolldot = varValueVec[8] * M_PI / 180;  // [rad/s]
 
-    // Servo Limitation
+  // Servo Limitation
-    State.maxServo = varValueVec[9]; // [degs]
+  State.maxServo = varValueVec[9]; // [degs]
 
-    // Vehicle Properties
+  // Vehicle Properties
-    State.massInitial = varValueVec[10]; // [kg]
+  State.massInitial = varValueVec[10];   // [kg]
-    State.vehicleHeight = varValueVec[13]; // [m]
+  State.vehicleHeight = varValueVec[13]; // [m]
-    State.vehicleRadius = varValueVec[14]; // [m]
+  State.vehicleRadius = varValueVec[14]; // [m]
-    State.momentArm = varValueVec[15];     // [m]
+  State.momentArm = varValueVec[15];     // [m]
 
-    // Sim Step Size
+  // Sim Step Size
-    State.stepSize = varValueVec[16]; // [ms]
+  State.stepSize = varValueVec[16]; // [ms]
 
-    // Other Properties
+  // PID Gains
-    State.burntime = varValueVec[12];                              // [s]
+  State.Kp = varValueVec[17];
-    State.massPropellant = varValueVec[11];                        // [kg]
+  State.Ki = varValueVec[18];
-    State.massBurnout = State.massInitial - State.massPropellant; // [kg]
+  State.Kd = -1;
-    State.mdot = State.massPropellant / State.burntime;           // [kg/s]
-    State.mass = State.massInitial;                               // [kg]
-    State.burnElapsed = 2000;                                     // [s]
-    PrevState.thrust = 0;                                         // [N]
 
-    bool outcome = sim(State, PrevState);
+  std::cout << State.Kd << "\n";
 
-    std::cout << "Finished"
+  // Other Properties
-              << "\n";
+  State.burntime = varValueVec[12];                             // [s]
+  State.massPropellant = varValueVec[11];                       // [kg]
+  State.massBurnout = State.massInitial - State.massPropellant; // [kg]
+  State.mdot = State.massPropellant / State.burntime;           // [kg/s]
+  State.mass = State.massInitial;                               // [kg]
+  State.burnElapsed = 2000;                                     // [s]
+  PrevState.thrust = 0;                                         // [N]
 
-    if (outcome == 1) {
+  bool outcome = sim(State, PrevState);
-      std::cout << "Sim Result = Success!";
+
-      return 0;
+  std::cout << "Finished"
+            << "\n";
+
+  if (outcome == 1) {
+    std::cout << "Sim Result = Success!";
+    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;