From cce72b9114a2475bf5afb74ecca40fb233d22c21 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 4 Nov 2021 15:56:55 -0700 Subject: [PATCH 01/13] Initial test function, still needs to be run on teensy to verify --- include/teensy.h | 34 +++++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/include/teensy.h b/include/teensy.h index 49cf3ef..32e966f 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,6 +1,8 @@ #include "Vehicle.h" #include +#include +#include void thrustInfo(struct Vehicle &); void processTVC(struct Vehicle &); @@ -79,8 +81,38 @@ void processTVC(Vehicle &State) { } void write2CSV(outVector &stateVector, Vehicle &State) { + // Serial.println("WARNING: write2CSV not implemented for TEENSY"); - Serial.println("WARNING: write2CSV not implemented for TEENSY"); + const int chipSelect = BUILTIN_SDCARD; + + 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"); + // don't do anything more: + return; + } + Serial.println("card initialized."); + + // Delete the file so it can be created again at the begining of the loop + if (SD.exists("simOut.csv")) { + SD.remove("simOut.csv"); + } + + // Open simOut.csv + File dataFile = SD.open("simOut.csv"); + if (dataFile) { + Serial.println("File successfully opened!"); + dataFile.println("It works!"); + + // close the file: + dataFile.close(); + + } else { + // if the file didn't open, print an error: + Serial.println("Error opening simOut.csv"); + } } void printSimResults(Vehicle &State) { From 05663a5ac76a422d9ac16a745623a5864eda84b0 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 4 Nov 2021 17:41:20 -0700 Subject: [PATCH 02/13] fixed file open error, it works! --- .vscode/extensions.json | 8 +++++--- include/teensy.h | 2 +- src/main.cpp | 4 +++- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 78f8c5e..935f093 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,8 +1,10 @@ { + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format "recommendations": [ "ms-vscode.cpptools", - "wayou.vscode-todo-highlight", + "platformio.platformio-ide", "usernamehw.errorlens", - "platformio.platformio-ide" + "wayou.vscode-todo-highlight" ] -} \ No newline at end of file +} diff --git a/include/teensy.h b/include/teensy.h index 32e966f..fd68bc1 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -101,7 +101,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) { } // Open simOut.csv - File dataFile = SD.open("simOut.csv"); + File dataFile = SD.open("simOut.csv", FILE_WRITE); if (dataFile) { Serial.println("File successfully opened!"); dataFile.println("It works!"); diff --git a/src/main.cpp b/src/main.cpp index 0c71335..2011f3e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -86,7 +86,9 @@ void loop() { init_Vehicle(State); Serial.println("Last run duration:" + String(millis() - last + " ms")); - delay(1000); + Serial.println("Sim Complete!"); + Serial.readString(); + Serial.println("Restarting Sim"); } } From c06693e6aac9cec06a94286944211b63fbca51ab Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 4 Nov 2021 17:51:52 -0700 Subject: [PATCH 03/13] added back delay at the end of the sim --- src/main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 2011f3e..5c374d5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -86,8 +86,7 @@ void loop() { init_Vehicle(State); Serial.println("Last run duration:" + String(millis() - last + " ms")); - Serial.println("Sim Complete!"); - Serial.readString(); + delay(1000); Serial.println("Restarting Sim"); } From a305b9599eeaaddbf44f3f23a37a3618dde581ed Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 4 Nov 2021 21:24:55 -0700 Subject: [PATCH 04/13] Now printing entire sim to simOut.csv on SD card --- include/native.h | 4 ++ include/sim.h | 3 -- include/teensy.h | 130 ++++++++++++++++++++++++++++++++++++----------- src/main.cpp | 18 +++++-- 4 files changed, 119 insertions(+), 36 deletions(-) diff --git a/include/native.h b/include/native.h index 1e69dce..9031117 100644 --- a/include/native.h +++ b/include/native.h @@ -117,6 +117,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/sim.h b/include/sim.h index 0904153..0045eff 100644 --- a/include/sim.h +++ b/include/sim.h @@ -235,9 +235,6 @@ 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) { diff --git a/include/teensy.h b/include/teensy.h index fd68bc1..f49d3fa 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -4,12 +4,16 @@ #include #include +double loadCellCalibrate(); +void initFile(); void thrustInfo(struct Vehicle &); void processTVC(struct Vehicle &); -void write2CSV(struct outVector &, struct Vehicle &); +void write2CSV(struct Vehicle &); void printSimResults(struct Vehicle &); +void teensyAbort(); -double loadCellCalibrate(); +const int chipSelect = BUILTIN_SDCARD; +File dataFile; double loadCellCalibrate() { // place code to calibrate load cells in here @@ -21,6 +25,38 @@ double loadCellCalibrate() { return loadTotal / 10; } +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."); + + // Delete any previous existing files + if (SD.exists("simOut.csv")) { + SD.remove("simOut.csv"); + } + + // 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 simOut.csv. \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"); @@ -80,42 +116,67 @@ void processTVC(Vehicle &State) { State.momentZ = 0; } -void write2CSV(outVector &stateVector, Vehicle &State) { - // Serial.println("WARNING: write2CSV not implemented for TEENSY"); +void write2CSV(Vehicle &State) { + dataFile.print(State.time); + dataFile.print(","); - const int chipSelect = BUILTIN_SDCARD; + dataFile.print(State.x); + dataFile.print(","); + dataFile.print(State.y); + dataFile.print(","); + dataFile.print(State.z); + dataFile.print(","); - Serial.print("Initializing SD card..."); + dataFile.print(State.vx); + dataFile.print(","); + dataFile.print(State.vy); + dataFile.print(","); + dataFile.print(State.vz); + dataFile.print(","); - // see if the card is present and can be initialized: - if (!SD.begin(chipSelect)) { - Serial.println("Card failed, or not present"); - // don't do anything more: - return; - } - Serial.println("card initialized."); + dataFile.print(State.ax); + dataFile.print(","); + dataFile.print(State.ay); + dataFile.print(","); + dataFile.print(State.az); + dataFile.print(","); - // Delete the file so it can be created again at the begining of the loop - if (SD.exists("simOut.csv")) { - SD.remove("simOut.csv"); - } + dataFile.print(State.yaw * 180 / M_PI); + dataFile.print(","); + dataFile.print(State.pitch * 180 / M_PI); + dataFile.print(","); + dataFile.print(State.roll * 180 / M_PI); + dataFile.print(","); - // Open simOut.csv - File dataFile = SD.open("simOut.csv", FILE_WRITE); - if (dataFile) { - Serial.println("File successfully opened!"); - dataFile.println("It works!"); + dataFile.print(State.yawdot * 180 / M_PI); + dataFile.print(","); + dataFile.print(State.pitchdot * 180 / M_PI); + dataFile.print(","); + dataFile.print(State.rolldot * 180 / M_PI); + dataFile.print(","); - // close the file: - dataFile.close(); + dataFile.print(State.xServoDegs); + dataFile.print(","); + dataFile.print(State.yServoDegs); + dataFile.print(","); - } else { - // if the file didn't open, print an error: - Serial.println("Error opening simOut.csv"); - } + dataFile.print(State.thrustFiring); + dataFile.print(","); + + dataFile.print(State.PIDx); + dataFile.print(","); + dataFile.print(State.PIDy); + dataFile.print(","); + + dataFile.print(State.thrust); + dataFile.print("\n"); } 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); @@ -138,5 +199,16 @@ 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) { + } } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 5c374d5..513c844 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -48,6 +48,8 @@ void setup() { loadCellCalibrate(); Serial.println("Load Cells Calibrated"); delay(1000); + initFile(); + delay(1000); } #endif @@ -60,6 +62,9 @@ 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) { @@ -70,25 +75,30 @@ void loop() { } #elif defined(TEENSY) void loop() { + last = millis(); vehicleDynamics(State, PrevState); thrustInfo(State); pidController(State, PrevState); TVC(State, PrevState); processTVC(State); - // state2vec(State, PrevState, stateVector); + write2CSV(State); + + // Set "prev" values for next timestep + PrevState = State; State.time += State.stepSize; if (State.z < 0.0) { - write2CSV(stateVector, State); printSimResults(State); init_Vehicle(State); Serial.println("Last run duration:" + String(millis() - last + " ms")); - - delay(1000); + + closeFile(); + delay(10000); Serial.println("Restarting Sim"); + Serial.println("==============================================================="); } } #endif From 897b5c1d7d8f51a0b779c19edf68703df1b94467 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 4 Nov 2021 21:56:39 -0700 Subject: [PATCH 05/13] outvector struct now only created when building native --- src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index 513c844..e7f5cf9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,13 +19,13 @@ unsigned long last; #if defined(NATIVE) || defined(_WIN32) #include "native.h" +outVector stateVector; #elif defined(TEENSY) #include "teensy.h" #endif Vehicle State; Vehicle PrevState; -outVector stateVector; #if defined(NATIVE) || defined(_WIN32) void setup() { From 48c1059c0a6902384c82848f245a9ce0eff711be Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Fri, 5 Nov 2021 06:04:30 -0700 Subject: [PATCH 06/13] fixed outVector size and removed teensy --- include/outVector.h | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) 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); From 9bce55f7af9af43f08bf88d0f9a7334fea8df749 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Fri, 5 Nov 2021 13:58:27 -0700 Subject: [PATCH 07/13] fixed time duration display --- src/main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index e7f5cf9..b5d289f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -92,13 +92,15 @@ void loop() { if (State.z < 0.0) { printSimResults(State); init_Vehicle(State); - Serial.println("Last run duration:" + String(millis() - last + " ms")); - + delay(1000); + Serial.println("Last run duration:" + String(millis() - last) + " ms"); + closeFile(); delay(10000); Serial.println("Restarting Sim"); - Serial.println("==============================================================="); + Serial.println( + "==============================================================="); } } #endif From cce451f069227a6b73e9626e7a618eb3a9d4f197 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 8 Nov 2021 11:19:06 -0700 Subject: [PATCH 08/13] its angry --- .vscode/settings.json | 3 ++- include/teensy.h | 46 ++++++++++++++++++++++++++++++++----------- 2 files changed, 36 insertions(+), 13 deletions(-) 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/teensy.h b/include/teensy.h index f49d3fa..ab0adfa 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,5 +1,4 @@ #include "Vehicle.h" - #include #include #include @@ -35,20 +34,43 @@ void initFile() { } Serial.println("Card initialized."); - // Delete any previous existing files + int i = 1; + Serial.print("simOut_" + String(i) + ".csv"); + String fileName; + if (SD.exists("simOut.csv")) { - SD.remove("simOut.csv"); - } + while (i > 0) { + fileName = "simOut_" + String(i) + ".csv"; - // Open simOut.csv - dataFile = SD.open("simOut.csv", FILE_WRITE); - if (dataFile) { - Serial.println("File successfully opened!"); + 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; + + } else { + i++; + } + } } else { - // if the file didn't open, print an error: - Serial.println("Error opening simOut.csv. \n\nABORTING SIMULATION"); - teensyAbort(); + // 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 @@ -176,7 +198,7 @@ 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); From 4f44788038c0c6ccd13ee87a1ccea2407f781c51 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Mon, 8 Nov 2021 13:31:49 -0700 Subject: [PATCH 09/13] no longer angry --- include/teensy.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/teensy.h b/include/teensy.h index ab0adfa..283e721 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -36,11 +36,11 @@ void initFile() { int i = 1; Serial.print("simOut_" + String(i) + ".csv"); - String fileName; + const char *fileName; if (SD.exists("simOut.csv")) { while (i > 0) { - fileName = "simOut_" + String(i) + ".csv"; + fileName = ("simOut_" + String(i) + ".csv").c_str(); if (!SD.exists(fileName)) { // Open simOut_i.csv From 7cffb33ff19c6f5fc8b5e049d1f7f8aa87e90656 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Mon, 8 Nov 2021 13:54:12 -0700 Subject: [PATCH 10/13] donezo --- include/teensy.h | 4 ++++ src/main.cpp | 14 +++++++------- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/include/teensy.h b/include/teensy.h index 283e721..027b542 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -232,5 +232,9 @@ void closeFile() { 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/src/main.cpp b/src/main.cpp index d1a400e..71d76d0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,6 +11,7 @@ #elif defined(TEENSY) #include +int BUILTIN_LED = 13; unsigned long last; #endif @@ -36,6 +37,8 @@ void setup() { } #elif defined(TEENSY) void setup() { + pinMode(BUILTIN_LED, OUTPUT); + digitalWrite(BUILTIN_LED, HIGH); delay(1000); init_Vehicle(State); Serial.println("Simulated Vehicle Initalized"); @@ -91,16 +94,13 @@ void loop() { if (State.z < 0.0) { printSimResults(State); - init_Vehicle(State); - delay(1000); - Serial.println("Last run duration:" + String(millis() - last) + " ms"); + Serial.println("Run duration:" + String(millis() - last) + " ms"); closeFile(); - delay(10000); - Serial.println("Restarting Sim"); - Serial.println( - "==============================================================="); + Serial.println("SUCCESS"); + Serial.println("Aborting Sim"); + teensyAbort(); } } #endif From 87fb8f8621ab12e58f1f77883dbb8ffc1d6ef718 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 8 Nov 2021 14:43:52 -0700 Subject: [PATCH 11/13] 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(); From f0577a8fce3f8fe6425a3fd81fb244cb5572bc6c Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Wed, 10 Nov 2021 14:42:47 -0700 Subject: [PATCH 12/13] outfile now has more precision --- include/teensy.h | 46 +++++++++++++++++++++++----------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/include/teensy.h b/include/teensy.h index e414339..c02166e 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -98,7 +98,7 @@ void thrustInfo(Vehicle &State) { } else { State.burnElapsed = 2000; // arbitrary number to ensure we don't burn } - //Serial.println(abs(State.burnVelocity + State.vz)); + // Serial.println(abs(State.burnVelocity + State.vz)); if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) { State.thrustFiring = true; @@ -138,58 +138,58 @@ void processTVC(Vehicle &State) { } void write2CSV(Vehicle &State) { - dataFile.print(State.time); + dataFile.print(String(State.time, 5)); dataFile.print(","); - dataFile.print(State.x); + dataFile.print(String(State.x, 5)); dataFile.print(","); - dataFile.print(State.y); + dataFile.print(String(State.y, 5)); dataFile.print(","); - dataFile.print(State.z); + dataFile.print(String(State.z, 5)); dataFile.print(","); - dataFile.print(State.vx); + dataFile.print(String(State.vx, 5)); dataFile.print(","); - dataFile.print(State.vy); + dataFile.print(String(State.vy, 5)); dataFile.print(","); - dataFile.print(State.vz); + dataFile.print(String(State.vz, 5)); dataFile.print(","); - dataFile.print(State.ax); + dataFile.print(String(State.ax, 5)); dataFile.print(","); - dataFile.print(State.ay); + dataFile.print(String(State.ay, 5)); dataFile.print(","); - dataFile.print(State.az); + dataFile.print(String(State.az, 5)); dataFile.print(","); - dataFile.print(State.yaw * 180.0 / M_PI); + dataFile.print(String(State.yaw * 180.0 / M_PI, 5)); dataFile.print(","); - dataFile.print(State.pitch * 180.0 / M_PI); + dataFile.print(String(State.pitch * 180.0 / M_PI, 5)); dataFile.print(","); - dataFile.print(State.roll * 180.0 / M_PI); + dataFile.print(String(State.roll * 180.0 / M_PI, 5)); dataFile.print(","); - dataFile.print(State.yawdot * 180.0 / M_PI); + dataFile.print(String(State.yawdot * 180.0 / M_PI, 5)); dataFile.print(","); - dataFile.print(State.pitchdot * 180.0 / M_PI); + dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5)); dataFile.print(","); - dataFile.print(State.rolldot * 180.0 / M_PI); + dataFile.print(String(State.rolldot * 180.0 / M_PI, 5)); dataFile.print(","); - dataFile.print(State.xServoDegs); + dataFile.print(String(State.xServoDegs, 5)); dataFile.print(","); - dataFile.print(State.yServoDegs); + dataFile.print(String(State.yServoDegs, 5)); dataFile.print(","); - dataFile.print(State.thrustFiring); + dataFile.print(String(State.thrustFiring, 5)); dataFile.print(","); - dataFile.print(State.PIDx); + dataFile.print(String(State.PIDx, 5)); dataFile.print(","); - dataFile.print(State.PIDy); + dataFile.print(String(State.PIDy, 5)); dataFile.print(","); - dataFile.print(State.thrust); + dataFile.print(String(State.thrust, 5)); dataFile.print("\n"); } From 0058af966740e4eb728a0737f2209cee05d84068 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 11 Nov 2021 21:58:57 -0700 Subject: [PATCH 13/13] 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; }