Gyro Support (#6)

* Initial IMU work

* IMU integration

* Set gyro identifier upon init

* Handle Gyro initialization

* Properly handle Gyro messages

---------

Co-authored-by: Zhiquan Yeo <[email protected]>

Author: zhiquanyeo

include/imu.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <Wire.h>
+#include <Adafruit_LSM6DSOX.h>
+
+#define IMU_I2C_ADDR 0x6B
+#define IMU_UPDATE_RATE_HZ 20
+
+namespace xrp {
+
+bool imuIsReady();
+
+void imuSetEnabled(bool enabled);
+bool imuIsEnabled();
+
+void imuInit(uint8_t addr, TwoWire *theWire);
+
+bool imuPeriodic();
+
+float gyroGetAngleX();
+float gyroGetRateX();
+
+float gyroGetAngleY();
+float gyroGetRateY();
+
+float gyroGetAngleZ();
+float gyroGetRateZ();
+
+void gyroReset();
+
+} // namespace xrp

include/robot.h

@@ -56,7 +56,7 @@ bool robotInitialized();
 uint8_t robotPeriodic();
 
 // Robot control
-void setEnabled(bool enabled);
+void robotSetEnabled(bool enabled);
 
 // Encoder Related
 void configureEncoder(int deviceId, int chA, int chB);

include/wpilibws.h

@@ -4,12 +4,20 @@
 
 namespace wpilibws {
 
+enum ws_gyro_axis_t {
+  AXIS_X,
+  AXIS_Y,
+  AXIS_Z
+};
+
 bool dsWatchdogActive();
 
 void processWSMessage(JsonDocument& jsonMsg);
 
 // Message Encoders
 std::string makeEncoderMessage(int deviceId, int count);
 std::string makeDIOMessage(int deviceId, bool value);
+std::string makeGyroCombinedMessage(float rates[3], float angles[3]);
+std::string makeGyroSingleMessage(ws_gyro_axis_t axis, float rate, float angle);
 
 } // namespace wpilibws

platformio.ini

@@ -9,3 +9,7 @@ board = rpipicow
 lib_deps =
     links2004/WebSockets@^2.4.1
     bblanchon/ArduinoJson
+    Wire
+    adafruit/Adafruit LSM6DS@^4.7.0
+    adafruit/Adafruit BusIO@^1.14.1
+    adafruit/Adafruit Unified Sensor@^1.1.9

src/imu.cpp

@@ -0,0 +1,156 @@
+#include "imu.h"
+
+namespace xrp {
+
+unsigned long _imuUpdatePeriod = 1000 / IMU_UPDATE_RATE_HZ;
+Adafruit_LSM6DSOX _lsm6;
+bool _imuReady = false;
+bool _imuEnabled = false;
+
+unsigned long _lastIMUUpdateTime = 0;
+bool _imuOnePassComplete = false;
+
+float _gyroOffsetsDegPerSec[3] = {0, 0, 0};
+float _gyroRatesDegPerSec[3] = {0, 0, 0};
+float _gyroAnglesDeg[3] = {0, 0, 0};
+
+float _radToDeg(float angleRad) {
+  return angleRad * 180.0 / PI;
+}
+
+bool imuIsReady() {
+    return _imuReady;
+}
+
+void imuSetEnabled(bool enabled) {
+  _imuEnabled = enabled;
+  Serial.printf("[IMU] %s\n", enabled ? "Enabling" : "Disabling");
+}
+
+bool imuIsEnabled() {
+  return _imuEnabled;
+}
+
+void imuInit(uint8_t addr, TwoWire *theWire) {
+  if (!_lsm6.begin_I2C(addr, theWire, 0)) {
+    Serial.println("Failed to find LSM6DSOX");
+    _imuReady = false;
+  }
+  else {
+    _imuReady = true;
+    Serial.println("--- IMU ---");
+    Serial.println("LSM6DSOX detected");
+    Serial.print("Accel Range: ");
+    switch (_lsm6.getAccelRange()) {
+      case LSM6DS_ACCEL_RANGE_2_G:
+        Serial.println("+-2G");
+        break;
+      case LSM6DS_ACCEL_RANGE_4_G:
+        Serial.println("+-4G");
+        break;
+      case LSM6DS_ACCEL_RANGE_8_G:
+        Serial.println("+-8G");
+        break;
+      case LSM6DS_ACCEL_RANGE_16_G:
+        Serial.println("+-16G");
+        break;
+    }
+
+    Serial.print("Gyro Range: ");
+    switch(_lsm6.getGyroRange()) {
+      case LSM6DS_GYRO_RANGE_125_DPS:
+        Serial.println("125 DPS");
+        break;
+      case LSM6DS_GYRO_RANGE_250_DPS:
+        Serial.println("250 DPS");
+        break;
+      case LSM6DS_GYRO_RANGE_500_DPS:
+        Serial.println("500 DPS");
+        break;
+      case LSM6DS_GYRO_RANGE_1000_DPS:
+        Serial.println("1000 DPS");
+        break;
+      case LSM6DS_GYRO_RANGE_2000_DPS:
+        Serial.println("2000 DPS");
+        break;
+      case ISM330DHCX_GYRO_RANGE_4000_DPS:
+        break;
+    }
+  }
+}
+
+bool imuPeriodic() {
+  if (!_imuReady) return false;
+  if (!_imuEnabled) return false;
+
+  unsigned long currTime = millis();
+  unsigned long dt = currTime - _lastIMUUpdateTime;
+
+  // Send the IMU data at the specified IMU_UPDATE_RATE_HZ
+  if (dt < _imuUpdatePeriod) return false;
+
+  // Get IMU data
+  sensors_event_t accel;
+  sensors_event_t gyro;
+  sensors_event_t temp;
+
+  _lsm6.getEvent(&accel, &gyro, &temp);
+
+  float rateX = _radToDeg(gyro.gyro.x);
+  float rateY = _radToDeg(gyro.gyro.y);
+  float rateZ = _radToDeg(gyro.gyro.z);
+  _gyroRatesDegPerSec[0] = rateX - _gyroOffsetsDegPerSec[0];
+  _gyroRatesDegPerSec[1] = rateY - _gyroOffsetsDegPerSec[1];
+  _gyroRatesDegPerSec[2] = rateZ - _gyroOffsetsDegPerSec[2];
+
+  // We can't integrate with only a single value, so bail out
+  if (!_imuOnePassComplete) {
+    _imuOnePassComplete = true;
+    _lastIMUUpdateTime = currTime;
+    return false;
+  }
+  
+  float dtInSeconds = dt / 1000.0;
+
+  for (int i = 0; i < 3; i++) {
+    _gyroAnglesDeg[i] = _gyroAnglesDeg[i] + (_gyroRatesDegPerSec[i] * dtInSeconds);
+  }
+
+  _lastIMUUpdateTime = currTime;
+  return true;
+}
+
+float gyroGetAngleX() {
+  return _gyroAnglesDeg[0];
+}
+
+float gyroGetRateX() {
+  return _gyroRatesDegPerSec[0];
+}
+
+float gyroGetAngleY() {
+  return _gyroAnglesDeg[1];
+}
+
+float gyroGetRateY() {
+  return _gyroRatesDegPerSec[1];
+}
+
+float gyroGetAngleZ() {
+  return _gyroAnglesDeg[2];
+}
+
+float gyroGetRateZ() {
+  return _gyroRatesDegPerSec[2];
+}
+
+void gyroReset() {
+  for (int i = 0; i < 3; i++) {
+    _gyroRatesDegPerSec[i] = 0;
+    _gyroAnglesDeg[i] = 0;
+  }
+
+  _imuOnePassComplete = false;
+}
+
+} // namespace xrp

src/main.cpp

@@ -4,8 +4,11 @@
 #include <WebSockets4WebServer.h>
 #include <WebSocketsServer.h>
 #include <WiFi.h>
+#include <Wire.h>
+
 #include <vector>
 
+#include "imu.h"
 #include "robot.h"
 #include "wpilibws.h"
 
@@ -94,8 +97,19 @@ void setup() {
 
   Serial.begin(115200);
 
+  // Set up the I2C pins
+  Wire1.setSCL(19);
+  Wire1.setSDA(18);
+  Wire1.begin();
+
   delay(2000);
 
+  // Initialize IMU
+  Serial.println("[IMU] Initializing IMU");
+  xrp::imuInit(IMU_I2C_ADDR, &Wire1);
+
+  // TODO Calibrate IMU
+
   // Busy-loop if there's no WiFi hardware
   if (WiFi.status() == WL_NO_MODULE) {
     Serial.println("[NET] No WiFi Module");
@@ -148,7 +162,9 @@ void loop() {
   // Disable the robot when we no longer have a connection
   int numConnectedClients = wsServer.connectedClients();
   if (lastCheckedNumClients > 0 && numConnectedClients == 0) {
-    xrp::setEnabled(false);
+    xrp::robotSetEnabled(false);
+    xrp::imuSetEnabled(false);
+    outboundMessages.clear();
   }
   lastCheckedNumClients = numConnectedClients;
 
@@ -170,6 +186,14 @@ void loop() {
       sendMessage(wpilibws::makeDIOMessage(0, xrp::isUserButtonPressed()));
     }
 
+    // Read Gyro Data
+    if (xrp::imuPeriodic()) {
+      float rateZ = xrp::gyroGetRateZ();
+      float angleZ = xrp::gyroGetAngleZ();
+      
+      sendMessage(wpilibws::makeGyroSingleMessage(wpilibws::AXIS_Z, rateZ, angleZ));
+    }
+
   }
 
   updateLoopTime(loopStartTime);

src/robot.cpp

@@ -249,7 +249,7 @@ bool isUserButtonPressed() {
   return digitalRead(XRP_BUILTIN_BUTTON) == LOW;
 }
 
-void setEnabled(bool enabled) {
+void robotSetEnabled(bool enabled) {
   // Prevent motors from starting with arbitrary values when enabling
   if (!_robotEnabled && enabled) {
     _pwmShutoff();