diff --git a/include/LoadCells.h b/include/LoadCells.h deleted file mode 100644 index 318f512..0000000 --- a/include/LoadCells.h +++ /dev/null @@ -1,39 +0,0 @@ - -#include -#pragma once - -struct LoadCells { - HX711 lc0; - HX711 lc1; - HX711 lc2; - HX711 lc3; - - // Current calibrated value - double lc0Val; - double lc1Val; - double lc2Val; - double lc3Val; - - // Calibration offset - double lc0Cal; - double lc1Cal; - double lc2Cal; - double lc3Cal; -}; - -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 update_LoadCells(LoadCells &loadCells) { - loadCells.lc0Val = loadCells.lc0.read() - loadCells.lc0Cal; - loadCells.lc1Val = loadCells.lc1.read() - loadCells.lc1Cal; - loadCells.lc2Val = loadCells.lc2.read() - loadCells.lc2Cal; - loadCells.lc3Val = loadCells.lc3.read() - loadCells.lc3Cal; -} \ No newline at end of file diff --git a/include/Vehicle.h b/include/Vehicle.h index 03db381..2c37635 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -44,6 +44,8 @@ struct Vehicle { double stepDuration; double time = 0.0; + + double lc0, lc1, lc2, lc3; }; void init_Vehicle(Vehicle &State) { diff --git a/include/teensy.h b/include/teensy.h index e987d8c..8e7d600 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,4 +1,3 @@ -#include "LoadCells.h" #include "Vehicle.h" #include #include @@ -8,7 +7,7 @@ double loadCellCalibrate(); void initFile(); void thrustInfo(struct Vehicle &); void processTVC(struct Vehicle &); -void write2CSV(struct Vehicle &, double a, double b, double c, double d); +void write2CSV(struct Vehicle &); void printSimResults(struct Vehicle &); void teensyAbort(); @@ -94,7 +93,7 @@ void thrustInfo(Vehicle &State) { } } -void processTVC(Vehicle &State, LoadCells &loadCells) { +void processTVC(Vehicle &State) { if (State.time == 0) { Serial.println("WARNING: processTVC not implemented for TEENSY"); } @@ -103,10 +102,14 @@ void processTVC(Vehicle &State, LoadCells &loadCells) { // Vector math to aqcuire thrust vector components // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER - State.Fx = (loadCells.lc1Val - loadCells.lc2Val) * r / R; - State.Fy = (loadCells.lc0Val - loadCells.lc3Val) * r / R; - State.Fz = - loadCells.lc0Val + loadCells.lc1Val + loadCells.lc2Val + loadCells.lc3Val; + State.Fx = (State.lc1 - State.lc2) * r / R; + State.Fy = (State.lc0 - State.lc3) * r / R; + State.Fz = State.lc0 + State.lc1 + State.lc2 + State.lc3; + + State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0)); + State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0)); + State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) + + (State.mass * g); // Calculate moment created by Fx and Fy State.momentX = State.Fx * State.momentArm; @@ -116,7 +119,7 @@ void processTVC(Vehicle &State, LoadCells &loadCells) { State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2)); } -void write2CSV(Vehicle &State, LoadCells &loadCells) { +void write2CSV(Vehicle &State) { dataFile.print(String(State.time, 5)); dataFile.print(","); @@ -168,13 +171,13 @@ void write2CSV(Vehicle &State, LoadCells &loadCells) { dataFile.print(String(State.stepDuration, 5)); dataFile.print(","); - dataFile.print(String(loadCells.lc0Val, 5)); + dataFile.print(String(State.lc0, 5)); dataFile.print(","); - dataFile.print(String(loadCells.lc1Val, 5)); + dataFile.print(String(State.lc1, 5)); dataFile.print(","); - dataFile.print(String(loadCells.lc2Val, 5)); + dataFile.print(String(State.lc2, 5)); dataFile.print(","); - dataFile.print(String(loadCells.lc3Val, 5)); + dataFile.print(String(State.lc3, 5)); dataFile.print("\n"); } diff --git a/src/main.cpp b/src/main.cpp index d4828fe..f613ae5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,10 +23,8 @@ unsigned long last, initTime; outVector stateVector; #elif defined(TEENSY) -#include "LoadCells.h" -LoadCells loadCells; - #include "teensy.h" +#include const int lc_clock = 23; const int lc_data_0 = 14; @@ -34,6 +32,16 @@ const int lc_data_1 = 15; const int lc_data_2 = 19; const int lc_data_3 = 20; +void read_lc0(); +void read_lc1(); +void read_lc2(); +void read_lc3(); + +HX711 lc0; +HX711 lc1; +HX711 lc2; +HX711 lc3; + #endif Vehicle State; @@ -58,20 +66,37 @@ void setup() { pinMode(lc_data_1, INPUT); pinMode(lc_data_2, INPUT); pinMode(lc_data_3, INPUT); - loadCells.lc0.begin(lc_data_0, lc_clock); - loadCells.lc1.begin(lc_data_1, lc_clock); - loadCells.lc2.begin(lc_data_2, lc_clock); - loadCells.lc3.begin(lc_data_3, lc_clock); + lc0.begin(lc_data_0, lc_clock); + lc1.begin(lc_data_1, lc_clock); + lc2.begin(lc_data_2, lc_clock); + lc3.begin(lc_data_3, lc_clock); + + // Attach ISRs to load cell data pins to read data when available + attachInterrupt(digitalPinToInterrupt(lc_data_0), read_lc0, LOW); + attachInterrupt(digitalPinToInterrupt(lc_data_1), read_lc1, LOW); + attachInterrupt(digitalPinToInterrupt(lc_data_2), read_lc2, LOW); + attachInterrupt(digitalPinToInterrupt(lc_data_3), read_lc3, LOW); + Serial.println("Load Cells Initialized"); - loadCells.lc0Cal = loadCellCalibrate(loadCells.lc0); - loadCells.lc1Cal = loadCellCalibrate(loadCells.lc1); - loadCells.lc2Cal = loadCellCalibrate(loadCells.lc2); - loadCells.lc3Cal = loadCellCalibrate(loadCells.lc3); + Serial.print("Calibrating"); + lc0.tare(); + Serial.print("."); + lc1.tare(); + Serial.print("."); + lc2.tare(); + Serial.print("."); + lc3.tare(); + Serial.println("."); Serial.println("Load Cells Calibrated"); delay(1000); init_Vehicle(State); + State.lc0 = lc0.get_value(); + State.lc1 = lc1.get_value(); + State.lc2 = lc2.get_value(); + State.lc3 = lc3.get_value(); + Serial.println("Simulated Vehicle Initalized"); delay(1000); @@ -80,8 +105,6 @@ void setup() { Serial.println("Starting Height Calculated"); delay(1000); - Serial.println("Load Cells Calibrated"); - delay(1000); initFile(); delay(1000); @@ -112,16 +135,17 @@ void loop() { void loop() { last = micros(); - update_LoadCells(loadCells); + Serial.println(State.time); vehicleDynamics(State, PrevState); thrustInfo(State); pidController(State, PrevState); TVC(State, PrevState); - processTVC(State, loadCells); + processTVC(State); + delay(2); State.stepDuration = micros() - last; - write2CSV(State, loadCells); + write2CSV(State); // Set "prev" values for next timestep PrevState = State; @@ -156,4 +180,13 @@ int main() { return 0; } -#endif \ No newline at end of file +#endif + +// ISRs to print data to serial monitor +void read_lc0() { State.lc0 = lc0.get_value(); } + +void read_lc1() { State.lc1 = lc1.get_value(); } + +void read_lc2() { State.lc2 = lc2.get_value(); } + +void read_lc3() { State.lc3 = lc3.get_value(); }