Real Hardware

[package]
name = "imu-mpu6050"
version = "0.1.0"

[dependencies]
i2cdev = { version = "0.6", source = "crates.io" }

use horus::prelude::*;
use i2cdev::core::I2CDevice;
use i2cdev::linux::LinuxI2CDevice;

const MPU6050_ADDR: u16 = 0x68;
const ACCEL_XOUT_H: u8 = 0x3B;
const PWR_MGMT_1: u8 = 0x6B;
const ACCEL_SCALE: f64 = 16384.0; // ±2g
const GYRO_SCALE: f64 = 131.0;    // ±250°/s

struct Mpu6050Node {
    i2c: LinuxI2CDevice,
    imu_pub: Topic<Imu>,
}

impl Mpu6050Node {
    fn new(bus: &str) -> Result<Self> {
        let mut i2c = LinuxI2CDevice::new(bus, MPU6050_ADDR)
            .map_err(|e| Error::from(ConfigError::Other(
                format!("failed to open {}: {}", bus, e)
            )))?;

        // Wake the sensor (it starts in sleep mode)
        i2c.smbus_write_byte_data(PWR_MGMT_1, 0x00)
            .map_err(|e| Error::from(ConfigError::Other(
                format!("failed to wake MPU6050: {}", e)
            )))?;

        Ok(Self {
            i2c,
            imu_pub: Topic::new("imu.raw")?,
        })
    }

    fn read_sensor(&mut self) -> Option<Imu> {
        let mut buf = [0u8; 14];
        // Read 14 bytes starting at ACCEL_XOUT_H
        // (accel XYZ 6 bytes + temp 2 bytes + gyro XYZ 6 bytes)
        if self.i2c.smbus_write_byte(ACCEL_XOUT_H).is_err() {
            return None;
        }
        if self.i2c.read(&mut buf).is_err() {
            return None;
        }

        let ax = i16::from_be_bytes([buf[0], buf[1]]) as f64 / ACCEL_SCALE * 9.81;
        let ay = i16::from_be_bytes([buf[2], buf[3]]) as f64 / ACCEL_SCALE * 9.81;
        let az = i16::from_be_bytes([buf[4], buf[5]]) as f64 / ACCEL_SCALE * 9.81;
        let gx = i16::from_be_bytes([buf[8], buf[9]]) as f64 / GYRO_SCALE;
        let gy = i16::from_be_bytes([buf[10], buf[11]]) as f64 / GYRO_SCALE;
        let gz = i16::from_be_bytes([buf[12], buf[13]]) as f64 / GYRO_SCALE;

        let mut imu = Imu::new();
        imu.linear_acceleration = [ax, ay, az];
        imu.angular_velocity = [gx.to_radians(), gy.to_radians(), gz.to_radians()];
        Some(imu)
    }
}

impl Node for Mpu6050Node {
    fn name(&self) -> &str { "Mpu6050" }

    fn tick(&mut self) {
        if let Some(imu) = self.read_sensor() {
            if imu.is_valid() {
                self.imu_pub.send(imu);
            }
        }
    }
}

fn main() -> Result<()> {
    let mut scheduler = Scheduler::new();

    scheduler.add(Mpu6050Node::new("/dev/i2c-1")?)
        .order(0)
        .rate(100_u64.hz())
        .on_miss(Miss::Skip)
        .build()?;

    scheduler.run()
}

[package]
name = "serial-motors"
version = "0.1.0"

[dependencies]
serialport = { version = "4.7", source = "crates.io" }

use horus::prelude::*;
use std::io::Write;
use std::time::Duration;

const WHEEL_BASE: f32 = 0.3;  // meters between wheels
const MAX_RPM: f32 = 200.0;

struct MotorNode {
    port: Box<dyn serialport::SerialPort>,
    cmd_sub: Topic<CmdVel>,
    odom_pub: Topic<Odometry>,
    x: f64,
    y: f64,
    theta: f64,
}

impl MotorNode {
    fn new(port_name: &str, baud: u32) -> Result<Self> {
        let port = serialport::new(port_name, baud)
            .timeout(Duration::from_millis(10))
            .open()
            .map_err(|e| Error::from(ConfigError::Other(
                format!("failed to open {}: {}", port_name, e)
            )))?;

        Ok(Self {
            port,
            cmd_sub: Topic::new("cmd_vel")?,
            odom_pub: Topic::new("odom")?,
            x: 0.0,
            y: 0.0,
            theta: 0.0,
        })
    }

    fn send_motor_command(&mut self, left_rpm: f32, right_rpm: f32) {
        // Protocol: "L<rpm>,R<rpm>\n" — adapt to your motor controller
        let cmd = format!("L{:.1},R{:.1}\n", left_rpm, right_rpm);
        let _ = self.port.write_all(cmd.as_bytes());
    }
}

impl Node for MotorNode {
    fn name(&self) -> &str { "Motors" }

    fn tick(&mut self) {
        let dt = 1.0 / 50.0_f32; // 50 Hz control loop

        if let Some(vel) = self.cmd_sub.recv() {
            // Differential drive kinematics
            let v_left = vel.linear - vel.angular * WHEEL_BASE / 2.0;
            let v_right = vel.linear + vel.angular * WHEEL_BASE / 2.0;

            // Convert m/s to RPM (assuming 0.05m wheel radius)
            let left_rpm = (v_left / 0.05 * 60.0 / (2.0 * std::f32::consts::PI))
                .clamp(-MAX_RPM, MAX_RPM);
            let right_rpm = (v_right / 0.05 * 60.0 / (2.0 * std::f32::consts::PI))
                .clamp(-MAX_RPM, MAX_RPM);

            self.send_motor_command(left_rpm, right_rpm);

            // Dead-reckoning odometry
            let dt = dt as f64;
            self.theta += vel.angular as f64 * dt;
            self.x += vel.linear as f64 * dt * self.theta.cos();
            self.y += vel.linear as f64 * dt * self.theta.sin();
        }

        let mut odom = Odometry::default();
        odom.pose = Pose2D {
            x: self.x,
            y: self.y,
            theta: self.theta,
            ..Default::default()
        };
        self.odom_pub.send(odom);
    }

    fn enter_safe_state(&mut self) {
        // SAFETY: stop both motors immediately
        self.send_motor_command(0.0, 0.0);
    }

    fn shutdown(&mut self) -> Result<()> {
        self.send_motor_command(0.0, 0.0);
        Ok(())
    }
}

fn main() -> Result<()> {
    let mut scheduler = Scheduler::new();

    scheduler.add(MotorNode::new("/dev/ttyUSB0", 115200)?)
        .order(10)
        .rate(50_u64.hz())
        .on_miss(Miss::SafeMode)
        .build()?;

    scheduler.run()
}

fn main() -> Result<()> {
    let mut scheduler = Scheduler::new()
        .tick_rate(100_u64.hz());

    // Sensor runs first — read IMU
    scheduler.add(Mpu6050Node::new("/dev/i2c-1")?)
        .order(0)
        .rate(100_u64.hz())
        .on_miss(Miss::Skip)
        .build()?;

    // Actuator runs after — write motors
    scheduler.add(MotorNode::new("/dev/ttyUSB0", 115200)?)
        .order(10)
        .rate(50_u64.hz())
        .on_miss(Miss::SafeMode)
        .build()?;

    scheduler.run()
}