diff --git a/HansonServo.ino b/HansonServo.ino index b3f3f52..e23673d 100644 --- a/HansonServo.ino +++ b/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; diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index da4209c..d9b3dd3 100644 --- a/PROTOCOL_MIGRATION.md +++ b/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 -[roll: 2 bytes LE, signed] // degrees × 100 +[accelX: 2 bytes LE, signed] // g-forces × 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:** ``` diff --git a/nodegraph.cpp b/nodegraph.cpp index 5ffe763..fff1b14 100644 --- a/nodegraph.cpp +++ b/nodegraph.cpp @@ -6,7 +6,7 @@ #include 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 - case VAR_IMU_HEADING: - // Heading: 0-360° → 0-4095 - if (imu.isReady()) { - outputValue = (uint16_t)((imu.getHeading() / 360.0f) * 4095.0f); + // ADXL345 accelerometer sources - scale to 0-4095 range + case VAR_ACCEL_X: + // X acceleration: -2g to +2g → 0-4095 (2048 = 0g) + if (adxl.isReady()) { + 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: - // Pitch: -90° to +90° → 0-4095 (2048 = level) - if (imu.isReady()) { - float pitch = constrain(imu.getPitch(), -90.0f, 90.0f); + case VAR_ACCEL_Y: + // Y acceleration: -2g to +2g → 0-4095 (2048 = 0g) + if (adxl.isReady()) { + 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: - // Roll: -180° to +180° → 0-4095 (2048 = level) - if (imu.isReady()) { - float roll = constrain(imu.getRoll(), -180.0f, 180.0f); - outputValue = (uint16_t)(((roll + 180.0f) / 360.0f) * 4095.0f); + case VAR_TILT_ROLL: + // Roll angle: -90° to +90° → 0-4095 (2048 = level) + if (adxl.isReady()) { + float roll = constrain(adxl.getRoll(), -90.0f, 90.0f); + outputValue = (uint16_t)(((roll + 90.0f) / 180.0f) * 4095.0f); } else { outputValue = 2048; } diff --git a/nodegraph.h b/nodegraph.h index 4870bae..aa3b1d3 100644 --- a/nodegraph.h +++ b/nodegraph.h @@ -20,10 +20,12 @@ enum VariableSource { VAR_SINE, VAR_ANALOG, VAR_SERVO_FEEDBACK, - // IMU sources (BNO055) - VAR_IMU_HEADING, // 0-360 degrees → 0-4095 - VAR_IMU_PITCH, // -90 to +90 degrees → 0-4095 (2048 = level) - VAR_IMU_ROLL, // -180 to +180 degrees → 0-4095 (2048 = level) + // ADXL345 accelerometer sources + VAR_ACCEL_X, // X acceleration → 0-4095 (-2g to +2g, 2048 = 0g) + VAR_ACCEL_Y, // Y acceleration → 0-4095 (-2g to +2g, 2048 = 0g) + 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) diff --git a/protocol.h b/protocol.h index 48338b1..c5aed65 100644 --- a/protocol.h +++ b/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) diff --git a/sensors.cpp b/sensors.cpp index 3f7d3c0..677e04f 100644 --- a/sensors.cpp +++ b/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 - if (id != 0xA0) { + + uint8_t id = read8(0x00); // DEVID register (should be 0xE5) + if (id != 0xE5) { ready = false; return false; } - - // Enter config mode - write8(0x3D, 0x00); - delay(25); - - // Set NDOF fusion mode - write8(0x3D, 0x0C); - delay(25); - + + // Enable measurement mode + write8(0x2D, 0x08); // POWER_CTL: measure mode + ready = true; return true; } -bool IMU::update() { +bool ADXL345::update() { if (!ready) return false; - - Wire.beginTransmission(addr); - 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; - + + readAccelData(); return true; } -uint16_t IMU::packPayload(uint8_t* buffer) const { - // Format: heading(2, unsigned) + roll(2, signed) + pitch(2, signed), all *100 - uint16_t h = (uint16_t)(heading * 100.0f); // 0..36000 fits in uint16 - int16_t r = (int16_t)(roll * 100.0f); - int16_t p = (int16_t)(pitch * 100.0f); - - 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; +float ADXL345::getPitch() const { + // Approximate pitch from Y/Z acceleration (Y = front/back axis) + // This is not as accurate as a proper IMU with gyroscope + if (accelZ == 0) return 0; + return atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * 180.0f / PI; } -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()) { - Serial.println("[Sensors] IMU initialized"); + + if (adxl.init()) { + 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()) { - imu.update(); + if (adxl.isReady()) { + adxl.update(); } - + // Handle streaming unsigned long now = millis(); - - if (imuStreamEnabled && imu.isReady() && (now - lastIMUSend >= imuInterval)) { - sendIMUPacket(); - lastIMUSend = now; + + if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) { + sendADXLPacket(); + lastADXLSend = now; } - + if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) { sendRadarPacket(); lastRadarSend = now; } } -void SensorManager::enableIMUStream(bool enable, uint16_t intervalMs) { - imuStreamEnabled = enable; - imuInterval = intervalMs; - lastIMUSend = millis(); +void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) { + adxlStreamEnabled = enable; + adxlInterval = intervalMs; + 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() { - uint8_t payload[6]; - uint16_t len = imu.packPayload(payload); - sendPacket(Tag::IMU, payload, len); +void SensorManager::sendADXLPacket() { + uint8_t payload[32]; // Buffer sized for current/future payload expansion + uint16_t len = adxl.packPayload(payload); + sendPacket(Tag::IMU, payload, len); // Reuse IMU tag for compatibility } void SensorManager::sendRadarPacket() { diff --git a/sensors.h b/sensors.h index 0d99d3f..d45fdbf 100644 --- a/sensors.h +++ b/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 - float getHeading() const { return heading; } - float getRoll() const { return roll; } - float getPitch() const { return pitch; } - + + // Get acceleration in g-forces + float getAccelX() const { return accelX; } + float getAccelY() const { return accelY; } + 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 radarStreamEnabled = true; - uint16_t imuInterval = 10; + bool adxlStreamEnabled = true; + bool radarStreamEnabled = false; + 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(); };