Tutorial 2: Build a Motor Controller

horus new motor-demo -r
cd motor-demo

use horus::prelude::*;

/// Velocity command sent to the motor.
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct MotorCommand {
    velocity: f32,    // Desired velocity (rad/s)
    max_torque: f32,  // Torque limit (N*m)
}

/// Motor state feedback.
#[derive(Debug, Clone, Copy, Default, Serialize, Deserialize, LogSummary)]
#[repr(C)]
struct MotorState {
    position: f32,    // Current position (radians)
    velocity: f32,    // Current velocity (rad/s)
    torque: f32,      // Applied torque (N*m)
    timestamp: f64,
}

struct MotorController {
    commands: Topic<MotorCommand>,     // Subscribe to commands
    state_pub: Topic<MotorState>,      // Publish state
    position: f32,                      // Accumulated position
    velocity: f32,                      // Current velocity
    dt: f32,                            // Time step (set from rate)
}

impl MotorController {
    fn new(rate_hz: f32) -> Result<Self> {
        Ok(Self {
            commands: Topic::new("motor.command")?,
            state_pub: Topic::new("motor.state")?,
            position: 0.0,
            velocity: 0.0,
            dt: 1.0 / rate_hz,
        })
    }
}

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

    fn tick(&mut self) {
        // IMPORTANT: call recv() every tick to consume the latest command.
        // Even if no command arrives, the motor continues integrating position.
        if let Some(cmd) = self.commands.recv() {
            // Simple velocity tracking with torque limit
            let error = cmd.velocity - self.velocity;
            let torque = error.clamp(-cmd.max_torque, cmd.max_torque);
            self.velocity += torque * self.dt;
        }

        // Integrate velocity → position
        self.position += self.velocity * self.dt;

        // Publish current state
        self.state_pub.send(MotorState {
            position: self.position,
            velocity: self.velocity,
            torque: 0.0,
            timestamp: 0.0,  // Simplified for tutorial
        });
    }

    // SAFETY: shutdown() is CRITICAL for actuator nodes. Without it, the motor
    // could continue running at its last commanded velocity after the scheduler stops.
    // Always zero velocity and publish the stopped state before returning.
    fn shutdown(&mut self) -> Result<()> {
        // CRITICAL: zero velocity before publishing — order matters.
        self.velocity = 0.0;
        self.state_pub.send(MotorState {
            position: self.position,
            velocity: 0.0,
            torque: 0.0,
            timestamp: 0.0,
        });
        eprintln!("Motor stopped safely at position {:.2} rad", self.position);
        Ok(())
    }
}

struct Commander {
    publisher: Topic<MotorCommand>,
    tick_count: u64,
}

impl Commander {
    fn new() -> Result<Self> {
        Ok(Self {
            publisher: Topic::new("motor.command")?,
            tick_count: 0,
        })
    }
}

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

    fn tick(&mut self) {
        let t = self.tick_count as f64 * 0.01; // At 100Hz

        // Send a sine wave velocity command
        let velocity = 1.0 * (t * 0.5).sin() as f32;

        self.publisher.send(MotorCommand {
            velocity,
            max_torque: 10.0,
        });

        self.tick_count += 1;
    }

    // SAFETY: shutdown() sends a zero-velocity command so the motor stops
    // even if the motor controller's own shutdown() hasn't run yet.
    fn shutdown(&mut self) -> Result<()> {
        self.publisher.send(MotorCommand { velocity: 0.0, max_torque: 0.0 });
        eprintln!("Commander shutting down, sent zero-velocity command.");
        Ok(())
    }
}

struct StateDisplay {
    subscriber: Topic<MotorState>,
    sample_count: u64,
}

impl StateDisplay {
    fn new() -> Result<Self> {
        Ok(Self {
            subscriber: Topic::new("motor.state")?,
            sample_count: 0,
        })
    }
}

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

    fn tick(&mut self) {
        // IMPORTANT: call recv() every tick to drain the topic buffer.
        if let Some(state) = self.subscriber.recv() {
            self.sample_count += 1;
            if self.sample_count % 50 == 0 {
                println!(
                    "pos={:.2} rad  vel={:.2} rad/s",
                    state.position, state.velocity,
                );
            }
        }
    }

    // SAFETY: shutdown() logs final display state for diagnostics.
    fn shutdown(&mut self) -> Result<()> {
        eprintln!("StateDisplay shutting down after {} samples", self.sample_count);
        Ok(())
    }
}

fn main() -> Result<()> {
    eprintln!("Motor controller demo starting...\n");

    let rate = 100.0; // Hz
    let mut scheduler = Scheduler::new();

    // Execution order: Commander (0) → MotorController (1) → StateDisplay (2).
    // Commander must publish before motor reads; motor must publish before display reads.

    scheduler.add(Commander::new()?)
        .order(0)
        // NOTE: .rate(100.hz()) triggers auto-RT detection at finalize().
        .rate(100.hz())
        .build()?;

    scheduler.add(MotorController::new(rate)?)
        .order(1)
        // NOTE: .rate(100.hz()) triggers auto-RT detection at finalize().
        .rate(100.hz())
        .build()?;

    scheduler.add(StateDisplay::new()?)
        .order(2)
        .build()?;

    scheduler.run()
}

horus run