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