From 0058af966740e4eb728a0737f2209cee05d84068 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 11 Nov 2021 21:58:57 -0700 Subject: [PATCH] Fixed differences between native and teensy, added getThrust() function to clean up thrustInfo() --- include/Vehicle.h | 2 +- include/native.h | 40 +++++++++++----------------------------- include/sim.h | 23 +++++++++++++++++++++++ include/teensy.h | 39 +++++++++++---------------------------- matlabHelpers/simPlot.m | 10 ++++++++++ src/main.cpp | 12 +++++------- 6 files changed, 61 insertions(+), 65 deletions(-) diff --git a/include/Vehicle.h b/include/Vehicle.h index ab9e9be..921c953 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -20,7 +20,7 @@ struct Vehicle { double burntime; double burnVelocity; double thrust, burnElapsed, burnStart; - bool thrustFiring = false; + int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn double PIDx, PIDy, Fx, Fy, Fz; double momentX, momentY, momentZ; diff --git a/include/native.h b/include/native.h index 9031117..d12f87a 100644 --- a/include/native.h +++ b/include/native.h @@ -8,41 +8,23 @@ void write2CSV(struct outVector &, struct Vehicle &); void printSimResults(struct Vehicle &); void thrustInfo(Vehicle &State) { - - if (State.burnElapsed != 2000) { - // determine where in the thrust curve we're at based on elapsed burn time - // as well as current mass - State.burnElapsed = (State.time - State.burnStart) / 1000; - State.mass = State.massInitial - (State.mdot * State.burnElapsed); - } - - else if (abs(State.burnVelocity + State.vz) < 0.001) { + if ((std::abs(State.burnVelocity + State.vz) < 1.03) && + (State.thrustFiring == 0)) { // Start burn State.burnStart = State.time; - State.burnElapsed = 0; - } + State.burnElapsed = 0.0; + State.thrustFiring = 1; - else - State.burnElapsed = 2000; // arbitrary number to ensure we don't burn + getThrust(State); - if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) { - State.thrustFiring = true; - State.thrust = 65.165 * State.burnElapsed - 2.3921; + } else if (State.thrustFiring == 1) { + State.burnElapsed = (State.time - State.burnStart) / 1000.0; + State.mass = State.massInitial - (State.mdot * State.burnElapsed); - } 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) - - 162.99 * pow(State.burnElapsed, 3) + - 235.6 * pow(State.burnElapsed, 2) - - 174.43 * State.burnElapsed + 67.17; + getThrust(State); - 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; - State.thrust = 0; + } else { + State.thrust = 0.0; } } diff --git a/include/sim.h b/include/sim.h index 6fa37e6..d107292 100644 --- a/include/sim.h +++ b/include/sim.h @@ -11,6 +11,7 @@ void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &); double derivative(double current, double previous, double step); double integral(double currentChange, double prevValue, double dt); double limit(double value, double upr, double lwr); +void getThrust(struct Vehicle &); // Any parameters that are constants should be declared here instead of // buried in code @@ -254,4 +255,26 @@ double limit(double value, double upr, double lwr) { value = value; return value; +} + +void getThrust(Vehicle &State) { + if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) { + 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.0) - + 11.609 * pow(State.burnElapsed, 5.0) + + 60.739 * pow(State.burnElapsed, 4.0) - + 162.99 * pow(State.burnElapsed, 3.0) + + 235.6 * pow(State.burnElapsed, 2.0) - + 174.43 * State.burnElapsed + 67.17; + + 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 = 2; + State.thrust = 0.0; + } } \ No newline at end of file diff --git a/include/teensy.h b/include/teensy.h index c02166e..06fa3ae 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -35,7 +35,6 @@ void initFile() { Serial.println("Card initialized."); int i = 1; - Serial.print("simOut_" + String(i) + ".csv"); const char *fileName; if (SD.exists("simOut.csv")) { @@ -84,38 +83,22 @@ void thrustInfo(Vehicle &State) { Serial.println("WARNING: thrustInfo not implemented for TEENSY"); } - if (State.burnElapsed != 2000) { - // determine where in the thrust curve we're at based on elapsed burn time - // as well as current mass + if ((abs(State.burnVelocity + State.vz) < 1.03) && + (State.thrustFiring == 0)) { + // Start burn + State.burnStart = State.time; + State.burnElapsed = 0.0; + State.thrustFiring = 1; + + getThrust(State); + + } else if (State.thrustFiring == 1) { State.burnElapsed = (State.time - State.burnStart) / 1000.0; State.mass = State.massInitial - (State.mdot * State.burnElapsed); - } else if (abs(State.burnVelocity + State.vz) < 0.01) { - // Start burn - State.burnStart = State.time; - State.burnElapsed = 0; + getThrust(State); } else { - State.burnElapsed = 2000; // arbitrary number to ensure we don't burn - } - // Serial.println(abs(State.burnVelocity + State.vz)); - - 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) + - 60.739 * pow(State.burnElapsed, 4) - - 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; - - if (State.burnElapsed > 3.45) { - State.thrustFiring = false; State.thrust = 0.0; } } diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m index 45e56aa..ba93e0f 100644 --- a/matlabHelpers/simPlot.m +++ b/matlabHelpers/simPlot.m @@ -30,6 +30,7 @@ Servo2 = T(:, 18); PIDx = T(:, 20); PIDy = T(:, 21); +thrust = T(:, 22); % Acceleration subplot(3, 1, 1) plot(t, az) @@ -91,3 +92,12 @@ plot(t, Servo2) title('Servo 2 Position vs Time') xlabel('Time (ms)') ylabel('Servo 2 Position (rad)') + +figure(4) + +% Servo 1 Position + +plot(t, thrust) +title('Thrust vs Time') +xlabel('Time (ms)') +ylabel('Thrust (N)') diff --git a/src/main.cpp b/src/main.cpp index 24f9b57..d9cfb0a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -36,14 +36,13 @@ void setup() { } #elif defined(TEENSY) void setup() { - delay(1000); + delay(5000); init_Vehicle(State); Serial.println("Simulated Vehicle Initalized"); delay(1000); // Determine when to burn burnStartTimeCalc(State); - Serial.println(State.burnVelocity); Serial.println("Starting Height Calculated"); delay(1000); loadCellCalibrate(); @@ -65,10 +64,9 @@ void loop() { // Set "prev" values for next timestep PrevState = State; - State.time += State.stepSize; - if (State.z < 0.0) { + if ((State.z < 0.0) && (State.thrustFiring == 2)) { write2CSV(stateVector, State); printSimResults(State); init_Vehicle(State); @@ -90,14 +88,14 @@ void loop() { State.time += State.stepSize; - if (State.z < 0.0) { + if ((State.z < 0.0) && (State.thrustFiring == 2)) { printSimResults(State); init_Vehicle(State); delay(1000); Serial.println("Last run duration:" + String(millis() - last) + " ms"); closeFile(); - delay(10000); + delay(20000); Serial.println("Restarting Sim"); Serial.println( @@ -113,7 +111,7 @@ int main() { do { loop(); - } while ((State.z > 0.0)); + } while ((State.z > 0.0) || (State.thrustFiring != 2)); return 0; }