Almost there boys

.vscode/settings.json

@@ -42,7 +42,10 @@
         "vector": "cpp",
         "cctype": "cpp",
         "sstream": "cpp",
-        "string": "cpp"
+        "string": "cpp",
+        "chrono": "cpp",
+        "ratio": "cpp",
+        "thread": "cpp"
     },
     "C_Cpp.clang_format_fallbackStyle": "LLVM",
     "editor.formatOnSave": true,

include/sim.h

@@ -77,30 +77,72 @@ void burnStartTimeCalc(Vehicle &State) {
   double mass, thrust, acceleration;
 
   // Piecewise functions for F15 thrust curve
-  for (double i = 0.148; i < 3.450; i = i + dt) {
+  for (double i = 0.0; i < 3.450; i = i + dt) {
     mass = State.massInitial - i * State.mdot;
 
     if ((i > 0.147) && (i < 0.420))
       thrust = 65.165 * i - 2.3921;
-
     else if ((i > 0.419) && (i < 3.383))
       thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) -
                162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17;
-
     else if ((i > 3.382) && (i < 3.46))
       thrust = -195.78 * i + 675.11;
+    else if (i < 0.148)
+      thrust = 0.0;
 
     acceleration = (thrust / mass) + g;
     velocity = integral(acceleration, velocity, State.stepSize);
     h = integral(velocity, h, State.stepSize);
   }
 
-  State.z = 18.88;//h + (pow(velocity, 2) / (2 * -g)); // starting height
+  State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
   State.burnVelocity = velocity;               // terminal velocity
-  std::cout << State.z << std::endl;
+  // std::cout << State.z << std::endl;
+  // std::cout << State.burnVelocity << std::endl;
 
   double burnStartTime = State.burnVelocity / -g;
   State.simTime = (State.burntime + burnStartTime) * 1000;
+
+  int local_thrustFiring = 0;
+  double local_t = 0.0;
+  double local_a = 0.0;
+  double local_v = 0.0;
+  double local_h = State.z;
+  double local_tb = 0.0;
+  double local_thrust = 0.0;
+
+  // Pre sim simulation to simulate the simulation
+  do {
+    if ((std::abs(local_v + State.burnVelocity) < 1.10) &&
+        (local_thrustFiring == 0))
+      local_thrustFiring = 1;
+
+    if (local_thrustFiring == 0) {
+      local_thrust = 0.0;
+
+    } else if (local_thrustFiring == 1) {
+      if ((local_tb > 0.147) && (local_tb < 0.420))
+        local_thrust = 65.165 * local_tb - 2.3921;
+      else if ((local_tb > 0.419) && (local_tb < 3.383))
+        local_thrust = 0.8932 * pow(local_tb, 6) - 11.609 * pow(local_tb, 5) +
+                       60.739 * pow(local_tb, 4) - 162.99 * pow(local_tb, 3) +
+                       235.6 * pow(local_tb, 2) - 174.43 * local_tb + 67.17;
+      else if ((local_tb > 3.382) && (local_tb < 3.451))
+        local_thrust = -195.78 * local_tb + 675.11;
+      else if (local_tb > 3.45) {
+        local_thrust = 0.0;
+        local_thrustFiring = 3;
+        std::cout << (local_h) << std::endl;
+        std::cout << (local_v) << std::endl;
+      }
+
+      local_tb += State.stepSize / 1000.0;
+    }
+
+    local_v += (((local_thrust / State.mass) + g) * State.stepSize / 1000);
+    local_h += local_v * State.stepSize / 1000.0;
+
+  } while (local_thrustFiring != 3);
 }
 
 void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
@@ -197,21 +239,16 @@ void thrustSelection(Vehicle &State, int t) {
     // as well as current mass
     State.burnElapsed = (t - State.burnStart) / 1000;
     State.mass = State.massInitial - (State.mdot * State.burnElapsed);
-  }
+  } else if (std::abs(State.burnVelocity + State.vz) < 0.001) {
-
-  else if (abs(State.burnVelocity + State.vz) < 0.001) {
     // Start burn
     State.burnStart = t;
     State.burnElapsed = 0;
-  }
+  } else
-
-  else
     State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
 
   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) +
@@ -219,13 +256,13 @@ void thrustSelection(Vehicle &State, int t) {
                    162.99 * pow(State.burnElapsed, 3) +
                    235.6 * pow(State.burnElapsed, 2) -
                    174.43 * State.burnElapsed + 67.17;
-
   else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
-    State.thrust = -195.78 * State.burnElapsed - 675.11;
+    State.thrust = -195.78 * State.burnElapsed + 675.11;
 
   if (State.burnElapsed > 3.45) {
     State.thrustFiring = false;
     State.thrust = 0;
+    State.az = g;
   }
 }
 

matlabHelpers/polyFitTest.asv

@@ -0,0 +1,82 @@
+clear all; clc;
+%% Create Thrust Curve using Piecewise Function
+dt = 0.001;
+
+t1 = 0.148 : dt : 0.419;
+t2 = 0.420 : dt : 3.382;
+t3 = 3.383 : dt : 3.45;
+
+y1 = 65.165.*t1 - 2.3921;
+y2 = 0.8932*t2.^6 - 11.609*t2.^5 + 60.739*t2.^4 - 162.99*t2.^3 ...
+    + 235.6*t2.^2 - 174.43*t2 + 67.17;
+y3 = -195.78*t3 + 675.11;
+
+m = 1.2;
+
+a = [y1, y2, y3] ./ m;
+t = 0.148 : dt : 3.45;
+thrustCurve = [t', (a*m)'];
+
+%% Integrate
+v = 0;
+h = 0;
+g = -9.81;
+a = a + g;
+
+for i = 1 : length(a)
+    v = ((a(i)) * dt) + v;
+    h = v * dt + h;
+end
+
+h = h + ((v^2) / (2*-g))
+vb = v
+
+thrustFiring = 0;
+dt = 0.001;
+v = 0;
+j = 1;
+k = 1;
+t = 0;
+
+while 1
+    if abs(v + vb) < 0.01
+        thrustFiring = 1;
+        tb = 0;
+    end
+    
+    if thrustFiring == 0
+        v = g*dt + v;
+        h = v*dt + h;
+        a = g;
+    elseif thrustFiring == 1
+        if ((tb >= 0.148) && (tb <= 3.45))
+            v = v + a(j)*dt;
+            h = h + v*dt;
+            
+            disp(tb)
+            j = j + 1;
+        else
+            v = g*dt + v;
+            h = v*dt + h;
+        end
+        
+        tb = tb + dt;
+    end
+    accel(k) = a;
+    vz(k) = v;
+    z(k) = h;
+    time(k) = t;
+
+    t = t + dt;
+    k = k + 1;
+    if h < 0
+        break
+    end
+end
+
+figure(1)
+plot(time, z)
+figure(2)
+plot(time, vz)
+figure(3)
+plot(time, accel)

matlabHelpers/polyFitTest.m

@@ -0,0 +1,88 @@
+clear all; clc;
+%% Create Thrust Curve using Piecewise Function
+dt = 0.001;
+
+t1 = 0.148 : dt : 0.419;
+t2 = 0.420 : dt : 3.382;
+t3 = 3.383 : dt : 3.45;
+
+y1 = 65.165.*t1 - 2.3921;
+y2 = 0.8932*t2.^6 - 11.609*t2.^5 + 60.739*t2.^4 - 162.99*t2.^3 ...
+    + 235.6*t2.^2 - 174.43*t2 + 67.17;
+y3 = -195.78*t3 + 675.11;
+
+m = 1.2;
+
+a = [y1, y2, y3] ./ m;
+t = 0.148 : dt : 3.45;
+thrustCurve = [t', (a*m)'];
+
+%% Integrate
+v = 0;
+h = 0;
+g = -9.81;
+a = a + g;
+
+for i = 1 : length(a)
+    v = ((a(i)) * dt) + v;
+    h = v * dt + h;
+end
+
+h = h + ((v^2) / (2*-g))
+vb = v
+
+thrustFiring = 0;
+dt = 0.001;
+v = 0;
+j = 1;
+k = 1;
+t = 0;
+
+while 1
+    if (abs(v + vb) < 0.005) && (thrustFiring == 0)
+        thrustFiring = 1;
+        tb = 0;
+    end
+    
+    if thrustFiring == 0
+        v = g*dt + v;
+        h = v*dt + h;
+        az = g;
+    elseif thrustFiring == 1
+        if ((tb >= 0.148) && (tb <= 3.45))
+            v = v + a(j)*dt;
+            h = h + v*dt;
+            az = a(j);
+
+            j = j + 1;
+        else
+            if tb > 3.45
+                thrustFiring = 0;
+            end
+            
+            v = g*dt + v;
+            h = v*dt + h;
+            az = g;
+        end
+        
+        tb = tb + dt;
+    end
+    
+    accel(k) = az;
+    vz(k) = v;
+    z(k) = h;
+    time(k) = t;
+
+    t = t + dt;
+    k = k + 1;
+    if ((h < 0) && (thrustFiring == 0))
+        break
+    end
+end
+
+figure(1)
+plot(time, z)
+figure(2)
+plot(time, vz)
+figure(3)
+plot(time, accel)

matlabHelpers/test.m

@@ -0,0 +1,46 @@
+clear all; close all; clc;
+
+g = -9.81;
+mass = 1.2;
+mdot = 0.0182;
+dt = 0.001;
+
+vprev = 0;
+hprev = 0;
+i = 1;
+
+for t = 0 : dt : 3.45
+    m = mass - mdot*t;
+    if t < 0.148
+        thrust = 0;
+    elseif (t > 0.147) && (t < 0.419)
+        thrust = 65.165.*t - 2.3921;
+    elseif (t > 0.420) && (t < 3.382)
+        thrust = 0.8932*t^6 - 11.609*t^5 + 60.739*t^4 - 162.99*t^3 ...
+            + 235.6*t^2 - 174.43*t + 67.17;
+    elseif (t > 3.381) && (t < 3.451)
+        thrust = -195.78*t + 675.11;
+    elseif t > 3.45
+        thrust = 0;
+    end
+   
+    acceleration(i) = (thrust / mass) + g;
+    velocity(i) = acceleration(i) * dt + vprev;
+    height(i) = velocity(i) * dt + hprev;
+    
+    vprev = velocity(i);
+    hprev = height(i);
+    i = i + 1;
+end
+figure(1)
+hold on
+t = 0 : dt : 3.45;
+plot(t + 0.7712, velocity - max(velocity))
+
+t1 = 0 : 0.001 : 0.7712;
+v = -9.81*t1;
+plot(t1, v)
+
+figure(2)
+h = 0.5*-9.81*t1.^2
+plot(t1, h)

src/main.cpp

@@ -28,8 +28,8 @@ int main() {
   State.vz = 0; // [m/s]
 
   // Initial YPR
-  State.yaw = 10 * M_PI / 180;   // [rad]
+  State.yaw = 75 * M_PI / 180;   // [rad]
-  State.pitch = 5 * M_PI / 180; // [rad]
+  State.pitch = 30 * M_PI / 180; // [rad]
   State.roll = 0 * M_PI / 180;   // [rad]
 
   // Initial YPRdot
@@ -38,7 +38,7 @@ int main() {
   State.rolldot = 0 * M_PI / 180;   // [rad/s]
 
   // Servo Limitation
-  State.maxServo = 7;   // [degs]
+  State.maxServo = 7;       // [degs]
   State.maxServoRate = 360; // [degs/sec]
 
   // Vehicle Properties