diff --git a/include/LoadCells.h b/include/LoadCells.h index c072ab7..7aefd5d 100644 --- a/include/LoadCells.h +++ b/include/LoadCells.h @@ -1,3 +1,6 @@ + +#include + struct LoadCells { HX711 loadcell_0; HX711 loadcell_1; @@ -15,35 +18,18 @@ struct LoadCells { 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.lc0Value = 0.0; - loadCells.lc1Value = 0.0; - loadCells.lc2Value = 0.0; - loadCells.lc3Value = 0.0; - - 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; - - 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); - - loadCells.loadcell_0.begin(lc_data_0, lc_clock); - loadCells.loadcell_1.begin(lc_data_1, lc_clock); - loadCells.loadcell_2.begin(lc_data_2, lc_clock); - loadCells.loadcell_3.begin(lc_data_3, lc_clock); - loadCells.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0); loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1); loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2); diff --git a/include/teensy.h b/include/teensy.h index 489c375..e82ae79 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,23 +1,10 @@ #include "Vehicle.h" -#include "LoadCells.h" void thrustInfo(struct Vehicle &); void processTVC(struct LoadCells &); void write2CSV(struct outVector &, struct Vehicle &); void printSimResults(struct Vehicle &); -double loadCellCalibrate(HX711 loadCell); - -double loadCellCalibrate(HX711 loadCell) { - // place code to calibrate load cells in here - double loadTotal = 0.0; - for (double t = 0; t == 10; ++t) { - loadTotal += loadCell.read(); - delay(15); - } - return loadTotal / 10.0; -} - void thrustInfo(Vehicle &State) { if (State.time == 0) { Serial.println("WARNING: thrustInfo not implemented for TEENSY"); @@ -60,14 +47,16 @@ void thrustInfo(Vehicle &State) { } } -void processTVC(LoadCells &loadCells) { +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)); - State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180)); + // 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/src/main.cpp b/src/main.cpp index 4310051..22e61b6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,8 +10,6 @@ #include #elif defined(TEENSY) #include -#include "HX711.h" - unsigned long last; #endif @@ -21,8 +19,18 @@ unsigned long last; #if defined(NATIVE) || defined(_WIN32) #include "native.h" #elif defined(TEENSY) -#include "teensy.h" + #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; @@ -38,6 +46,17 @@ void setup() { } #elif defined(TEENSY) void setup() { + 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"); @@ -48,8 +67,6 @@ void setup() { Serial.println("Starting Height Calculated"); delay(1000); - LoadCells loadCells; - init_LoadCells(loadCells); Serial.println("Load Cells Calibrated"); @@ -81,7 +98,7 @@ void loop() { thrustInfo(State); pidController(State, PrevState); TVC(State, PrevState); - processTVC(State); + processTVC(State, loadCells); // state2vec(State, PrevState, stateVector); State.time += State.stepSize;