encapsulation and optimization

code/arduino/Movement-sensor-code/Movement-sensor-code.ino

@@ -1,6 +1,6 @@
 #include "headerFile.h"
 
-SensorManager::Rotation offset;
+// SensorManager::Rotation offset;
 
 void setup() {
   Serial.begin(9600);
@@ -10,51 +10,49 @@ void setup() {
   sensorManager.sensorSetup();
 
 	//ws server address, port and URL
-	webSocket.begin("145.3.245.22", 8001, "");
+	webSocket.begin("145.28.160.108", 8001, "");
 	// try every 500 again if connection has failed
 	webSocket.setReconnectInterval(500);
 }
 
 void loop() {
-  SensorManager::Rotation rotation = sensorManager.readLoop();
+  SensorManager::eulerAngles eulerRotation = sensorManager.getEulerAngles();
 
   // Subtract offset
-  rotation.i -= offset.i;
+  // rotation.i -= offset.i;
-  rotation.j -= offset.j;
+  // rotation.j -= offset.j;
-  rotation.k -= offset.k;
+  // rotation.k -= offset.k;
-  rotation.w -= offset.w;
+  // rotation.w -= offset.w;
 
   // Convert quaternion to Euler angles in radians
-  float roll = atan2(2.0f * (rotation.w * rotation.i + rotation.j * rotation.k), 1.0f - 2.0f * (rotation.i * rotation.i + rotation.j * rotation.j));
+
-  float pitch = asin(2.0f * (rotation.w * rotation.j - rotation.k * rotation.i));
-  float yaw = atan2(2.0f * (rotation.w * rotation.k + rotation.i * rotation.j), 1.0f - 2.0f * (rotation.j * rotation.j + rotation.k * rotation.k));
 
   // Convert to degrees
-  float rollDegrees = roll * 180.0f / PI;
+  // float rollDegrees = roll * 180.0f / PI;
-  float pitchDegrees = pitch * 180.0f / PI;
+  // float pitchDegrees = pitch * 180.0f / PI;
-  float yawDegrees = yaw * 180.0f / PI;
+  // float yawDegrees = yaw * 180.0f / PI;
 
-  Serial.print(roll);
+  Serial.print(eulerRotation.roll);
   Serial.print(" ");
-  Serial.print(pitch);
+  Serial.print(eulerRotation.pitch);
   Serial.print(" ");
-  Serial.print(yaw);
+  Serial.print(eulerRotation.yaw);
-  sendData(roll, pitch, yaw);
+  sendData(eulerRotation.roll, eulerRotation.pitch, eulerRotation.yaw);
 
   Serial.println();
   webSocket.loop();
-
-  if (Serial.available()) {
-    String command = Serial.readStringUntil('\n');
-    command.trim(); // remove any trailing whitespace
-    if (command == "setZeroPoint") {
-      setZeroPoint();
-    }
-  }
-}
-void setZeroPoint() {
-  offset = sensorManager.readLoop();
 }
+//   if (Serial.available()) {
+//     String command = Serial.readStringUntil('\n');
+//     command.trim(); // remove any trailing whitespace
+//     if (command == "setZeroPoint") {
+//       setZeroPoint();
+//     }
+//   }
+// }
+// void setZeroPoint() {
+//   offset = sensorManager.readLoop();
+// }
 
 void sendData(float roll, float pitch, float yaw){
   String message = "{\"Sensor\": 1, \"roll\":\"" + String(roll) + "\",\"pitch\":\"" + String(pitch) + "\",\"yaw\":\"" + String(yaw) + "\"}";

code/arduino/Movement-sensor-code/SensorManager.cpp

@@ -15,12 +15,12 @@ void SensorManager::sensorSetup() {
   //start sensorfunction and start autocalibration
   //once calibration is enabled it attempts to every 5 min
 
-  Wire.setClock(400000);  //Increase I2C data rate to 400kHz
+  Wire.setClock(400000);                          //Increase I2C data rate to 400kHz
-  myIMU.calibrateAll();   //Turn on cal for Accel, Gyro, and Mag
+  myIMU.calibrateAll();                           //Turn on cal for Accel, Gyro, and Mag
-  myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms
+  myIMU.enableGyroIntegratedRotationVector(100);  //send data every 100ms
-  myIMU.enableMagnetometer(100);     //Send data update every 100ms
+  myIMU.enableMagnetometer(100);                  //Send data update every 100ms
-  myIMU.saveCalibration();           //Saves the current dynamic calibration data (DCD) to memory
+  myIMU.saveCalibration();                        //Saves the current dynamic calibration data (DCD) to memory
-  myIMU.requestCalibrationStatus();  //Sends command to get the latest calibration status
+  myIMU.requestCalibrationStatus();               //Sends command to get the latest calibration status
 
   if (myIMU.calibrationComplete() == true) {
     Serial.println("Calibration data successfully stored");
@@ -29,23 +29,31 @@ void SensorManager::sensorSetup() {
   Serial.println(F("magnetometer rotation enabled"));
 }
 
-SensorManager::Rotation SensorManager::readLoop() {
+SensorManager::RotationQuintillions SensorManager::getQuintillions() {
   if (myIMU.dataAvailable() == true) {
     float i = myIMU.getQuatI();
     float j = myIMU.getQuatJ();
     float k = myIMU.getQuatK();
     float w = myIMU.getQuatReal();
 
-    Rotation rotation = { i, j, k, w };
+    RotationQuintillions rotation = { i, j, k, w };
     return rotation;
-  }
+  } else {
-  else {
     float i = myIMU.getQuatI();
     float j = myIMU.getQuatJ();
     float k = myIMU.getQuatK();
     float w = myIMU.getQuatReal();
 
-    Rotation rotation = { i, j, k, w };
+    RotationQuintillions rotation = { i, j, k, w };
     return rotation;
   }
 }
+
+SensorManager::eulerAngles SensorManager::getEulerAngles() {
+  SensorManager::RotationQuintillions rotation = getQuintillions();
+  float roll = atan2(2.0f * (rotation.w * rotation.i + rotation.j * rotation.k), 1.0f - 2.0f * (rotation.i * rotation.i + rotation.j * rotation.j));
+  float pitch = asin(2.0f * (rotation.w * rotation.j - rotation.k * rotation.i));
+  float yaw = atan2(2.0f * (rotation.w * rotation.k + rotation.i * rotation.j), 1.0f - 2.0f * (rotation.j * rotation.j + rotation.k * rotation.k));
+  eulerAngles EulerAngles = { roll, pitch, yaw };
+  return EulerAngles;
+}

code/arduino/Movement-sensor-code/SensorManager.h

@@ -5,18 +5,26 @@
 #include "SparkFun_BNO080_Arduino_Library.h"
 
 class SensorManager {
-  public:
+public:
-    SensorManager();
+  SensorManager();
-    void sensorSetup();
+  void sensorSetup();
-    struct Rotation {
+  struct eulerAngles {
-      float i;
+    float yaw;
-      float j;
+    float pitch;
-      float k;
+    float roll;
-      float w;
+  };
-    };
+  eulerAngles getEulerAngles();
-    Rotation readLoop();
+
-  private:
+
-    BNO080 myIMU;
+private:
+  struct RotationQuintillions {
+    float i;
+    float j;
+    float k;
+    float w;
+  };
+  RotationQuintillions getQuintillions();
+  BNO080 myIMU;
 };
 
 #endif