Cleaned up PID

include/Vehicle.h

@@ -34,7 +34,7 @@ struct Vehicle {
   double Kp, Ki, Kd;
   double yError, yPrevError;
   double pError, pPrevError;
-  double y_pidRefeed, p_pidRefeed, i_yError, i_pError = 0;
+  double i_yError, i_pError = 0;
 
   double simTime;
   int stepSize;

include/sim.h

@@ -3,8 +3,8 @@
 
 void burnStartTimeCalc(struct Vehicle &);
 void thrustSelection(struct Vehicle &, int t);
-void lqrCalc(struct Vehicle &);
-void pidController(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);
@@ -30,7 +30,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
   do {
     thrustSelection(State, t);
     // lqrCalc(State);
-    pidController(State);
+    pidController(State, PrevState);
     TVC(State);
     vehicleDynamics(State, PrevState, t);
     state2vec(State, stateVector, t);
@@ -179,36 +179,32 @@ void lqrCalc(Vehicle &State) {
     State.LQRy = -1 * State.thrust;
 }
 
-void pidController(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);
 
-  if (State.thrust > 1) {
-    State.yError = State.yaw;   // - State.y_pidRefeed;
-    State.pError = State.pitch; // - State.p_pidRefeed;
+  // Make sure we start reacting when we start burning
+  if (State.thrust > 0.01) {
+    // Integral of Error
+    State.i_yError = integral(State.yaw, State.i_yError, State.stepSize);
+    State.i_pError = integral(State.pitch, State.i_pError, State.stepSize);
 
-    State.i_yError += State.yError * State.stepSize / 1000;
-    State.i_pError += State.pError * State.stepSize / 1000;
+    // Derivative of Error
+    double d_yError = derivative(State.yaw, PrevState.yaw, State.stepSize);
+    double d_pError = derivative(State.pitch, PrevState.pitch, State.stepSize);
 
-    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 +
+    // PID Function - it says LQR but this is just so that it gets passed to the
+    // TVC block properly
+    State.LQRx = (State.Kp * State.yaw + State.Ki * State.i_yError +
                   State.Kd * d_yError);
-    State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
+    State.LQRy = (State.Kp * State.pitch + 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;
@@ -228,7 +224,7 @@ void pidController(Vehicle &State) {
 }
 
 void TVC(Vehicle &State) {
-  if (State.thrust < 1) {
+  if (State.thrust < 0.1) {
     // Define forces and moments for t = 0
     State.Fx = 0;
     State.Fy = 0;
@@ -313,18 +309,12 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
     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);
+    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 Body frame
     State.vx = integral(State.ax, PrevState.vx, State.stepSize);
@@ -439,6 +429,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
   }
 
   outfile.close();
+  std::cout << "Output File Closed\n";
 }
 
 double derivative(double current, double previous, double step) {

input.csv

@@ -1,8 +1,8 @@
 vx,0,m/s
 vy,0,m/s
 vz,0,m/s
-yaw,5,degs
-pitch,10,degs
+yaw,10,degs
+pitch,0,degs
 roll,0,degs
 yawdot,0,degs/s
 pitchdot,0,degs/s
@@ -15,6 +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, x
-Kd,0, x
+Kp,-0.25,x
+Ki,0.4,x
+Kd,1,x

src/main.cpp

@@ -24,7 +24,7 @@ int main() {
   if (!inFile.is_open())
     throw std::runtime_error("Could not open file");
 
-  std::vector<double> varValueVec = std::vector<double>(17, 0.0);
+  std::vector<double> varValueVec = std::vector<double>(20, 0.0);
   std::string varName, varValue, varUnits;
 
   for (int i; i < 20; i++) {
@@ -66,6 +66,8 @@ int main() {
   State.Ki = varValueVec[18];
   State.Kd = varValueVec[19];
 
+  std::cout << State.Kp << "\n";
+  std::cout << State.Ki << "\n";
   std::cout << State.Kd << "\n";
 
   // Other Properties
@@ -79,8 +81,7 @@ int main() {
 
   bool outcome = sim(State, PrevState);
 
-  std::cout << "Finished"
-            << "\n";
+  std::cout << "Finished\n";
 
   if (outcome == 1) {
     std::cout << "Sim Result = Success!";