Merge branch '36-move-init_loadcells-to-teensy-h' into 'main'

Resolve "Move init_loadCells() to teensy.h"

Closes #36

See merge request lander-team/lander-cpp!28

include/Vehicle.h

@@ -22,7 +22,7 @@ struct Vehicle {
   double thrust, burnElapsed, burnStart;
   int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
 
-  double PIDx, PIDy, Fx, Fy, Fz;
+  double PIDx, PIDy, Fx, Fy, Fz, F;
   double momentX, momentY, momentZ;
 
   double I11, I22, I33;
@@ -41,8 +41,12 @@ struct Vehicle {
 
   double simTime;
   double stepSize;
+  double stepDuration;
 
   double time = 0.0;
+
+  double lc0, lc1, lc2, lc3 = 0.0;
+  double lc0_processed, lc1_processed, lc2_processed, lc3_processed = 0.0;
 };
 
 void init_Vehicle(Vehicle &State) {
@@ -77,7 +81,7 @@ void init_Vehicle(Vehicle &State) {
   State.momentArm = 0.145;       // [m]
 
   // Sim Step Size
-  State.stepSize = 1.0; // [ms]
+  State.stepSize = 5.0; // [ms]
 
   // Other Properties
   State.massPropellant = 0.06;                                  // [kg]

include/native.h

@@ -117,10 +117,10 @@ void printSimResults(Vehicle &State) {
   std::cout << "Final Angles:   [" << State.yaw << ", " << State.pitch << "]"
             << std::endl;
 
-  if (landing_velocity < 0.5) {
-    std::cout << "Landing Velocity  < 0.5 m/s      | PASS | ";
+  if (landing_velocity < 1.0) {
+    std::cout << "Landing Velocity  < 1.0 m/s      | PASS | ";
   } else {
-    std::cout << "Landing Velocity  < 0.5 m/s      | FAIL | ";
+    std::cout << "Landing Velocity  < 1.0 m/s      | FAIL | ";
   }
   std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
             << State.vz << "]" << std::endl;

include/teensy.h

@@ -1,27 +1,137 @@
 #include "Vehicle.h"
 #include <Arduino.h>
+#include <HX711.h>
+#include <PWMServo.h>
 #include <SD.h>
 #include <SPI.h>
 
-double loadCellCalibrate();
+void testGimbal(class PWMServo &, class PWMServo &);
+void initLoadCells(struct Vehicle &);
 void initFile();
 void thrustInfo(struct Vehicle &);
-void processTVC(struct Vehicle &);
+void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
 void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
+double loadCellFilter(double current, double previous);
 
+// Load cell stuff
+HX711 lc0;
+HX711 lc1;
+HX711 lc2;
+HX711 lc3;
+
+void read_lc0();
+void read_lc1();
+void read_lc2();
+void read_lc3();
+
+const int lc_clock = 23;
+const int pin_lc0 = 14;
+const int pin_lc1 = 15;
+const int pin_lc2 = 19;
+const int pin_lc3 = 20;
+const int pin_igniter = 7;
+
+double lcGain0 = -98.32;
+double lcGain1 = -97.59;
+double lcGain2 = -100.51;
+double lcGain3 = -118.65;
+
+// SD card stuff
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
-double loadCellCalibrate() {
-  // place code to calibrate load cells in here
-  double loadTotal;
-  for (double t = 0.0; t == 10.0; ++t) {
-    loadTotal += 1.0;
-    delay(15);
+void testGimbal(PWMServo &servo1, PWMServo &servo2) {
+  int servoTest = 90;
+
+  servo1.write(servoTest);
+  servo2.write(servoTest);
+
+  // Servo 1 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(90 + servoTest);
+    delay(30);
   }
-  return loadTotal / 10.0;
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(90 + servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 1 & 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(90 + servoTest);
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(90 + servoTest);
+    servo2.write(90 + servoTest);
+    delay(30);
+  }
+
+  delay(30);
+  // BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
+  servo1.write(90);
+  servo2.write(90);
+}
+
+void initLoadCells(Vehicle &State) {
+  // 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(pin_lc0, INPUT);
+  pinMode(pin_lc1, INPUT);
+  pinMode(pin_lc2, INPUT);
+  pinMode(pin_lc3, INPUT);
+  lc0.begin(pin_lc0, lc_clock);
+  lc1.begin(pin_lc1, lc_clock);
+  lc2.begin(pin_lc2, lc_clock);
+  lc3.begin(pin_lc3, lc_clock);
+
+  Serial.print("Calibrating");
+  lc0.set_scale();
+  lc1.set_scale();
+  lc2.set_scale();
+  lc3.set_scale();
+
+  lc0.tare(50);
+  Serial.print(".");
+  lc1.tare(50);
+  Serial.print(".");
+  lc2.tare(50);
+  Serial.println(".");
+  lc3.tare(50);
+
+  // Attach ISRs to load cell data pins to read data when available
+  attachInterrupt(digitalPinToInterrupt(pin_lc0), read_lc0, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc1), read_lc1, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
+
+  Serial.println("Load Cells Initialized");
+
+  // Initializes State variables
+  State.lc0 = 9.81 * 0.001 * lc0.get_value() / lcGain0;
+  State.lc1 = 9.81 * 0.001 * lc1.get_value() / lcGain1;
+  State.lc2 = 9.81 * 0.001 * lc2.get_value() / lcGain2;
+  State.lc3 = 9.81 * 0.001 * lc3.get_value() / lcGain3;
 }
 
 void initFile() {
@@ -75,21 +185,20 @@ void initFile() {
   // File Header
   dataFile.println(
       "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
-      "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust");
+      "Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3,lc0_"
+      "processed,lc1_processed,"
+      "lc2_processed,lc3_processed");
 }
 
 void thrustInfo(Vehicle &State) {
-  if (State.time == 0) {
-    Serial.println("WARNING: thrustInfo not implemented for TEENSY");
-  }
-
   if ((abs(State.burnVelocity + State.vz) < 1.03) &&
       (State.thrustFiring == 0)) {
     // Start burn
     State.burnStart = State.time;
     State.burnElapsed = 0.0;
-    State.thrustFiring = 1;
 
+    analogWrite(pin_igniter, 256);
+    State.thrustFiring = 1;
     getThrust(State);
 
   } else if (State.thrustFiring == 1) {
@@ -98,17 +207,25 @@ void thrustInfo(Vehicle &State) {
 
     getThrust(State);
 
+    double r = 3.0;
+    double R = 5.0;
+    // Vector math to aqcuire thrust vector components
+    // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
+    // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
+    State.thrust = State.lc0_processed + State.lc1_processed +
+                   State.lc2_processed + State.lc3_processed;
+    State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
+    State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
+    State.Fz =
+        sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
+        (state.mass * g);
+
   } else {
     State.thrust = 0.0;
   }
 }
 
-void processTVC(Vehicle &State) {
-  if (State.time == 0.0) {
-    Serial.println("WARNING: processTVC not implemented for TEENSY");
-  }
-
-  // Vector math to aqcuire thrust vector components
+void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
   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)) +
@@ -118,6 +235,11 @@ void processTVC(Vehicle &State) {
   State.momentX = State.Fx * State.momentArm;
   State.momentY = State.Fy * State.momentArm;
   State.momentZ = 0.0;
+
+  State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
+
+  servo1.write(90 + State.xServoDegs);
+  servo2.write(90 + State.yServoDegs);
 }
 
 void write2CSV(Vehicle &State) {
@@ -166,13 +288,29 @@ void write2CSV(Vehicle &State) {
 
   dataFile.print(String(State.thrustFiring, 5));
   dataFile.print(",");
-
-  dataFile.print(String(State.PIDx, 5));
-  dataFile.print(",");
-  dataFile.print(String(State.PIDy, 5));
-  dataFile.print(",");
-
   dataFile.print(String(State.thrust, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.stepDuration, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.lc0, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc1, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc2, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc3, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.lc0_processed, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc1_processed, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc2_processed, 5));
+  dataFile.print(",");
+  dataFile.print(String(State.lc3_processed, 5));
+
   dataFile.print("\n");
 }
 
@@ -195,10 +333,10 @@ void printSimResults(Vehicle &State) {
   Serial.print("Final Angles:   [" + String(State.yaw) + ", " +
                String(State.pitch) + "]\n");
 
-  if (landing_velocity < 0.5) {
-    Serial.print("Landing Velocity  < 0.5 m/s      | PASS | ");
+  if (landing_velocity < 1.0) {
+    Serial.print("Landing Velocity  < 1.0 m/s      | PASS | ");
   } else {
-    Serial.print("Landing Velocity  < 0.5 m/s      | FAIL | ");
+    Serial.print("Landing Velocity  < 1.0 m/s      | FAIL | ");
   }
   Serial.print("Final Velocity: [" + String(State.vx) + ", " +
                String(State.vy) + ", " + String(State.vz) + "]\n");
@@ -220,3 +358,41 @@ void teensyAbort() {
     delay(1000);
   }
 }
+
+// ISRs to print data to serial monitor
+void read_lc0() {
+  State.lc0 = lc0.get_value();
+  State.lc0_processed = 0.00981 * State.lc0 / lcGain0;
+  State.lc0_processed =
+      loadCellFilter(State.lc0_processed, PrevState.lc0_processed);
+  PrevState.lc0_processed = State.lc0_processed;
+}
+void read_lc1() {
+  State.lc1 = lc1.get_value();
+  State.lc1_processed = 0.00981 * State.lc1 / lcGain1;
+  State.lc1_processed =
+      loadCellFilter(State.lc1_processed, PrevState.lc1_processed);
+  PrevState.lc1_processed = State.lc1_processed;
+}
+void read_lc2() {
+  State.lc2 = lc2.get_value();
+  State.lc2_processed = 0.00981 * State.lc2 / lcGain2;
+  State.lc2_processed =
+      loadCellFilter(State.lc2_processed, PrevState.lc2_processed);
+  PrevState.lc2_processed = State.lc2_processed;
+}
+void read_lc3() {
+  State.lc3 = lc3.get_value();
+  State.lc3_processed = 0.00981 * State.lc3 / lcGain3;
+  State.lc3_processed =
+      loadCellFilter(State.lc3_processed, PrevState.lc3_processed);
+  PrevState.lc3_processed = State.lc3_processed;
+}
+
+double loadCellFilter(double current, double previous) {
+  if (abs(current - previous > 60.0)) {
+    return previous;
+  } else {
+    return current;
+  }
+}

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

src/main.cpp

@@ -1,6 +1,4 @@
-#define M_PI 3.14159265359
-
-#if defined(NATIVE) || defined(_WIN32)
+#if defined(_WIN32) || defined(NATIVE)
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -10,106 +8,130 @@
 #include <vector>
 #elif defined(TEENSY)
 #include <Arduino.h>
-
-int BUILTIN_LED = 13;
-unsigned long last;
 #endif
 
+#define M_PI 3.14159265359
 #include "Vehicle.h"
 #include "sim.h"
 
-#if defined(NATIVE) || defined(_WIN32)
-#include "native.h"
-outVector stateVector;
-#elif defined(TEENSY)
-#include "teensy.h"
-#endif
-
 Vehicle State;
 Vehicle PrevState;
 
-#if defined(NATIVE) || defined(_WIN32)
-void setup() {
-  init_Vehicle(State);
+#if defined(TEENSY)
+
+int BUILTIN_LED = 13;
+unsigned long last, initTime;
+
+#include "teensy.h"
+#include <HX711.h>
+#include <PWMServo.h>
+
+PWMServo servo1;
+PWMServo servo2;
+
+const int pin_servo1 = 33;
+const int pin_servo2 = 29;
+const int pin_igniter = 7;
+
+int servoPos = 90; // variable to store the servo position;
 
-  // Determine when to burn
-  burnStartTimeCalc(State);
-}
-#elif defined(TEENSY)
 void setup() {
+  Serial.begin(9600);
+
   delay(5000);
+  Serial.println("Simulation Countdown:");
+  for (int i = 0; i < 10; i++) {
+    Serial.println(10 - i);
+    delay(1000);
+  }
+
+  initLoadCells(State);
   init_Vehicle(State);
+
+  servo1.attach(pin_servo1);
+  servo2.attach(pin_servo2);
+  testGimbal(servo1, servo2);
+  Serial.println("Servos Tested");
+
+  delay(3000);
   Serial.println("Simulated Vehicle Initalized");
-  delay(1000);
 
   // Determine when to burn
+  analogWrite(pin_igniter, 0);
   burnStartTimeCalc(State);
   Serial.println("Starting Height Calculated");
-  delay(1000);
-  loadCellCalibrate();
-  Serial.println("Load Cells Calibrated");
-  delay(1000);
+
   initFile();
-  delay(1000);
-}
-#endif
 
-#if defined(NATIVE) || defined(_WIN32)
+  // PLACE BUTTON HERE---------------------------------------------
+
+  initTime = micros();
+}
+
 void loop() {
+  last = micros();
+
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-  processTVC(State);
-  state2vec(State, PrevState, stateVector);
+  processTVC(State, servo1, servo2);
 
-  // Set "prev" values for next timestep
-  PrevState = State;
-  State.time += State.stepSize;
-
-  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
-    write2CSV(stateVector, State);
-    printSimResults(State);
-    init_Vehicle(State);
-  }
-}
-#elif defined(TEENSY)
-void loop() {
-
-  last = millis();
-  vehicleDynamics(State, PrevState);
-  thrustInfo(State);
-  pidController(State, PrevState);
-  TVC(State, PrevState);
-  processTVC(State);
   write2CSV(State);
 
-  // Set "prev" values for next timestep
+  // Set previous values for next timestep
   PrevState = State;
-
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+    analogWrite(pin_igniter, 0);
+    Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
+                   " seconds");
     printSimResults(State);
-    Serial.println("Run duration:" + String(millis() - last) + " ms");
 
     closeFile();
-    delay(20000);
+    delay(3000);
 
     Serial.println("SUCCESS");
-    Serial.println("Aborting Sim");
+    Serial.println("Exiting Sim");
     teensyAbort();
   }
+
+  State.stepDuration = micros() - last;
+  delayMicroseconds((1000.0 * State.stepSize) - State.stepDuration);
 }
+
 #endif
 
 #if defined(_WIN32) || defined(NATIVE)
+
+#include "native.h"
+outVector stateVector;
+
 int main() {
 
-  setup();
+  init_Vehicle(State);
+
+  // Determine when to burn
+  burnStartTimeCalc(State);
 
   do {
-    loop();
+    vehicleDynamics(State, PrevState);
+    thrustInfo(State);
+    pidController(State, PrevState);
+    TVC(State, PrevState);
+    processTVC(State);
+    state2vec(State, PrevState, stateVector);
+
+    // Set "prev" values for next timestep
+    PrevState = State;
+    State.time += State.stepSize;
+
+    if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+      write2CSV(stateVector, State);
+      printSimResults(State);
+      init_Vehicle(State);
+    }
   } while ((State.z > 0.0) || (State.thrustFiring != 2));
 
   return 0;