Tutorial 6: Write a Reusable Driver

// simplified
// src/main.rs
use horus::prelude::*;
use horus::hardware::{self, NodeParams};
use horus::register_driver;
use std::io::{Read, Write};
use std::time::Duration;

/// Serial motor controller driver.
///
/// Communicates with a motor controller over UART using a simple
/// binary protocol: [0xFF, motor_id, position_hi, position_lo, checksum].
struct SerialMotorDriver {
    port: Box<dyn serialport::SerialPort>,
    motor_ids: Vec<u8>,
    state_pub: Topic<JointState>,
    cmd_sub: Topic<JointCommand>,
}

impl SerialMotorDriver {
    fn from_params(params: &NodeParams) -> Result<Self> {
        let port_name: String = params.get("port")?;
        let baudrate: u32 = params.get_or("baudrate", 115200);
        let motor_ids: Vec<u8> = params.get_or("motor_ids", vec![1]);
        let topic_prefix: String = params.get_or("topic_prefix", "motor".to_string());

        let port = serialport::new(&port_name, baudrate)
            .timeout(Duration::from_millis(10))
            .open()
            .map_err(|e| Error::from(ConfigError::Other(
                format!("Failed to open {}: {}", port_name, e)
            )))?;

        Ok(Self {
            port,
            motor_ids,
            state_pub: Topic::new(&format!("{}.joint_state", topic_prefix))?,
            cmd_sub: Topic::new(&format!("{}.joint_cmd", topic_prefix))?,
        })
    }

    /// Read current positions from the controller.
    fn read_positions(&mut self) -> Vec<f64> {
        let mut positions = Vec::with_capacity(self.motor_ids.len());

        for &id in &self.motor_ids {
            // Send query: [0xFF, id, 0x00, 0x00, checksum]
            let checksum = 0xFF ^ id;
            let cmd = [0xFF, id, 0x00, 0x00, checksum];
            if self.port.write_all(&cmd).is_err() {
                positions.push(0.0);
                continue;
            }

            // Read response: [0xFF, id, pos_hi, pos_lo, checksum]
            let mut buf = [0u8; 5];
            if self.port.read_exact(&mut buf).is_ok() && buf[0] == 0xFF && buf[1] == id {
                let raw = ((buf[2] as u16) << 8) | buf[3] as u16;
                // Convert raw position (0-4095) to radians (0-2pi)
                positions.push(raw as f64 / 4095.0 * std::f64::consts::TAU);
            } else {
                positions.push(0.0);
            }
        }

        positions
    }

    /// Send position commands to the controller.
    fn write_positions(&mut self, positions: &[f64]) {
        for (i, &id) in self.motor_ids.iter().enumerate() {
            if let Some(&pos) = positions.get(i) {
                // Convert radians to raw (0-4095)
                let raw = ((pos / std::f64::consts::TAU) * 4095.0).clamp(0.0, 4095.0) as u16;
                let hi = (raw >> 8) as u8;
                let lo = (raw & 0xFF) as u8;
                let checksum = 0xFF ^ id ^ hi ^ lo;
                let cmd = [0xFF, id, hi, lo, checksum];
                let _ = self.port.write_all(&cmd);
            }
        }
    }
}

// simplified
impl Node for SerialMotorDriver {
    fn name(&self) -> &str { "SerialMotorDriver" }

    fn init(&mut self) -> Result<()> {
        // Verify communication by reading positions once
        let positions = self.read_positions();
        log::info!(
            "SerialMotorDriver initialized: {} motors, positions: {:?}",
            self.motor_ids.len(),
            positions
        );
        Ok(())
    }

    fn tick(&mut self) {
        // Read current state and publish
        let positions = self.read_positions();
        self.state_pub.send(JointState {
            name: self.motor_ids.iter().map(|id| format!("motor_{}", id)).collect(),
            position: positions,
            ..Default::default()
        });

        // Apply commands if any
        if let Some(cmd) = self.cmd_sub.recv() {
            self.write_positions(&cmd.position);
        }
    }

    fn shutdown(&mut self) -> Result<()> {
        log::info!("SerialMotorDriver shutting down — stopping all motors");
        // Send zero velocity to all motors
        let zeros = vec![0.0; self.motor_ids.len()];
        self.write_positions(&zeros);
        Ok(())
    }

    fn enter_safe_state(&mut self) {
        // Emergency: stop all motors immediately
        let zeros = vec![0.0; self.motor_ids.len()];
        self.write_positions(&zeros);
    }

    fn is_safe_state(&self) -> bool {
        true // Motors stopped
    }
}

// Register so hardware::load() can instantiate this driver from horus.toml
register_driver!(SerialMotorDriver, SerialMotorDriver::from_params);

// simplified
fn main() -> Result<()> {
    env_logger::init();

    // hardware::load() returns Vec<(String, Box<dyn Node>)>
    // Each entry is a ready-to-use node from [hardware] config
    let nodes = hardware::load()?;

    let mut sched = Scheduler::new()
        .tick_rate(100_u64.hz());

    for (name, node) in nodes {
        sched = sched
            .add(node)
                .rate(100_u64.hz())
                .on_miss(Miss::SafeMode)
                .build()?;
    }

    sched.build()?.run()
}