From 541d7f428109275da0ad3bdab9333743355c0172 Mon Sep 17 00:00:00 2001 From: Matthew Robinaugh Date: Mon, 8 Nov 2021 15:37:56 -0700 Subject: [PATCH 1/6] Added Struct for load cells --- .vscode/extensions.json | 18 +++++++++-------- include/LoadCells.h | 43 +++++++++++++++++++++++++++++++++++++++++ include/teensy.h | 12 +++++------- platformio.ini | 3 ++- src/main.cpp | 8 +++++++- 5 files changed, 67 insertions(+), 17 deletions(-) create mode 100644 include/LoadCells.h diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 78f8c5e..8fde7d0 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,8 +1,10 @@ -{ - "recommendations": [ - "ms-vscode.cpptools", - "wayou.vscode-todo-highlight", - "usernamehw.errorlens", - "platformio.platformio-ide" - ] -} \ No newline at end of file +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "ms-vscode.cpptools", + "platformio.platformio-ide", + "usernamehw.errorlens", + "wayou.vscode-todo-highlight" + ] +} diff --git a/include/LoadCells.h b/include/LoadCells.h new file mode 100644 index 0000000..5150db6 --- /dev/null +++ b/include/LoadCells.h @@ -0,0 +1,43 @@ +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; +}; + +void init_LoadCells(LoadCells &loadCells) { + + + 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); // Begin serial connection + pinMode(LED_BUILTIN, OUTPUT); // Configure LED pin + // 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.lc0Calibration = loadCellCalibrate(loadcell_0); + loadCells.lc1Calibration = loadCellCalibrate(loadcell_1); + loadCells.lc2Calibration = loadCellCalibrate(loadcell_2); + loadCells.lc3Calibration = loadCellCalibrate(loadcell_3); +} \ No newline at end of file diff --git a/include/teensy.h b/include/teensy.h index 49cf3ef..4f6f64e 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,22 +1,20 @@ #include "Vehicle.h" -#include - void thrustInfo(struct Vehicle &); void processTVC(struct Vehicle &); void write2CSV(struct outVector &, struct Vehicle &); void printSimResults(struct Vehicle &); -double loadCellCalibrate(); +double loadCellCalibrate(struct LoadCells &); -double loadCellCalibrate() { +double loadCellCalibrate(LoadCells &loadCells) { // place code to calibrate load cells in here - double loadTotal; + double loadTotal = 0.0; for (double t = 0; t == 10; ++t) { - loadTotal += 1; + loadTotal += loadCell.read(); delay(15); } - return loadTotal / 10; + return loadTotal / 10.0; } void thrustInfo(Vehicle &State) { diff --git a/platformio.ini b/platformio.ini index dcd71dc..529de37 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,13 +8,14 @@ ; 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 +lib_deps = bogde/HX711@^0.7.4 [env:teensy41] 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 f2ad13d..4310051 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,7 @@ #include #elif defined(TEENSY) #include +#include "HX711.h" unsigned long last; #endif @@ -21,6 +22,7 @@ unsigned long last; #include "native.h" #elif defined(TEENSY) #include "teensy.h" +#include "LoadCells.h" #endif Vehicle State; @@ -45,7 +47,11 @@ void setup() { burnStartTimeCalc(State); Serial.println("Starting Height Calculated"); delay(1000); - loadCellCalibrate(); + + LoadCells loadCells; + + init_LoadCells(loadCells); + Serial.println("Load Cells Calibrated"); delay(1000); } From 1ec624cc55abbb4911800c658af855827bcb4f22 Mon Sep 17 00:00:00 2001 From: Matthew Robinaugh Date: Wed, 10 Nov 2021 14:36:31 -0700 Subject: [PATCH 2/6] Update to load cell calibration and initialize lc --- include/LoadCells.h | 8 ++++++-- include/teensy.h | 9 +++++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/include/LoadCells.h b/include/LoadCells.h index 5150db6..abcaf2d 100644 --- a/include/LoadCells.h +++ b/include/LoadCells.h @@ -25,8 +25,7 @@ void init_LoadCells(LoadCells &loadCells) { const int lc_data_2 = 2; const int lc_data_3 = 3; - Serial.begin(9600); // Begin serial connection - pinMode(LED_BUILTIN, OUTPUT); // Configure LED pin + Serial.begin(9600); // Configure clock pin with high impedance to protect pin (if this doesn't // work, change to OUTPUT) pinMode(lc_clock, INPUT); @@ -36,6 +35,11 @@ void init_LoadCells(LoadCells &loadCells) { pinMode(lc_data_2, INPUT); pinMode(lc_data_3, INPUT); + loadcell_0.begin(lc_data_0, lc_clock); + loadcell_1.begin(lc_data_1, lc_clock); + loadcell_2.begin(lc_data_2, lc_clock); + loadcell_3.begin(lc_data_3, lc_clock); + loadCells.lc0Calibration = loadCellCalibrate(loadcell_0); loadCells.lc1Calibration = loadCellCalibrate(loadcell_1); loadCells.lc2Calibration = loadCellCalibrate(loadcell_2); diff --git a/include/teensy.h b/include/teensy.h index 4f6f64e..489c375 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -1,13 +1,14 @@ #include "Vehicle.h" +#include "LoadCells.h" void thrustInfo(struct Vehicle &); -void processTVC(struct Vehicle &); +void processTVC(struct LoadCells &); void write2CSV(struct outVector &, struct Vehicle &); void printSimResults(struct Vehicle &); -double loadCellCalibrate(struct LoadCells &); +double loadCellCalibrate(HX711 loadCell); -double loadCellCalibrate(LoadCells &loadCells) { +double loadCellCalibrate(HX711 loadCell) { // place code to calibrate load cells in here double loadTotal = 0.0; for (double t = 0; t == 10; ++t) { @@ -59,7 +60,7 @@ void thrustInfo(Vehicle &State) { } } -void processTVC(Vehicle &State) { +void processTVC(LoadCells &loadCells) { if (State.time == 0) { Serial.println("WARNING: processTVC not implemented for TEENSY"); } From 01cf0966f26185c86a338d56bbcd2555732d351f Mon Sep 17 00:00:00 2001 From: Matthew Robinaugh Date: Wed, 10 Nov 2021 14:42:57 -0700 Subject: [PATCH 3/6] Initialize load cell values --- include/LoadCells.h | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/include/LoadCells.h b/include/LoadCells.h index abcaf2d..c072ab7 100644 --- a/include/LoadCells.h +++ b/include/LoadCells.h @@ -16,7 +16,11 @@ struct LoadCells { }; 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; @@ -35,13 +39,13 @@ void init_LoadCells(LoadCells &loadCells) { pinMode(lc_data_2, INPUT); pinMode(lc_data_3, INPUT); - loadcell_0.begin(lc_data_0, lc_clock); - loadcell_1.begin(lc_data_1, lc_clock); - loadcell_2.begin(lc_data_2, lc_clock); - loadcell_3.begin(lc_data_3, lc_clock); + 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(loadcell_0); - loadCells.lc1Calibration = loadCellCalibrate(loadcell_1); - loadCells.lc2Calibration = loadCellCalibrate(loadcell_2); - loadCells.lc3Calibration = loadCellCalibrate(loadcell_3); + 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 From 4ef761921723b756fcf5f7325d92d9109e53dc6a Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Fri, 12 Nov 2021 13:35:06 -0700 Subject: [PATCH 4/6] =?UTF-8?q?compiling=20=F0=9F=98=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/LoadCells.h | 40 +++++++++++++--------------------------- include/teensy.h | 21 +++++---------------- src/main.cpp | 29 +++++++++++++++++++++++------ 3 files changed, 41 insertions(+), 49 deletions(-) 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; From 2a0426334d47e3b1d8257a14456d5be6226dc670 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Fri, 12 Nov 2021 13:38:06 -0700 Subject: [PATCH 5/6] load cells dont run on native --- platformio.ini | 1 - 1 file changed, 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index 529de37..a4fe110 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,7 +11,6 @@ [env:NATIVE] platform = native build_flags = -std=c++11 -Wall -O1 -D NATIVE -lib_deps = bogde/HX711@^0.7.4 [env:teensy41] platform = teensy From 175c5a0b4160bb5d06744a27feb498e470eaa1a7 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Fri, 12 Nov 2021 13:50:26 -0700 Subject: [PATCH 6/6] initialize loadTotal --- include/teensy.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/teensy.h b/include/teensy.h index d6c054d..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);