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/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/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..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 49cf3ef..f49d3fa 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,13 +1,19 @@ #include "Vehicle.h" #include - -void thrustInfo(struct Vehicle &); -void processTVC(struct Vehicle &); -void write2CSV(struct outVector &, struct Vehicle &); -void printSimResults(struct Vehicle &); +#include +#include double loadCellCalibrate(); +void initFile(); +void thrustInfo(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 @@ -19,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"); @@ -78,12 +116,67 @@ void processTVC(Vehicle &State) { State.momentZ = 0; } -void write2CSV(outVector &stateVector, Vehicle &State) { +void write2CSV(Vehicle &State) { + dataFile.print(State.time); + dataFile.print(","); - Serial.println("WARNING: write2CSV not implemented for TEENSY"); + dataFile.print(State.x); + dataFile.print(","); + dataFile.print(State.y); + dataFile.print(","); + dataFile.print(State.z); + dataFile.print(","); + + dataFile.print(State.vx); + dataFile.print(","); + dataFile.print(State.vy); + dataFile.print(","); + dataFile.print(State.vz); + dataFile.print(","); + + dataFile.print(State.ax); + dataFile.print(","); + dataFile.print(State.ay); + dataFile.print(","); + dataFile.print(State.az); + dataFile.print(","); + + 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(","); + + 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(","); + + dataFile.print(State.xServoDegs); + dataFile.print(","); + dataFile.print(State.yServoDegs); + dataFile.print(","); + + 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); @@ -106,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 f2ad13d..d1a400e 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() { @@ -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,24 +75,32 @@ 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); + Serial.println("Last run duration:" + String(millis() - last) + " ms"); + + closeFile(); + delay(10000); + Serial.println("Restarting Sim"); + Serial.println( + "==============================================================="); } } #endif