Merge branch '31-avionics-integration-test' into 'main'

Resolve "Avionics Integration Test"

Closes #31

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

include/LoadCells.h

@@ -1,37 +0,0 @@
-
-#include <HX711.h>
-
-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;
-};
-
-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.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0);
-  loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1);
-  loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2);
-  loadCells.lc3Calibration = loadCellCalibrate(loadCells.loadcell_3);
-}

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;
@@ -44,6 +44,8 @@ struct Vehicle {
   double stepDuration;
 
   double time = 0.0;
+
+  double lc0, lc1, lc2, lc3;
 };
 
 void init_Vehicle(Vehicle &State) {
@@ -78,7 +80,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) {
+  if (landing_velocity < 1.0) {
-    std::cout << "Landing Velocity  < 0.5 m/s      | PASS | ";
+    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,12 +1,14 @@
 #include "Vehicle.h"
 #include <Arduino.h>
+#include <PWMServo.h>
 #include <SD.h>
 #include <SPI.h>
 
+void testGimbal(class PWMServo &, class PWMServo &);
 double loadCellCalibrate();
 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();
@@ -14,14 +16,51 @@ void teensyAbort();
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
-double loadCellCalibrate() {
+void testGimbal(PWMServo &servo1, PWMServo &servo2) {
-  // place code to calibrate load cells in here
+  int servoTest = 0;
-  double loadTotal = 0.0;
+
-  for (double t = 0.0; t == 10.0; ++t) {
+  servo1.write(servoTest);
-    loadTotal += 1.0;
+  servo2.write(servoTest);
-    delay(15);
+
+  // Servo 1 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(servoTest);
+    delay(30);
   }
-  return loadTotal / 10.0;
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo2.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo2.write(servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 1 & 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(servoTest);
+    servo2.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(servoTest);
+    servo2.write(servoTest);
+    delay(30);
+  }
+
+  delay(30);
+  servo1.write(0);
+  servo2.write(0);
 }
 
 void initFile() {
@@ -75,7 +114,7 @@ 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,thrust,simResponse");
+      "Servo1,Servo2,thrustFiring,thrust,simResponse,lc0,lc1,lc2,lc3");
 }
 
 void thrustInfo(Vehicle &State) {
@@ -103,16 +142,21 @@ void thrustInfo(Vehicle &State) {
   }
 }
 
-void processTVC(Vehicle &State, LoadCells &loadCells) {
+void processTVC(Vehicle &State, PWMServo &servo1, PWMServo &servo2) {
   if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
-
+  double r = 3.0;
+  double R = 5.0;
   // Vector math to aqcuire thrust vector components
   // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
   // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
-  State.Fx = loadCells.lc1Value + loadCells.lc2Value;
+  State.Fx = (State.lc1 - State.lc2) * r / R;
-  State.Fy = loadCells.lc0Value + loadCells.lc3Value;
+  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);
 
@@ -120,6 +164,11 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   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(State.xServoDegs);
+  servo2.write(State.yServoDegs);
 }
 
 void write2CSV(Vehicle &State) {
@@ -172,8 +221,17 @@ void write2CSV(Vehicle &State) {
   dataFile.print(",");
 
   dataFile.print(String(State.stepDuration, 5));
-  dataFile.print("\n");
+  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("\n");
 }
 
 void printSimResults(Vehicle &State) {
@@ -195,10 +253,10 @@ void printSimResults(Vehicle &State) {
   Serial.print("Final Angles:   [" + String(State.yaw) + ", " +
                String(State.pitch) + "]\n");
 
-  if (landing_velocity < 0.5) {
+  if (landing_velocity < 1.0) {
-    Serial.print("Landing Velocity  < 0.5 m/s      | PASS | ");
+    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");

src/main.cpp

@@ -1,6 +1,4 @@
-#define M_PI 3.14159265359
+#if defined(_WIN32) || defined(NATIVE)
-
-#if defined(NATIVE) || defined(_WIN32)
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -10,78 +8,172 @@
 #include <vector>
 #elif defined(TEENSY)
 #include <Arduino.h>
-
-int BUILTIN_LED = 13;
-unsigned long last, initTime;
 #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 "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;
 Vehicle PrevState;
 
-#if defined(NATIVE) || defined(_WIN32)
+#if defined(TEENSY)
-void setup() {
+
-  init_Vehicle(State);
+int BUILTIN_LED = 13;
+unsigned long last, initTime;
+
+#include "teensy.h"
+#include <HX711.h>
+#include <PWMServo.h>
+
+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;
+
+void init_loadCells();
+void read_lc0();
+void read_lc1();
+void read_lc2();
+void read_lc3();
+
+HX711 lc0;
+HX711 lc1;
+HX711 lc2;
+HX711 lc3;
+
+PWMServo servo1;
+PWMServo servo2;
+
+const int pin_servo1 = 33;
+const int pin_servo2 = 29;
+
+int servoPos = 0; // variable to store the servo position;
 
-  // Determine when to burn
-  burnStartTimeCalc(State);
-}
-#elif defined(TEENSY)
 void setup() {
   Serial.begin(9600);
 
-  // Configure clock pin with high impedance to protect pin (if this doesn't
+  delay(5000);
-  // work, change to OUTPUT)
+  Serial.println("Simulation Countdown:");
-  pinMode(lc_clock, INPUT);
+  for (int i = 0; i < 10; i++) {
-  // Configure load cell data pins as inputs
+    Serial.println(10 - i);
-  pinMode(lc_data_0, INPUT);
+    delay(1000);
-  pinMode(lc_data_1, INPUT);
+  }
-  pinMode(lc_data_2, INPUT);
-  pinMode(lc_data_3, INPUT);
 
-  delay(1000);
+  init_loadCells();
   init_Vehicle(State);
+  State.lc0 = lc0.get_value();
+  State.lc1 = lc1.get_value();
+  State.lc2 = lc2.get_value();
+  State.lc3 = lc3.get_value();
+
+  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
   burnStartTimeCalc(State);
   Serial.println("Starting Height Calculated");
-  delay(1000);
 
-  init_LoadCells(loadCells);
-
-  Serial.println("Load Cells Calibrated");
-  delay(1000);
   initFile();
-  delay(1000);
 
   initTime = micros();
 }
+
+void loop() {
+  last = micros();
+
+  vehicleDynamics(State, PrevState);
+  thrustInfo(State);
+  pidController(State, PrevState);
+  TVC(State, PrevState);
+  processTVC(State, servo1, servo2);
+
+  State.stepDuration = micros() - last;
+  write2CSV(State);
+
+  // Set previous values for next timestep
+  PrevState = State;
+  State.time += State.stepSize;
+
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+    printSimResults(State);
+    Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
+                   " seconds");
+
+    closeFile();
+    delay(3000);
+
+    Serial.println("SUCCESS");
+    Serial.println("Exiting Sim");
+    teensyAbort();
+  }
+
+  delay(State.stepSize - ((micros() - last) / 1000.0));
+}
+
+void init_loadCells() {
+  // 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);
+
+  // 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");
+
+  Serial.print("Calibrating");
+  lc0.tare();
+  Serial.print(".");
+  lc1.tare();
+  Serial.print(".");
+  lc2.tare();
+  Serial.print(".");
+  lc3.tare();
+  Serial.println(".");
+  Serial.println("Load Cells Calibrated");
+}
+
+// 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(); }
 #endif
 
-#if defined(NATIVE) || defined(_WIN32)
+#if defined(_WIN32) || defined(NATIVE)
-void loop() {
+
+#include "native.h"
+outVector stateVector;
+
+int main() {
+
+  init_Vehicle(State);
+
+  // Determine when to burn
+  burnStartTimeCalc(State);
+
+  do {
     vehicleDynamics(State, PrevState);
     thrustInfo(State);
     pidController(State, PrevState);
@@ -98,52 +190,6 @@ void loop() {
       printSimResults(State);
       init_Vehicle(State);
     }
-}
-#elif defined(TEENSY)
-void loop() {
-
-  last = micros();
-  vehicleDynamics(State, PrevState);
-  thrustInfo(State);
-  pidController(State, PrevState);
-  TVC(State, PrevState);
-  
-  processTVC(State, loadCells);
-
-  State.stepDuration = micros() - last;
-  write2CSV(State);
-
-  // Set "prev" values for next timestep
-  PrevState = State;
-  
-  // state2vec(State, PrevState, stateVector);
-
-
-  State.time += State.stepSize;
-
-  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
-    printSimResults(State);
-    Serial.println("Run duration:" + String(micros() - initTime) + " us");
-
-    closeFile();
-    delay(20000);
-
-    Serial.println("SUCCESS");
-    Serial.println("Aborting Sim");
-    teensyAbort();
-  }
-
-  delay(20 - ((micros() - last) * 1000.0));
-}
-#endif
-
-#if defined(_WIN32) || defined(NATIVE)
-int main() {
-
-  setup();
-
-  do {
-    loop();
   } while ((State.z > 0.0) || (State.thrustFiring != 2));
 
   return 0;