diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 8fde7d0..935f093 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,10 +1,10 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "ms-vscode.cpptools", - "platformio.platformio-ide", - "usernamehw.errorlens", - "wayou.vscode-todo-highlight" - ] -} +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "ms-vscode.cpptools", + "platformio.platformio-ide", + "usernamehw.errorlens", + "wayou.vscode-todo-highlight" + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index acd4c6e..cbe555d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -45,7 +45,8 @@ "string": "cpp", "chrono": "cpp", "ratio": "cpp", - "thread": "cpp" + "thread": "cpp", + "string_view": "cpp" }, "C_Cpp.clang_format_fallbackStyle": "LLVM", "editor.formatOnSave": true, diff --git a/include/Vehicle.h b/include/Vehicle.h index 31de448..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; @@ -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/native.h b/include/native.h index 1e69dce..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; } } @@ -117,6 +99,10 @@ void write2CSV(outVector &stateVector, 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; + double landing_angle = pow(State.yaw * State.yaw + State.pitch * State.pitch, .5); diff --git a/include/outVector.h b/include/outVector.h index e77c77f..360a396 100644 --- a/include/outVector.h +++ b/include/outVector.h @@ -4,11 +4,7 @@ #define OUTVECTOR_H struct outVector { -#if defined(NATIVE) || defined(_WIN32) - int length = 100000; // current sim runs ~5000 steps, x2 just in case -#elif defined(TEENSY) - int length = 1000; // current sim runs ~5000 steps, x2 just in case -#endif + int length = 10000; // current sim runs ~5000 steps, x2 just in case std::vector x = std::vector(length, 0.0); std::vector y = std::vector(length, 0.0); diff --git a/include/sim.h b/include/sim.h index 0904153..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 @@ -19,7 +20,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 +38,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 +146,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 +154,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 +166,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); @@ -235,18 +235,15 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) { stateVector.PIDy[t] = State.PIDy; stateVector.thrust[t] = State.thrust; - - // Set "prev" values for next timestep - PrevState = State; } 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) { @@ -258,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 e82ae79..d6c054d 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,49 +1,105 @@ #include "Vehicle.h" +#include +#include +#include +double loadCellCalibrate(); +void initFile(); void thrustInfo(struct Vehicle &); -void processTVC(struct LoadCells &); -void write2CSV(struct outVector &, struct Vehicle &); +void processTVC(struct Vehicle &); +void write2CSV(struct Vehicle &); void printSimResults(struct Vehicle &); +void teensyAbort(); + +const int chipSelect = BUILTIN_SDCARD; +File dataFile; + +double loadCellCalibrate() { + // place code to calibrate load cells in here + double loadTotal; + for (double t = 0.0; t == 10.0; ++t) { + loadTotal += 1.0; + delay(15); + } + return loadTotal / 10.0; +} + +void initFile() { + Serial.print("Initializing SD card..."); + + // see if the card is present and can be initialized: + if (!SD.begin(chipSelect)) { + Serial.println("Card failed, or not present. \n\nABORTING SIMULATION"); + teensyAbort(); + } + Serial.println("Card initialized."); + + int i = 1; + const char *fileName; + + if (SD.exists("simOut.csv")) { + while (i > 0.0) { + fileName = ("simOut_" + String(i) + ".csv").c_str(); + + if (!SD.exists(fileName)) { + // Open simOut_i.csv + dataFile = SD.open(fileName, FILE_WRITE); + + if (dataFile) { + Serial.println("File successfully opened!"); + + } else { + // if the file didn't open, print an error: + Serial.println("Error opening output file. \n\nABORTING SIMULATION"); + teensyAbort(); + } + i = 0.0; + + } else { + i++; + } + } + } else { + // Open simOut.csv + dataFile = SD.open("simOut.csv", FILE_WRITE); + if (dataFile) { + Serial.println("File successfully opened!"); + + } else { + // if the file didn't open, print an error: + Serial.println("Error opening output file. \n\nABORTING SIMULATION"); + teensyAbort(); + } + } + + // File Header + dataFile.println( + "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot," + "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"); +} void thrustInfo(Vehicle &State) { if (State.time == 0) { 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 - 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 ((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; } } @@ -63,15 +119,70 @@ void processTVC(Vehicle &State, LoadCells &loadCells) { // 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(outVector &stateVector, Vehicle &State) { +void write2CSV(Vehicle &State) { + dataFile.print(String(State.time, 5)); + dataFile.print(","); - Serial.println("WARNING: write2CSV not implemented for TEENSY"); + dataFile.print(String(State.x, 5)); + dataFile.print(","); + dataFile.print(String(State.y, 5)); + dataFile.print(","); + dataFile.print(String(State.z, 5)); + dataFile.print(","); + + dataFile.print(String(State.vx, 5)); + dataFile.print(","); + dataFile.print(String(State.vy, 5)); + dataFile.print(","); + dataFile.print(String(State.vz, 5)); + dataFile.print(","); + + dataFile.print(String(State.ax, 5)); + dataFile.print(","); + dataFile.print(String(State.ay, 5)); + dataFile.print(","); + dataFile.print(String(State.az, 5)); + dataFile.print(","); + + dataFile.print(String(State.yaw * 180.0 / M_PI, 5)); + dataFile.print(","); + dataFile.print(String(State.pitch * 180.0 / M_PI, 5)); + dataFile.print(","); + dataFile.print(String(State.roll * 180.0 / M_PI, 5)); + dataFile.print(","); + + dataFile.print(String(State.yawdot * 180.0 / M_PI, 5)); + dataFile.print(","); + dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5)); + dataFile.print(","); + dataFile.print(String(State.rolldot * 180.0 / M_PI, 5)); + dataFile.print(","); + + dataFile.print(String(State.xServoDegs, 5)); + dataFile.print(","); + dataFile.print(String(State.yServoDegs, 5)); + dataFile.print(","); + + dataFile.print(String(State.thrustFiring, 5)); + dataFile.print(","); + + dataFile.print(String(State.PIDx, 5)); + dataFile.print(","); + dataFile.print(String(State.PIDy, 5)); + dataFile.print(","); + + dataFile.print(String(State.thrust, 5)); + dataFile.print("\n"); } void printSimResults(Vehicle &State) { + 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); @@ -94,5 +205,20 @@ void printSimResults(Vehicle &State) { Serial.print("Final Velocity: [" + String(State.vx) + ", " + String(State.vy) + ", " + String(State.vz) + "]\n"); - Serial.print("\n\nSimulation Complete\n"); + Serial.print("\nSimulation Complete\n"); +} + +void closeFile() { + // close the file: + dataFile.close(); + Serial.println("File closed\n"); +} + +void teensyAbort() { + while (1) { + digitalWrite(BUILTIN_LED, HIGH); + delay(1000); + digitalWrite(BUILTIN_LED, LOW); + delay(1000); + } } \ No newline at end of file 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 22e61b6..17ff880 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,8 @@ #include #elif defined(TEENSY) #include + +int BUILTIN_LED = 13; unsigned long last; #endif @@ -18,6 +20,7 @@ unsigned long last; #if defined(NATIVE) || defined(_WIN32) #include "native.h" +outVector stateVector; #elif defined(TEENSY) #include "LoadCells.h" @@ -35,7 +38,6 @@ const int lc_data_3 = 3; Vehicle State; Vehicle PrevState; -outVector stateVector; #if defined(NATIVE) || defined(_WIN32) void setup() { @@ -71,6 +73,8 @@ void setup() { Serial.println("Load Cells Calibrated"); delay(1000); + initFile(); + delay(1000); } #endif @@ -83,9 +87,11 @@ void loop() { processTVC(State); state2vec(State, PrevState, stateVector); + // 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); @@ -93,6 +99,7 @@ void loop() { } #elif defined(TEENSY) void loop() { + last = millis(); vehicleDynamics(State, PrevState); thrustInfo(State); @@ -103,14 +110,16 @@ void loop() { State.time += State.stepSize; - if (State.z < 0.0) { - write2CSV(stateVector, State); + if ((State.z < 0.0) && (State.thrustFiring == 2)) { printSimResults(State); - init_Vehicle(State); - Serial.println("Last run duration:" + String(millis() - last + " ms")); + Serial.println("Run duration:" + String(millis() - last) + " ms"); - delay(1000); - Serial.println("Restarting Sim"); + closeFile(); + delay(20000); + + Serial.println("SUCCESS"); + Serial.println("Aborting Sim"); + teensyAbort(); } } #endif @@ -122,7 +131,7 @@ int main() { do { loop(); - } while ((State.z > 0.0)); + } while ((State.z > 0.0) || (State.thrustFiring != 2)); return 0; }