modified to use adxl345 instaad of bno055

HansonServo.ino

@@ -161,7 +161,7 @@ void runNodeAnimation() {
           }
           break;
       default:
-        break;
+          break;
       }
       currentTick = 0;
   }
@@ -206,10 +206,10 @@ void sendHeartbeat() {
   uint16_t flags = 0;
   
   // Build flags
-  if (imu.isReady()) flags |= 0x01;
+  if (adxl.isReady()) flags |= 0x01;
   if (animState.current && animState.current->isActive()) flags |= 0x02;
   if (motorStream.active) flags |= 0x04;
-  if (sensors.isIMUStreamEnabled()) flags |= 0x08;
+  if (sensors.isADXLStreamEnabled()) flags |= 0x08;
   if (sensors.isRadarStreamEnabled()) flags |= 0x10;
 
   payload[0] = uptime & 0xFF;

PROTOCOL_MIGRATION.md

@@ -101,7 +101,7 @@ ushort Crc16Ccitt(byte[] data)
 
 | Tag | Description |
 |-----|-------------|
-| `IMU0` | IMU data (heading, roll, pitch) |
+| `IMU0` | IMU data (accel x,y,z) |
 | `RDAR` | Radar target data |
 | `STAT` | System state/heartbeat |
 | `ACK!` | Acknowledge (success) |
@@ -225,12 +225,24 @@ When enabled, device streams `MPOS` packets every 50ms.
 #### `IMU0` - IMU Data (device → host)
 **Payload:**
 ```
-[heading: 2 bytes LE, signed]  // degrees × 100
+[accelX: 2 bytes LE, signed]   // g-forces × 100
-[roll: 2 bytes LE, signed]     // degrees × 100
+[accelY: 2 bytes LE, signed]   // g-forces × 100
+[accelZ: 2 bytes LE, signed]   // g-forces × 100
 [pitch: 2 bytes LE, signed]    // degrees × 100
+[roll: 2 bytes LE, signed]     // degrees × 100
 ```
 *Sent automatically when IMU streaming is enabled*
 
+**Coordinate System:**
+- X: left/right axis (affects roll)
+- Y: front/back axis (affects pitch)
+- Z: up/down axis
+
+**Notes:**
+- Acceleration values scaled by 100 (not 1000 as before)
+- Euler angles calculated from accelerometer data
+- Heading/yaw not available (accelerometer only)
+
 #### `RDAR` - Radar Data (device → host)
 **Payload:**
 ```

nodegraph.cpp

@@ -6,7 +6,7 @@
 #include <cstring>
 
 extern RobotConfig config;
-extern IMU imu;
+extern ADXL345 adxl;
 extern Radar radar;
 
 
@@ -65,29 +65,48 @@ void VariableNode::evaluate(uint32_t currentTick) {
       outputValue = config.getMotorPosition(arg0);
       break;
       
-    // IMU sources - scale to 0-4095 range
+    // ADXL345 accelerometer sources - scale to 0-4095 range
-    case VAR_IMU_HEADING:
+    case VAR_ACCEL_X:
-      // Heading: 0-360° → 0-4095
+      // X acceleration: -2g to +2g → 0-4095 (2048 = 0g)
-      if (imu.isReady()) {
+      if (adxl.isReady()) {
-        outputValue = (uint16_t)((imu.getHeading() / 360.0f) * 4095.0f);
+        float accel = constrain(adxl.getAccelX(), -2.0f, 2.0f);
+        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
       } else {
-        outputValue = 2048;  // Center if not ready
+        outputValue = 2048;
       }
       break;
-    case VAR_IMU_PITCH:
+    case VAR_ACCEL_Y:
-      // Pitch: -90° to +90° → 0-4095 (2048 = level)
+      // Y acceleration: -2g to +2g → 0-4095 (2048 = 0g)
-      if (imu.isReady()) {
+      if (adxl.isReady()) {
-        float pitch = constrain(imu.getPitch(), -90.0f, 90.0f);
+        float accel = constrain(adxl.getAccelY(), -2.0f, 2.0f);
+        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_ACCEL_Z:
+      // Z acceleration: -2g to +2g → 0-4095 (2048 = 0g)
+      if (adxl.isReady()) {
+        float accel = constrain(adxl.getAccelZ(), -2.0f, 2.0f);
+        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_TILT_PITCH:
+      // Pitch angle: -90° to +90° → 0-4095 (2048 = level)
+      if (adxl.isReady()) {
+        float pitch = constrain(adxl.getPitch(), -90.0f, 90.0f);
         outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
       } else {
         outputValue = 2048;
       }
       break;
-    case VAR_IMU_ROLL:
+    case VAR_TILT_ROLL:
-      // Roll: -180° to +180° → 0-4095 (2048 = level)
+      // Roll angle: -90° to +90° → 0-4095 (2048 = level)
-      if (imu.isReady()) {
+      if (adxl.isReady()) {
-        float roll = constrain(imu.getRoll(), -180.0f, 180.0f);
+        float roll = constrain(adxl.getRoll(), -90.0f, 90.0f);
-        outputValue = (uint16_t)(((roll + 180.0f) / 360.0f) * 4095.0f);
+        outputValue = (uint16_t)(((roll + 90.0f) / 180.0f) * 4095.0f);
       } else {
         outputValue = 2048;
       }

nodegraph.h

@@ -20,10 +20,12 @@ enum VariableSource {
   VAR_SINE,
   VAR_ANALOG,
   VAR_SERVO_FEEDBACK,
-  // IMU sources (BNO055)
+  // ADXL345 accelerometer sources
-  VAR_IMU_HEADING,    // 0-360 degrees → 0-4095
+  VAR_ACCEL_X,        // X acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
-  VAR_IMU_PITCH,      // -90 to +90 degrees → 0-4095 (2048 = level)
+  VAR_ACCEL_Y,        // Y acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
-  VAR_IMU_ROLL,       // -180 to +180 degrees → 0-4095 (2048 = level)
+  VAR_ACCEL_Z,        // Z acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
+  VAR_TILT_PITCH,     // Pitch angle from accel → 0-4095 (-90° to +90°, 2048 = level)
+  VAR_TILT_ROLL,      // Roll angle from accel → 0-4095 (-90° to +90°, 2048 = level)
   // Radar sources (RD-03D) - primary target
   VAR_RADAR_X,        // X position (angle) → 0-4095 (2048 = center)
   VAR_RADAR_Y         // Y distance → 0-4095 (scaled 0-500cm)

protocol.h

@@ -41,7 +41,7 @@ namespace Tag {
   constexpr char MSTRM[4] = {'M','S','T','M'};  // Motor stream control
   
   // Sensors
-  constexpr char IMU[4]   = {'I','M','U','0'};  // IMU data (heading, roll, pitch)
+  constexpr char IMU[4]   = {'I','M','U','0'};  // ADXL acceleration data (x,y,z)
   constexpr char RADAR[4] = {'R','D','A','R'};  // Radar targets
   constexpr char FRAME[4] = {'F','R','M','E'};  // Animation frame events (frame, mode, status)
   

sensors.cpp

@@ -6,7 +6,7 @@
 // ============================================================================
 
 Radar radar;
-IMU imu;
+ADXL345 adxl;
 SensorManager sensors;
 
 // ============================================================================
@@ -128,79 +128,115 @@ uint16_t Radar::packPayload(uint8_t* buffer) const {
 }
 
 // ============================================================================
-// IMU Implementation
+// ADXL345 Implementation
 // ============================================================================
 
-IMU::IMU(uint8_t addr) : addr(addr) {}
+ADXL345::ADXL345(uint8_t addr) : addr(addr) {}
 
-bool IMU::init() {
+bool ADXL345::init() {
   Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
   delay(100);
-  
+
-  uint8_t id = read8(0x00);  // CHIP_ID register
+  uint8_t id = read8(0x00);  // DEVID register (should be 0xE5)
-  if (id != 0xA0) {
+  if (id != 0xE5) {
     ready = false;
     return false;
   }
-  
+
-  // Enter config mode
+  // Enable measurement mode
-  write8(0x3D, 0x00);
+  write8(0x2D, 0x08);  // POWER_CTL: measure mode
-  delay(25);
+
-  
-  // Set NDOF fusion mode
-  write8(0x3D, 0x0C);
-  delay(25);
-  
   ready = true;
   return true;
 }
 
-bool IMU::update() {
+bool ADXL345::update() {
   if (!ready) return false;
-  
+
-  Wire.beginTransmission(addr);
+  readAccelData();
-  Wire.write(0x1A);  // Euler angles start register
-  Wire.endTransmission();
-  
-  Wire.requestFrom(addr, (uint8_t)6);
-  if (Wire.available() < 6) return false;
-  
-  int16_t rawHeading = Wire.read() | (Wire.read() << 8);
-  int16_t rawPitch = Wire.read() | (Wire.read() << 8);
-  int16_t rawRoll = Wire.read() | (Wire.read() << 8);
-  
-  // Convert from 1/16 degree units
-  heading = rawHeading / 16.0f;
-  roll = -(rawRoll / 16.0f);  // Inverted so right roll is positive
-  pitch = rawPitch / 16.0f;
-  
   return true;
 }
 
-uint16_t IMU::packPayload(uint8_t* buffer) const {
+float ADXL345::getPitch() const {
-  // Format: heading(2, unsigned) + roll(2, signed) + pitch(2, signed), all *100
+  // Approximate pitch from Y/Z acceleration (Y = front/back axis)
-  uint16_t h = (uint16_t)(heading * 100.0f);   // 0..36000 fits in uint16
+  // This is not as accurate as a proper IMU with gyroscope
-  int16_t r = (int16_t)(roll * 100.0f);
+  if (accelZ == 0) return 0;
-  int16_t p = (int16_t)(pitch * 100.0f);
+  return atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * 180.0f / PI;
-  
-  buffer[0] = h & 0xFF;
-  buffer[1] = (h >> 8) & 0xFF;
-  buffer[2] = r & 0xFF;
-  buffer[3] = (r >> 8) & 0xFF;
-  buffer[4] = p & 0xFF;
-  buffer[5] = (p >> 8) & 0xFF;
-  
-  return 6;
 }
 
-void IMU::write8(uint8_t reg, uint8_t value) {
+float ADXL345::getRoll() const {
+  // Approximate roll from X/Z acceleration (X = left/right axis)
+  if (accelZ == 0) return 0;
+  return atan2(accelX, accelZ) * 180.0f / PI;
+}
+
+void ADXL345::getEulerAngles(float& pitch, float& roll) const {
+  // Calculate Euler angles from accelerometer data
+  // Note: Heading (yaw) cannot be determined from accelerometer alone
+  // Coordinate system: X=left/right, Y=front/back, Z=up/down
+
+  if (!ready) {
+    pitch = 0.0f;
+    roll = 0.0f;
+    return;
+  }
+
+  // Pitch (front/back tilt) = atan2(-accelY, sqrt(accelX² + accelZ²))
+  // Roll (left/right tilt) = atan2(accelX, accelZ)
+
+  float accelMagnitude = sqrt(accelX * accelX + accelZ * accelZ);
+  if (accelMagnitude > 0.01f) {  // Avoid division by very small numbers
+    pitch = atan2(-accelY, accelMagnitude) * 180.0f / PI;
+  } else {
+    pitch = 0.0f;
+  }
+
+  if (fabs(accelZ) > 0.01f) {  // Avoid division by zero
+    roll = atan2(accelX, accelZ) * 180.0f / PI;
+  } else {
+    roll = 0.0f;
+  }
+}
+
+uint16_t ADXL345::packPayload(uint8_t* buffer) const {
+  // Format: accelX(2) + accelY(2) + accelZ(2) + pitch(2) + roll(2), all ×100
+  // Accelerations in g-forces ×100, angles in degrees ×100
+
+  // Pack acceleration data
+  int16_t x = (int16_t)(accelX * 100.0f);
+  int16_t y = (int16_t)(accelY * 100.0f);
+  int16_t z = (int16_t)(accelZ * 100.0f);
+
+  buffer[0] = x & 0xFF;
+  buffer[1] = (x >> 8) & 0xFF;
+  buffer[2] = y & 0xFF;
+  buffer[3] = (y >> 8) & 0xFF;
+  buffer[4] = z & 0xFF;
+  buffer[5] = (z >> 8) & 0xFF;
+
+  // Calculate and pack Euler angles
+  float pitch_deg, roll_deg;
+  getEulerAngles(pitch_deg, roll_deg);
+
+  int16_t pitch = (int16_t)(pitch_deg * 100.0f);
+  int16_t roll = (int16_t)(roll_deg * 100.0f);
+
+  buffer[6] = pitch & 0xFF;
+  buffer[7] = (pitch >> 8) & 0xFF;
+  buffer[8] = roll & 0xFF;
+  buffer[9] = (roll >> 8) & 0xFF;
+
+  return 10;
+}
+
+void ADXL345::write8(uint8_t reg, uint8_t value) {
   Wire.beginTransmission(addr);
   Wire.write(reg);
   Wire.write(value);
   Wire.endTransmission();
 }
 
-uint8_t IMU::read8(uint8_t reg) {
+uint8_t ADXL345::read8(uint8_t reg) {
   Wire.beginTransmission(addr);
   Wire.write(reg);
   Wire.endTransmission();
@@ -208,47 +244,69 @@ uint8_t IMU::read8(uint8_t reg) {
   return Wire.available() ? Wire.read() : 0xFF;
 }
 
+void ADXL345::readAccelData() {
+  // Read 6 bytes starting from DATAX0 register (0x32)
+  Wire.beginTransmission(addr);
+  Wire.write(0x32);  // Start at DATAX0
+  Wire.endTransmission();
+
+  Wire.requestFrom(addr, (uint8_t)6);
+  if (Wire.available() < 6) return;
+
+  // ADXL345 outputs 10-bit values (2 bytes per axis)
+  int16_t x_raw = Wire.read() | (Wire.read() << 8);
+  int16_t y_raw = Wire.read() | (Wire.read() << 8);
+  int16_t z_raw = Wire.read() | (Wire.read() << 8);
+
+  // Convert to g-forces: ±2g range, 10-bit = ±512 LSB, 4mg/LSB
+  const float scale = 0.00390625f;  // 4mg/LSB = 0.004g, but we use 1/256 for 10-bit
+
+  accelX = x_raw * scale;
+  accelY = y_raw * scale;
+  accelZ = z_raw * scale;
+}
+
 // ============================================================================
 // Sensor Manager Implementation
 // ============================================================================
 
 void SensorManager::init() {
   radar.init();
-  
+
-  if (imu.init()) {
+  if (adxl.init()) {
-    Serial.println("[Sensors] IMU initialized");
+    Serial.println("[Sensors] ADXL345 initialized");
   } else {
-    Serial.println("[Sensors] IMU not detected");
+    Serial.println("[Sensors] ADXL345 not detected");
   }
-  
+
   Serial.println("[Sensors] Radar initialized");
 }
 
 void SensorManager::update() {
   // Update sensors
   radar.update();
-  if (imu.isReady()) {
+  if (adxl.isReady()) {
-    imu.update();
+    adxl.update();
   }
-  
+
   // Handle streaming
   unsigned long now = millis();
-  
+
-  if (imuStreamEnabled && imu.isReady() && (now - lastIMUSend >= imuInterval)) {
+  if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) {
-    sendIMUPacket();
+    sendADXLPacket();
-    lastIMUSend = now;
+    lastADXLSend = now;
   }
-  
+
   if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
     sendRadarPacket();
     lastRadarSend = now;
   }
 }
 
-void SensorManager::enableIMUStream(bool enable, uint16_t intervalMs) {
+void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
-  imuStreamEnabled = enable;
+  adxlStreamEnabled = enable;
-  imuInterval = intervalMs;
+  adxlInterval = intervalMs;
-  lastIMUSend = millis();
+  lastADXLSend = millis();
 }
 
 void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
@@ -257,10 +315,10 @@ void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
   lastRadarSend = millis();
 }
 
-void SensorManager::sendIMUPacket() {
+void SensorManager::sendADXLPacket() {
-  uint8_t payload[6];
+  uint8_t payload[32];  // Buffer sized for current/future payload expansion
-  uint16_t len = imu.packPayload(payload);
+  uint16_t len = adxl.packPayload(payload);
-  sendPacket(Tag::IMU, payload, len);
+  sendPacket(Tag::IMU, payload, len);  // Reuse IMU tag for compatibility
 }
 
 void SensorManager::sendRadarPacket() {

sensors.h

@@ -53,33 +53,41 @@ private:
 };
 
 // ============================================================================
-// IMU - BNO055 9-DOF Sensor
+// IMU - ADXL345 3-axis Accelerometer
 // ============================================================================
 
-class IMU {
+class ADXL345 {
 public:
-  IMU(uint8_t addr = 0x29);
+  ADXL345(uint8_t addr = 0x53);
-  
+
   bool init();
   bool update();  // Read latest values
-  
+
-  // Get euler angles in degrees
+  // Get acceleration in g-forces
-  float getHeading() const { return heading; }
+  float getAccelX() const { return accelX; }
-  float getRoll() const { return roll; }
+  float getAccelY() const { return accelY; }
-  float getPitch() const { return pitch; }
+  float getAccelZ() const { return accelZ; }
-  
+
+  // Get tilt angles in degrees (approximated from gravity)
+  float getPitch() const;  // Pitch from Y/Z acceleration (Y=front/back)
+  float getRoll() const;   // Roll from X/Z acceleration (X=left/right)
+
+  // Get Euler angles (pitch and roll only, no heading without magnetometer)
+  void getEulerAngles(float& pitch, float& roll) const;
+
   bool isReady() const { return ready; }
-  
+
-  // Pack into payload buffer (6 bytes: h, r, p as int16 * 100)
+  // Pack into payload buffer (10 bytes: x,y,z accel + pitch,roll angles)
   uint16_t packPayload(uint8_t* buffer) const;
-  
+
 private:
   uint8_t addr;
   bool ready = false;
-  float heading = 0, roll = 0, pitch = 0;
+  float accelX = 0, accelY = 0, accelZ = 0;
-  
+
   void write8(uint8_t reg, uint8_t value);
   uint8_t read8(uint8_t reg);
+  void readAccelData();
 };
 
 // ============================================================================
@@ -87,7 +95,7 @@ private:
 // ============================================================================
 
 extern Radar radar;
-extern IMU imu;
+extern ADXL345 adxl;
 
 // ============================================================================
 // Sensor Manager
@@ -97,23 +105,23 @@ class SensorManager {
 public:
   void init();
   void update();
-  
+
   // Streaming control
-  void enableIMUStream(bool enable, uint16_t intervalMs = 100);
+  void enableADXLStream(bool enable, uint16_t intervalMs = 100);
   void enableRadarStream(bool enable, uint16_t intervalMs = 100);
-  
+
-  bool isIMUStreamEnabled() const { return imuStreamEnabled; }
+  bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
   bool isRadarStreamEnabled() const { return radarStreamEnabled; }
-  
+
 private:
-  bool imuStreamEnabled = true;
+  bool adxlStreamEnabled = true;
-  bool radarStreamEnabled = true;
+  bool radarStreamEnabled = false;
-  uint16_t imuInterval = 10;
+  uint16_t adxlInterval = 10;
   uint16_t radarInterval = 10;
-  unsigned long lastIMUSend = 0;
+  unsigned long lastADXLSend = 0;
   unsigned long lastRadarSend = 0;
-  
+
-  void sendIMUPacket();
+  void sendADXLPacket();
   void sendRadarPacket();
 };