From 87fb8f8621ab12e58f1f77883dbb8ffc1d6ef718 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 8 Nov 2021 14:43:52 -0700 Subject: [PATCH] fixed, gonna make some more changes to make sure we're utilizing our whole burn --- include/Vehicle.h | 36 +++++++++++++++---------------- include/sim.h | 55 +++++++++++++++++++++++------------------------ include/teensy.h | 49 +++++++++++++++++++++-------------------- src/main.cpp | 1 + 4 files changed, 70 insertions(+), 71 deletions(-) diff --git a/include/Vehicle.h b/include/Vehicle.h index 31de448..ab9e9be 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -28,47 +28,47 @@ struct Vehicle { double I11, I22, I33; double I11dot, I22dot, I33dot; - int maxServo; - int maxServoRate; + double maxServo; + double maxServoRate; double xServoDegs, yServoDegs; double xServoDegsDot, yServoDegsDot; double Kp, Ki, Kd; double yError, yPrevError; double pError, pPrevError; - double i_yError, i_pError = 0; + double i_yError, i_pError = 0.0; double d_yError, d_pError; double simTime; - int stepSize; + double stepSize; - int time = 0; + double time = 0.0; }; void init_Vehicle(Vehicle &State) { // PID Gains State.Kp = -6.8699; - State.Ki = 0; + State.Ki = 0.0; State.Kd = -0.775; // Initial Velocity - State.vx = 0; // [m/s] - State.vy = 0; // [m/s] - State.vz = 0; // [m/s] + State.vx = 0.0; // [m/s] + State.vy = 0.0; // [m/s] + State.vz = 0.0; // [m/s] // Initial YPR - State.yaw = 45 * M_PI / 180; // [rad] - State.pitch = 45 * M_PI / 180; // [rad] - State.roll = 0 * M_PI / 180; // [rad] + State.yaw = 45.0 * M_PI / 180.0; // [rad] + State.pitch = 45.0 * M_PI / 180.0; // [rad] + State.roll = 0.0 * M_PI / 180.0; // [rad] // Initial YPRdot - State.yawdot = 1 * M_PI / 180; // [rad/s] - State.pitchdot = -1 * M_PI / 180; // [rad/s] - State.rolldot = 0 * M_PI / 180; // [rad/s] + State.yawdot = 1.0 * M_PI / 180.0; // [rad/s] + State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s] + State.rolldot = 0.0 * M_PI / 180.0; // [rad/s] // Servo Limitation - State.maxServo = 7; // [degs] - State.maxServoRate = 360; // [degs/sec] + State.maxServo = 7.0; // [degs] + State.maxServoRate = 360.0; // [degs/sec] // Vehicle Properties State.massInitial = 1.2; // [kg] @@ -77,7 +77,7 @@ void init_Vehicle(Vehicle &State) { State.momentArm = 0.145; // [m] // Sim Step Size - State.stepSize = 1; // [ms] + State.stepSize = 1.0; // [ms] // Other Properties State.massPropellant = 0.06; // [kg] diff --git a/include/sim.h b/include/sim.h index 0045eff..6fa37e6 100644 --- a/include/sim.h +++ b/include/sim.h @@ -19,7 +19,7 @@ double const g = -9.81; void burnStartTimeCalc(Vehicle &State) { double velocity = State.vz; - double h = 0; + double h = 0.0; double mass, thrust; @@ -37,39 +37,39 @@ void burnStartTimeCalc(Vehicle &State) { else if ((i > 3.382) && (i < 3.46)) thrust = -195.78 * i + 675.11; else - thrust = 0; + thrust = 0.0; velocity = (((thrust / mass) + g) * dt) + velocity; h = (((thrust / mass) + g) * dt) + h; } - State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height - State.z = 18.9; + State.z = h + (pow(velocity, 2) / (2.0 * -g)); // starting height + State.z = 19.05; State.burnVelocity = velocity; // terminal velocity double burnStartTime = State.burnVelocity / -g; - State.simTime = (State.burntime + burnStartTime) * 1000; + State.simTime = (State.burntime + burnStartTime) * 1000.0; } void vehicleDynamics(Vehicle &State, Vehicle &PrevState) { // Moment of Inertia - State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) + - pow(State.vehicleRadius, 2) / 4); - State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) + - pow(State.vehicleRadius, 2) / 4); + State.I11 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) + + pow(State.vehicleRadius, 2) / 4.0); + State.I22 = State.mass * ((1 / 12.0) * pow(State.vehicleHeight, 2) + + pow(State.vehicleRadius, 2) / 4.0); State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2); // Idot if (State.time < 0.1) { - State.I11dot = 0; - State.I22dot = 0; - State.I33dot = 0; + State.I11dot = 0.0; + State.I22dot = 0.0; + State.I33dot = 0.0; - State.x = 0; - State.y = 0; + State.x = 0.0; + State.y = 0.0; - State.ax = 0; - State.ay = 0; + State.ax = 0.0; + State.ay = 0.0; State.az = State.Fz / State.massInitial; } else { @@ -145,7 +145,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) { State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize); // TVC block properly - State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError + State.Kd * State.d_yError) / State.momentArm; @@ -154,8 +153,8 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) { State.momentArm; } else { - State.PIDx = 0; - State.PIDy = 0; + State.PIDx = 0.0; + State.PIDy = 0.0; } // PID Force Limiter @@ -166,18 +165,18 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) { void TVC(Vehicle &State, Vehicle &PrevState) { if (State.thrust < 0.1) { // Define forces and moments for t = 0 - State.Fx = 0; - State.Fy = 0; + State.Fx = 0.0; + State.Fy = 0.0; State.Fz = g * State.massInitial; - State.momentX = 0; - State.momentY = 0; - State.momentZ = 0; + State.momentX = 0.0; + State.momentY = 0.0; + State.momentZ = 0.0; } else { // Convert servo position to degrees for comparison to max allowable - State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust); - State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust); + State.xServoDegs = (180.0 / M_PI) * asin(State.PIDx / State.thrust); + State.yServoDegs = (180.0 / M_PI) * asin(State.PIDy / State.thrust); // Limit Servo Position State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo); @@ -238,12 +237,12 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) { } double derivative(double current, double previous, double step) { - double dxdt = (current - previous) / (step / 1000); + double dxdt = (current - previous) / (step / 1000.0); return dxdt; } double integral(double currentChange, double prevValue, double dt) { - return (currentChange * dt / 1000) + prevValue; + return (currentChange * dt / 1000.0) + prevValue; } double limit(double value, double upr, double lwr) { diff --git a/include/teensy.h b/include/teensy.h index 283e721..e414339 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -17,11 +17,11 @@ File dataFile; double loadCellCalibrate() { // place code to calibrate load cells in here double loadTotal; - for (double t = 0; t == 10; ++t) { - loadTotal += 1; + for (double t = 0.0; t == 10.0; ++t) { + loadTotal += 1.0; delay(15); } - return loadTotal / 10; + return loadTotal / 10.0; } void initFile() { @@ -39,7 +39,7 @@ void initFile() { const char *fileName; if (SD.exists("simOut.csv")) { - while (i > 0) { + while (i > 0.0) { fileName = ("simOut_" + String(i) + ".csv").c_str(); if (!SD.exists(fileName)) { @@ -54,7 +54,7 @@ void initFile() { Serial.println("Error opening output file. \n\nABORTING SIMULATION"); teensyAbort(); } - i = 0; + i = 0.0; } else { i++; @@ -87,23 +87,22 @@ 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.burnElapsed = (State.time - State.burnStart) / 1000.0; State.mass = State.massInitial - (State.mdot * State.burnElapsed); - } - else if (abs(State.burnVelocity + State.vz) < 0.001) { + } else if (abs(State.burnVelocity + State.vz) < 0.01) { // Start burn State.burnStart = State.time; State.burnElapsed = 0; - } - else + } 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) + @@ -117,25 +116,25 @@ void thrustInfo(Vehicle &State) { if (State.burnElapsed > 3.45) { State.thrustFiring = false; - State.thrust = 0; + State.thrust = 0.0; } } void processTVC(Vehicle &State) { - if (State.time == 0) { + if (State.time == 0.0) { Serial.println("WARNING: processTVC not implemented for TEENSY"); } // Vector math to aqcuire thrust vector components - State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180)); - State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180)); + State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0)); + State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0)); State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) + (State.mass * g); // Calculate moment created by Fx and Fy State.momentX = State.Fx * State.momentArm; State.momentY = State.Fy * State.momentArm; - State.momentZ = 0; + State.momentZ = 0.0; } void write2CSV(Vehicle &State) { @@ -163,18 +162,18 @@ void write2CSV(Vehicle &State) { dataFile.print(State.az); dataFile.print(","); - dataFile.print(State.yaw * 180 / M_PI); + dataFile.print(State.yaw * 180.0 / M_PI); dataFile.print(","); - dataFile.print(State.pitch * 180 / M_PI); + dataFile.print(State.pitch * 180.0 / M_PI); dataFile.print(","); - dataFile.print(State.roll * 180 / M_PI); + dataFile.print(State.roll * 180.0 / M_PI); dataFile.print(","); - dataFile.print(State.yawdot * 180 / M_PI); + dataFile.print(State.yawdot * 180.0 / M_PI); dataFile.print(","); - dataFile.print(State.pitchdot * 180 / M_PI); + dataFile.print(State.pitchdot * 180.0 / M_PI); dataFile.print(","); - dataFile.print(State.rolldot * 180 / M_PI); + dataFile.print(State.rolldot * 180.0 / M_PI); dataFile.print(","); dataFile.print(State.xServoDegs); @@ -195,9 +194,9 @@ void write2CSV(Vehicle &State) { } void printSimResults(Vehicle &State) { - State.yaw = State.yaw * 180 / M_PI; - State.pitch = State.pitch * 180 / M_PI; - State.roll = State.roll * 180 / M_PI; + State.yaw = State.yaw * 180.0 / M_PI; + State.pitch = State.pitch * 180.0 / M_PI; + State.roll = State.roll * 180.0 / M_PI; double landing_angle = pow(State.yaw * State.yaw + State.pitch * State.pitch, .5); diff --git a/src/main.cpp b/src/main.cpp index d1a400e..24f9b57 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -43,6 +43,7 @@ void setup() { // Determine when to burn burnStartTimeCalc(State); + Serial.println(State.burnVelocity); Serial.println("Starting Height Calculated"); delay(1000); loadCellCalibrate();