diff --git a/include/teensy.h b/include/teensy.h index b2056b9..07402d8 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -7,7 +7,7 @@ double loadCellCalibrate(); void initFile(); void thrustInfo(struct Vehicle &); void processTVC(struct Vehicle &); -void write2CSV(struct Vehicle &); +void write2CSV(struct Vehicle &, double a, double b, double c, double d); void printSimResults(struct Vehicle &); void teensyAbort(); @@ -75,7 +75,7 @@ void initFile() { // File Header dataFile.println( "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot," - "Servo1,Servo2,thrustFiring,thrust,simResponse"); + "Servo1,Servo2,thrustFiring,thrust,simResponse,l0,l1,l2,l3"); } void thrustInfo(Vehicle &State) { @@ -122,7 +122,7 @@ void processTVC(Vehicle &State, LoadCells &loadCells) { State.momentZ = 0.0; } -void write2CSV(Vehicle &State) { +void write2CSV(Vehicle &State, double a, double b, double c, double d) { dataFile.print(String(State.time, 5)); dataFile.print(","); @@ -172,8 +172,17 @@ void write2CSV(Vehicle &State) { dataFile.print(","); dataFile.print(String(State.stepDuration, 5)); - dataFile.print("\n"); + dataFile.print(","); + dataFile.print(String(a, 5)); + dataFile.print(","); + dataFile.print(String(b, 5)); + dataFile.print(","); + dataFile.print(String(c, 5)); + dataFile.print(","); + dataFile.print(String(d, 5)); + + dataFile.print("\n"); } void printSimResults(Vehicle &State) { diff --git a/src/main.cpp b/src/main.cpp index 4382951..d7d3459 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -29,10 +29,10 @@ LoadCells loadCells; #include "teensy.h" const int lc_clock = 23; -const int lc_data_0 = 0; -const int lc_data_1 = 1; -const int lc_data_2 = 2; -const int lc_data_3 = 3; +const int lc_data_0 = 14; +const int lc_data_1 = 15; +const int lc_data_2 = 19; +const int lc_data_3 = 20; #endif @@ -107,17 +107,17 @@ void loop() { thrustInfo(State); pidController(State, PrevState); TVC(State, PrevState); - + processTVC(State, loadCells); State.stepDuration = micros() - last; - write2CSV(State); + write2CSV(State, loadCells.lc0Value, loadCells.lc1Value, loadCells.lc2Value, + loadCells.lc3Value); // Set "prev" values for next timestep PrevState = State; - - // state2vec(State, PrevState, stateVector); + // state2vec(State, PrevState, stateVector); State.time += State.stepSize;