diff --git a/include/Vehicle.h b/include/Vehicle.h index 654531f..d4e0342 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -20,6 +20,9 @@ struct Vehicle { double burntime; double burnVelocity; double thrust, burnElapsed, burnStart; + bool thrustFiring = false; + ; + double LQRx, LQRy, Fx, Fy, Fz; double momentX, momentY, momentZ; diff --git a/include/outVector.h b/include/outVector.h index f4550d9..e40535d 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -29,6 +29,8 @@ struct outVector { std::vector servo1 = std::vector(length, 0.0); std::vector servo2 = std::vector(length, 0.0); + + std::vector thrustFiring = std::vector(length, 0.0); }; #endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index 9858ace..fa3b5e3 100644 --- a/include/sim.h +++ b/include/sim.h @@ -106,10 +106,10 @@ void thrustSelection(Vehicle &State, int t) { else State.burnElapsed = 2000; // arbitrary number to ensure we don't burn - if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) + 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)) + } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383)) State.thrust = 0.8932 * pow(State.burnElapsed, 6) - 11.609 * pow(State.burnElapsed, 5) + 60.739 * pow(State.burnElapsed, 4) - @@ -119,6 +119,9 @@ void thrustSelection(Vehicle &State, int t) { else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46)) State.thrust = -195.78 * State.burnElapsed + 675.11; + + if (State.burnElapsed > 3.45) + State.thrustFiring = false; } void lqrCalc(Vehicle &State) { @@ -323,6 +326,8 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) { stateVector.servo1[t] = State.xServoDegs; stateVector.servo2[t] = State.yServoDegs; + + stateVector.thrustFiring[t] = State.thrustFiring; } void write2CSV(outVector &stateVector) { @@ -340,7 +345,7 @@ void write2CSV(outVector &stateVector) { // 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" + "pitchdot, rolldot, Servo1, Servo2, thrustFiring" << std::endl; std::cout << "Writing to csv...\n"; @@ -370,7 +375,9 @@ void write2CSV(outVector &stateVector) { outfile << stateVector.rolldot[t] * 180 / M_PI << ", "; outfile << stateVector.servo1[t] << ", "; - outfile << stateVector.servo2[t] << std::endl; + outfile << stateVector.servo2[t] << ", "; + + outfile << stateVector.thrustFiring[t] << std::endl; } outfile.close();