started debugging

include/Vehicle.h

@@ -35,7 +35,6 @@ struct Vehicle {
   double yError, yPrevError;
   double pError, pPrevError;
   double i_yError, i_pError = 0;
-  int pidTest = 0;
 
   double simTime;
   int stepSize;

include/outVector.h

@@ -4,7 +4,7 @@
 #define OUTVECTOR_H
 
 struct outVector {
-  int length = 10000; // current sim runs ~5000 steps, x2 just in case
+  int length = 100000; // current sim runs ~5000 steps, x2 just in case
 
   std::vector<double> x = std::vector<double>(length, 0.0);
   std::vector<double> y = std::vector<double>(length, 0.0);
@@ -33,6 +33,8 @@ struct outVector {
 
   std::vector<double> LQRx = std::vector<double>(length, 0.0);
   std::vector<double> LQRy = std::vector<double>(length, 0.0);
+
+  std::vector<double> thrust = std::vector<double>(length, 0.0);
 };
 
 #endif

include/sim.h

@@ -14,7 +14,7 @@ double integral(double x2, double x1, double dt);
 
 // Any parameters that are constants should be declared here instead of buried
 // in code
-double const dt = 0.001;
+double const dt = 0.01;
 double const g = -9.81;
 
 bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
@@ -29,14 +29,13 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
   // Start Sim
   do {
     thrustSelection(State, t);
-    // lqrCalc(State);
+    pidController(State, PrevState); // lqrCalc(State);
-    pidController(State, PrevState);
     TVC(State);
     vehicleDynamics(State, PrevState, t);
     state2vec(State, stateVector, t);
 
-    t++;
+    t += State.stepSize;
-  } while ((State.z > 0.0) || (State.thrust > 1.0));
+  } while ((State.z > 0.0) || (State.thrust > 0.1));
 
   write2CSV(stateVector, State);
 
@@ -59,7 +58,7 @@ void burnStartTimeCalc(Vehicle &State) {
   double velocity = State.vz;
   double h = 0;
 
-  double a, j, mass, thrust;
+  double mass, thrust;
 
   // Piecewise functions for F15 thrust curve
   for (double i = 0.148; i < 3.450; i = i + dt) {
@@ -107,6 +106,7 @@ void thrustSelection(Vehicle &State, int t) {
   if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
     State.thrustFiring = true;
     State.thrust = 65.165 * State.burnElapsed - 2.3921;
+
   } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
     State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
                    11.609 * pow(State.burnElapsed, 5) +
@@ -210,7 +210,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
                   State.Kd * d_yError);
     State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
                   State.Kd * d_pError);
-
   } else {
     State.LQRx = 0;
     State.LQRy = 0;
@@ -274,7 +273,7 @@ void TVC(Vehicle &State) {
 
 void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
   // Idot
-  if (t < 1) {
+  if (t < 0.1) {
     State.I11dot = 0;
     State.I22dot = 0;
     State.I33dot = 0;
@@ -285,20 +284,20 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
   }
 
   // pdot, qdot, rdot
-  State.yawddot = (State.momentX - State.I11dot * State.yawdot +
+  State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
-                   State.I22 * State.pitchdot * State.rolldot -
+                   State.I22 * PrevState.pitchdot * PrevState.rolldot -
-                   State.I33 * State.pitchdot * State.rolldot) /
+                   State.I33 * PrevState.pitchdot * PrevState.rolldot) /
                   State.I11;
-  State.pitchddot = (State.momentY - State.I22dot * State.pitchdot -
+  State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
-                     State.I11 * State.rolldot * State.yawdot +
+                     State.I11 * PrevState.rolldot * PrevState.yawdot +
-                     State.I33 * State.rolldot * State.yawdot) /
+                     State.I33 * PrevState.rolldot * PrevState.yawdot) /
                     State.I22;
-  State.rollddot = (State.momentZ - State.I33dot * State.rolldot +
+  State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
-                    State.I11 * State.pitchdot * State.yawdot -
+                    State.I11 * PrevState.pitchdot * PrevState.yawdot -
-                    State.I22 * State.pitchdot * State.yawdot) /
+                    State.I22 * PrevState.pitchdot * PrevState.yawdot) /
                    State.I33;
 
-  if (t < 1) {
+  if (t < 0.1) {
     State.x = 0;
     State.y = 0;
 
@@ -322,7 +321,7 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
     State.az = (State.Fz / State.mass) +
                (State.vy * State.yawdot - State.pitchdot * State.vx);
 
-    // vx vy vz in Body frame
+    // vx vy vz in Earth frame
     State.vx = integral(State.ax, PrevState.vx, State.stepSize);
     State.vy = integral(State.ay, PrevState.vy, State.stepSize);
     State.vz = integral(State.az, PrevState.vz, State.stepSize);
@@ -333,13 +332,14 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
     State.z = integral(State.vz, PrevState.z, State.stepSize);
 
     // Euler Angles
-    State.phidot = State.yawdot + (State.pitchdot * sin(State.yaw) +
+    State.phidot =
-                                   State.rolldot * cos(State.yaw)) *
+        State.yawdot + (sin(State.pitch) * (State.rolldot * cos(State.yaw) +
-                                      (sin(State.pitch) / cos(State.pitch));
+                                            State.pitchdot * sin(State.yaw))) /
+                           cos(State.pitch);
     State.thetadot =
-        State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.pitch);
+        State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
     State.psidot =
-        (State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) /
+        (State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
         cos(State.pitch);
 
     State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
@@ -379,6 +379,7 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
 
   stateVector.LQRx[t] = State.LQRx;
   stateVector.LQRy[t] = State.LQRy;
+  stateVector.thrust[t] = State.thrust;
 }
 
 void write2CSV(outVector &stateVector, Vehicle &State) {
@@ -396,13 +397,13 @@ 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, LQRx, LQRy, thrust"
           << std::endl;
 
   std::cout << "Writing to csv...\n";
 
   // writing to output file
-  for (int t = 0; t < State.simTime; t++) {
+  for (int t = 0; t < State.simTime; t += State.stepSize) {
     outfile << t << ", ";
 
     outfile << stateVector.x[t] << ", ";
@@ -431,7 +432,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
     outfile << stateVector.thrustFiring[t] << ", ";
 
     outfile << stateVector.LQRx[t] << ", ";
-    outfile << stateVector.LQRy[t] << std::endl;
+    outfile << stateVector.LQRy[t] << ", ";
+    outfile << stateVector.thrust[t] << std::endl;
   }
 
   outfile.close();

input.csv

@@ -1,7 +1,7 @@
 vx,0,m/s
 vy,0,m/s
 vz,0,m/s
-yaw,10,degs
+yaw,45,degs
 pitch,0,degs
 roll,0,degs
 yawdot,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,-1,x
+Kp,-6.8699,x
-Ki,-1,x
+Ki,0,x
-Kd,-1,x
+Kd,-0.775,x

matlabHelpers/simPlot.m

@@ -27,6 +27,9 @@ rolldot     = T(:, 16);
 Servo1      = T(:, 17);
 Servo2      = T(:, 18);
 
+LQRx      = T(:, 19);
+LQRy      = T(:, 20);
+
 % Acceleration
 subplot(3, 1, 1)
 plot(t, az)
@@ -91,3 +94,20 @@ title('Servo 2 Position vs Time')
 xlabel('Time (ms)')
 ylabel('Servo 2 Position (rad)')
 %saveas(gcf,'outputs/Servo Position vs Time.png')
+
+figure(4)
+
+% Servo 1 Position
+subplot(2, 1, 1)
+plot(t, LQRx)
+title('LQRx vs Time')
+xlabel('Time (ms)')
+ylabel('LQRx')
+
+% Servo 2 Position
+subplot(2, 1, 2)
+plot(t, LQRy)
+title('LQRy vs Time')
+xlabel('Time (ms)')
+ylabel('LQRy')
+%saveas(gcf,'outputs/Servo Position vs Time.png')

src/main.cpp

@@ -75,6 +75,9 @@ int main() {
   State.burnElapsed = 2000;                                     // [s]
   PrevState.thrust = 0;                                         // [N]
 
+  PrevState = State;
+  
+  std::cout << "START\n";
   bool outcome = sim(State, PrevState);
 
   std::cout << "Finished\n";