From cce72b9114a2475bf5afb74ecca40fb233d22c21 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Thu, 4 Nov 2021 15:56:55 -0700 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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