diff --git a/include/LoadCells.h b/include/LoadCells.h new file mode 100644 index 0000000..7aefd5d --- /dev/null +++ b/include/LoadCells.h @@ -0,0 +1,37 @@ + +#include + +struct LoadCells { + HX711 loadcell_0; + HX711 loadcell_1; + HX711 loadcell_2; + HX711 loadcell_3; + + double lc0Value; + double lc1Value; + double lc2Value; + double lc3Value; + + double lc0Calibration; + double lc1Calibration; + double lc2Calibration; + double lc3Calibration; +}; + +double loadCellCalibrate(HX711 loadCell) { + // place code to calibrate load cells in here + double loadTotal = 0.0; + for (int t = 0; t == 10; ++t) { + loadTotal += loadCell.read(); + delay(15); + } + return loadTotal / 10.0; +} + +void init_LoadCells(LoadCells &loadCells) { + + loadCells.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0); + loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1); + loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2); + loadCells.lc3Calibration = loadCellCalibrate(loadCells.loadcell_3); +} \ No newline at end of file diff --git a/include/teensy.h b/include/teensy.h index 62d7076..c069d64 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -16,7 +16,7 @@ File dataFile; double loadCellCalibrate() { // place code to calibrate load cells in here - double loadTotal; + double loadTotal = 0.0; for (double t = 0.0; t == 10.0; ++t) { loadTotal += 1.0; delay(15); @@ -103,14 +103,16 @@ void thrustInfo(Vehicle &State) { } } -void processTVC(Vehicle &State) { - if (State.time == 0.0) { +void processTVC(Vehicle &State, LoadCells &loadCells) { + if (State.time == 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.0)); - State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0)); + // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER + // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER + State.Fx = loadCells.lc1Value + loadCells.lc2Value; + State.Fy = loadCells.lc0Value + loadCells.lc3Value; State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) + (State.mass * g); diff --git a/platformio.ini b/platformio.ini index dcd71dc..a4fe110 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,7 +8,6 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html - [env:NATIVE] platform = native build_flags = -std=c++11 -Wall -O1 -D NATIVE @@ -18,3 +17,4 @@ platform = teensy board = teensy41 framework = arduino build_flags = -std=c++11 -Wall -O1 -D TEENSY +lib_deps = bogde/HX711@^0.7.4 diff --git a/src/main.cpp b/src/main.cpp index 21d20d5..17ff880 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,7 +22,18 @@ unsigned long last; #include "native.h" outVector stateVector; #elif defined(TEENSY) + +#include "LoadCells.h" +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; + #endif Vehicle State; @@ -37,7 +48,18 @@ void setup() { } #elif defined(TEENSY) void setup() { - delay(5000); + Serial.begin(9600); + + // Configure clock pin with high impedance to protect pin (if this doesn't + // work, change to OUTPUT) + pinMode(lc_clock, INPUT); + // Configure load cell data pins as inputs + pinMode(lc_data_0, INPUT); + pinMode(lc_data_1, INPUT); + pinMode(lc_data_2, INPUT); + pinMode(lc_data_3, INPUT); + + delay(1000); init_Vehicle(State); Serial.println("Simulated Vehicle Initalized"); delay(1000); @@ -46,7 +68,9 @@ void setup() { burnStartTimeCalc(State); Serial.println("Starting Height Calculated"); delay(1000); - loadCellCalibrate(); + + init_LoadCells(loadCells); + Serial.println("Load Cells Calibrated"); delay(1000); initFile(); @@ -81,11 +105,8 @@ void loop() { thrustInfo(State); pidController(State, PrevState); TVC(State, PrevState); - processTVC(State); - write2CSV(State); - - // Set "prev" values for next timestep - PrevState = State; + processTVC(State, loadCells); + // state2vec(State, PrevState, stateVector); State.time += State.stepSize;