Control Messages

use horus::prelude::*; // Provides control::MotorCommand;

// Velocity control
let mut vel_cmd = MotorCommand::velocity(0, 1.5);  // motor_id=0, 1.5 rad/s

// Position control with max velocity
let pos_cmd = MotorCommand::position(0, 3.14, 2.0);  // motor_id=0, 3.14 rad, max 2 rad/s

// Stop motor
let stop_cmd = MotorCommand::stop(0);

// Check validity — is_valid() requires target, max_velocity, max_acceleration and
// feed_forward to all be finite. velocity()/position()/stop() leave the limits at
// f64::INFINITY, so set finite limits before validating.
vel_cmd.max_velocity = 10.0;
vel_cmd.max_acceleration = 5.0;
if vel_cmd.is_valid() {
    println!("Target: {:.2}", vel_cmd.target);
}

use horus::prelude::*; // Provides control::DifferentialDriveCommand;

// Direct wheel velocities
let cmd = DifferentialDriveCommand::new(1.0, 1.2);  // left=1.0, right=1.2 rad/s

// From linear and angular velocities
let wheel_base = 0.5;   // 50cm between wheels
let wheel_radius = 0.1; // 10cm wheels
let cmd = DifferentialDriveCommand::from_twist(
    0.5,  // 0.5 m/s forward
    0.2,  // 0.2 rad/s rotation
    wheel_base,
    wheel_radius
);

// Stop
let stop = DifferentialDriveCommand::stop();

use horus::prelude::*; // Provides control::ServoCommand;

// Position command (radians)
let cmd = ServoCommand::new(0, 1.57);  // servo_id=0, 90 degrees

// With specific speed (0-1)
let cmd = ServoCommand::with_speed(0, 1.57, 0.3);  // 30% speed

// From degrees
let cmd = ServoCommand::from_degrees(0, 90.0);

// Disable servo (release torque)
let disable = ServoCommand::disable(0);

use horus::prelude::*; // Provides control::PidConfig;

// Full PID
let pid = PidConfig::new(1.0, 0.1, 0.05);  // Kp, Ki, Kd

// P-only controller
let p_only = PidConfig::proportional(2.0);

// PI controller
let pi = PidConfig::pi(1.5, 0.2);

// PD controller
let pd = PidConfig::pd(1.0, 0.1);

// With limits
let pid_limited = PidConfig::new(1.0, 0.1, 0.05)
    .with_limits(10.0, 100.0);  // integral_limit, output_limit

// Validation — is_valid() requires finite integral_limit/output_limit, so it
// only passes after with_limits(); PidConfig::new() leaves both at f64::INFINITY.
if pid_limited.is_valid() {
    println!("Kp={}, Ki={}, Kd={}", pid_limited.kp, pid_limited.ki, pid_limited.kd);
}

use horus::prelude::*; // Provides control::TrajectoryPoint;

// Simple 2D trajectory point
let point = TrajectoryPoint::new_2d(
    1.0, 2.0,   // x, y position
    0.5, 0.3,   // vx, vy velocity
    1.5         // time from start (seconds)
);

// Stationary point (x, y, z)
let waypoint = TrajectoryPoint::stationary(1.0, 2.0, 0.0);

use horus::prelude::*; // Provides control::JointCommand;

let mut cmd = JointCommand::new();

// These return `Result<(), &'static str>` — the error is the 16-joint capacity
// limit, not a HorusError, so `?` alone will not compile inside a function
// returning `Result<T>`. Convert it:
cmd.add_position("shoulder", 0.5).map_err(|e| Error::node("arm", e))?;
cmd.add_position("elbow", 1.0).map_err(|e| Error::node("arm", e))?;
cmd.add_position("wrist", -0.3).map_err(|e| Error::node("arm", e))?;

cmd.add_velocity("gripper", 0.2).map_err(|e| Error::node("arm", e))?;

use horus::prelude::*;

struct MotorDriverNode {
    cmd_sub: Topic<CmdVel>,
    left_pub: Topic<MotorCommand>,
    right_pub: Topic<MotorCommand>,
    wheel_base: f64,
    wheel_radius: f64,
    max_speed: f64,
}

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

    fn tick(&mut self) {
        if let Some(cmd) = self.cmd_sub.recv() {
            // Convert CmdVel to differential drive
            let diff = DifferentialDriveCommand::from_twist(
                cmd.linear as f64,
                cmd.angular as f64,
                self.wheel_base,
                self.wheel_radius
            );

            // Clamp wheel velocities to the drive limit (rad/s)
            let left = diff.left_velocity.clamp(-self.max_speed, self.max_speed);
            let right = diff.right_velocity.clamp(-self.max_speed, self.max_speed);

            // Send per-motor velocity commands
            self.left_pub.send(MotorCommand::velocity(0, left));
            self.right_pub.send(MotorCommand::velocity(1, right));
        }
    }
}