From cf93accf8a76f8b683b61d4f04e43628954a5663 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 1 Nov 2021 12:27:54 -0700 Subject: [PATCH 1/2] Almost there boys --- .vscode/settings.json | 5 +- include/sim.h | 65 ++++++++++++++++++++------ matlabHelpers/polyFitTest.asv | 82 ++++++++++++++++++++++++++++++++ matlabHelpers/polyFitTest.m | 88 +++++++++++++++++++++++++++++++++++ matlabHelpers/test.m | 46 ++++++++++++++++++ src/main.cpp | 6 +-- 6 files changed, 274 insertions(+), 18 deletions(-) create mode 100644 matlabHelpers/polyFitTest.asv create mode 100644 matlabHelpers/polyFitTest.m create mode 100644 matlabHelpers/test.m diff --git a/.vscode/settings.json b/.vscode/settings.json index 0f4f1c9..acd4c6e 100644 --- a/.vscode/settings.json +++ b/.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, diff --git a/include/sim.h b/include/sim.h index 97a1238..94403fe 100644 --- a/include/sim.h +++ b/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 (abs(State.burnVelocity + State.vz) < 0.001) { + } else if (std::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; } } diff --git a/matlabHelpers/polyFitTest.asv b/matlabHelpers/polyFitTest.asv new file mode 100644 index 0000000..5c8d85f --- /dev/null +++ b/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) diff --git a/matlabHelpers/polyFitTest.m b/matlabHelpers/polyFitTest.m new file mode 100644 index 0000000..8d7a557 --- /dev/null +++ b/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) diff --git a/matlabHelpers/test.m b/matlabHelpers/test.m new file mode 100644 index 0000000..8ae3a5a --- /dev/null +++ b/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) diff --git a/src/main.cpp b/src/main.cpp index ee7d759..a05a65a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -28,8 +28,8 @@ int main() { State.vz = 0; // [m/s] // Initial YPR - State.yaw = 10 * M_PI / 180; // [rad] - State.pitch = 5 * M_PI / 180; // [rad] + State.yaw = 75 * 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 From 2c194312d3b1e0efebc9cf6ba166cb370ee93d07 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 1 Nov 2021 13:16:51 -0700 Subject: [PATCH 2/2] Fixed --- include/sim.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/sim.h b/include/sim.h index 0c95b92..767626b 100644 --- a/include/sim.h +++ b/include/sim.h @@ -143,6 +143,8 @@ void burnStartTimeCalc(Vehicle &State) { local_h += local_v * State.stepSize / 1000.0; } while (local_thrustFiring != 3); + + State.z = State.z - local_h; } void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { @@ -239,7 +241,7 @@ 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 (std::abs(State.burnVelocity + State.vz) < 0.90) { // Start burn State.burnStart = t; State.burnElapsed = 0;