Finish I2C IMU

* Still cannot detect upward motion
* Can detect left and right

diff --git a/02-I2CIMU/02-I2CIMU.ino b/02-I2CIMU/02-I2CIMU.ino
index 74f966a4e2fe85eb6097ef342c3d3f2633e66ad8..c83b16964b60d7fdb088b4965c8585e4732f3759 100644 (file)
--- a/02-I2CIMU/02-I2CIMU.ino
+++ b/02-I2CIMU/02-I2CIMU.ino
@@ -1,6 +1,201 @@
+/**
+ * @file 02-I2cIMU.ino
+ * @author Advaith Menon, Anish Gajula, Lila Phonekeo
+ * @version 2025.09.16
+ * @brief Accelrometer reading
+ */
+#include <Arduino.h>
+#include <Adafruit_NeoPixel.h>
+#include <stdint.h>
+#include <ICM_20948.h>
+
+#define NUM_LEDS 1
+#define LED_PIN 8 /* TODO */
+#define GOOD_VAL 1000
+
+#define I2C_ADDR ICM_20948_I2C_ADDR_AD1
+#define CLOCKF 400000
+
+
+ICM_20948_Status_e wi2c(uint8_t reg, uint8_t *data, uint32_t len,
+        void *user);
+ICM_20948_Status_e ri2c(uint8_t reg, uint8_t *buff, uint32_t len,
+        void *user);
+void color_change(int16_t x, int16_t y, int16_t z);
+
+ICM_20948_Serif_t imu_serif = {
+    wi2c, ri2c, NULL};
+
+ICM_20948_Device_t imu;
+
+Adafruit_NeoPixel pixel(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);
+
+struct color {
+    uint8_t r;
+    uint8_t g;
+    uint8_t b;
+};
+
+struct color xDir[2] = {
+    {255, 0, 0},
+    {0, 255, 0}};
+struct color yDir[2] = {
+    {255, 255, 0},
+    {0, 255, 255}
+};
+struct color zDir[2] = {
+    {255, 0, 255},
+    {0, 0, 255}
+};
+
+struct color allColor[3][2] = {
+    {
+        {255, 0, 0},
+        {0, 255, 0}
+    },
+    {
+        {255, 255, 0},
+        {0, 255, 255}
+    },
+    {
+        {255, 0, 255},
+        {0, 0, 255}
+    }
+};
+
+uint8_t curr_col = 0;
+uint8_t curr_dir = 0;
+
+
 
 void setup() {
+    Serial.begin(9600);
+    Serial.println("hello");
+    Wire.begin();
+    Wire.setClock(CLOCKF);
+    ICM_20948_init_struct(&imu);
+    ICM_20948_link_serif(&imu, &imu_serif);
+    while (ICM_20948_check_id(&imu) != ICM_20948_Stat_Ok)
+    {
+        Serial.println("whoami does not match. Halting...");
+        delay(1000);
+    }
+
+    ICM_20948_Status_e stat = ICM_20948_Stat_Err;
+    uint8_t whoami = 0x00;
+    while ((stat != ICM_20948_Stat_Ok) || (whoami != ICM_20948_WHOAMI))
+    {
+        whoami = 0x00;
+        stat = ICM_20948_get_who_am_i(&imu, &whoami);
+        Serial.print("whoami does not match (0x");
+        Serial.print(whoami, HEX);
+        Serial.print("). Halting...");
+        Serial.println();
+        delay(1000);
+    }
+
+    // Here we are doing a SW reset to make sure the device starts in a known state
+    ICM_20948_sw_reset(&imu);
+    delay(250);
+
+
+    // set devide state
+    ICM_20948_set_sample_mode(&imu,
+            (ICM_20948_InternalSensorID_bm)(ICM_20948_Internal_Acc
+                | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
+
+
+    // Set full scale ranges for gyro
+    ICM_20948_fss_t fss;
+    fss.a = gpm2;
+    fss.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
+    ICM_20948_set_full_scale(&imu, 
+        (ICM_20948_InternalSensorID_bm)(ICM_20948_Internal_Acc | 
+        ICM_20948_Internal_Gyr), fss);
+
+    // wake up sleepy sensor
+    ICM_20948_sleep(&imu, false);
+    ICM_20948_low_power(&imu, false);
+
+    pixel.begin();
+    pixel.setBrightness(100);
 }
 
+
 void loop() {
+    delay(100);
+
+    ICM_20948_AGMT_t agmt = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0}};
+    if (ICM_20948_get_agmt(&imu, &agmt) == ICM_20948_Stat_Ok)
+    {
+        char stri[64];
+        snprintf(stri, sizeof(stri), "x=%d, y=%d, z=%d", agmt.acc.axes.x,
+                agmt.acc.axes.y, agmt.acc.axes.z);
+        Serial.println(stri);
+        color_change(agmt.acc.axes.x, agmt.acc.axes.y, agmt.acc.axes.z);
+    }
+    else
+    {
+        Serial.println("cooked");
+    }
+}
+
+
+void color_change(int16_t x, int16_t y, int16_t z) {
+    uint16_t xa, ya, za;
+    int16_t co; // chosen one
+    struct color c;
+    /* find direction */
+    xa = (x < 0) ? -x : x;
+    ya = (y < 0) ? -y : y;
+    za = (z < 0) ? -z : z;
+
+    /* set dirn */
+    if (xa >= GOOD_VAL) {
+        curr_col = 0;
+        co = x;
+    } else if (ya >= GOOD_VAL) {
+        curr_col = 1;
+        co = y;
+    } else if (za >= GOOD_VAL) {
+        curr_col = 2;
+        co = z;
+    }
+
+    curr_dir = (co < 0) ? 1 : 0;
+
+    c = allColor[curr_col][curr_dir];
+
+    pixel.setPixelColor(0, pixel.Color(c.r, c.g, c.b));
+    pixel.show();
+
+}
+
+
+ICM_20948_Status_e wi2c(uint8_t reg, uint8_t *data, uint32_t len, void *user)
+{
+  Wire.beginTransmission(I2C_ADDR);
+  Wire.write(reg);
+  Wire.write(data, len);
+  Wire.endTransmission();
+
+  return ICM_20948_Stat_Ok;
+}
+
+ICM_20948_Status_e ri2c(uint8_t reg, uint8_t *buff, uint32_t len, void *user)
+{
+  Wire.beginTransmission(I2C_ADDR);
+  Wire.write(reg);
+  Wire.endTransmission(false); // Send repeated start
+
+  uint32_t num_received = Wire.requestFrom(I2C_ADDR, len);
+  if (num_received == len)
+  {
+    for (uint32_t i = 0; i < len; i++)
+    {
+      buff[i] = Wire.read();
+    }
+  }
+
+  return ICM_20948_Stat_Ok;
 }