mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-08-02 11:31:34 +00:00
now reading from load cells each loop
This commit is contained in:
18
src/main.cpp
18
src/main.cpp
@@ -58,6 +58,17 @@ 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);
|
||||
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.println("Load Cells Calibrated");
|
||||
|
||||
delay(1000);
|
||||
init_Vehicle(State);
|
||||
@@ -69,8 +80,6 @@ void setup() {
|
||||
Serial.println("Starting Height Calculated");
|
||||
delay(1000);
|
||||
|
||||
init_LoadCells(loadCells);
|
||||
|
||||
Serial.println("Load Cells Calibrated");
|
||||
delay(1000);
|
||||
initFile();
|
||||
@@ -103,6 +112,7 @@ void loop() {
|
||||
void loop() {
|
||||
|
||||
last = micros();
|
||||
update_LoadCells(loadCells);
|
||||
vehicleDynamics(State, PrevState);
|
||||
thrustInfo(State);
|
||||
pidController(State, PrevState);
|
||||
@@ -111,8 +121,8 @@ void loop() {
|
||||
processTVC(State, loadCells);
|
||||
|
||||
State.stepDuration = micros() - last;
|
||||
write2CSV(State, loadCells.lc0Value, loadCells.lc1Value, loadCells.lc2Value,
|
||||
loadCells.lc3Value);
|
||||
write2CSV(State, loadCells.lc0Val, loadCells.lc1Val, loadCells.lc2Val,
|
||||
loadCells.lc3Val);
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
PrevState = State;
|
||||
|
Reference in New Issue
Block a user