Control Messages

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

// Velocity control
let 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
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
if pid.is_valid() {
    println!("Kp={}, Ki={}, Kd={}", pid.kp, pid.ki, pid.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();

// Add position commands
cmd.add_position("shoulder", 0.5)?;
cmd.add_position("elbow", 1.0)?;
cmd.add_position("wrist", -0.3)?;

// Add velocity commands
cmd.add_velocity("gripper", 0.2)?;

use horus::prelude::*;

struct MotorDriverNode {
    cmd_sub: Topic<CmdVel>,
    left_motor_pub: Topic<MotorCommand>,
    right_motor_pub: Topic<MotorCommand>,
    wheel_base: f64,
    wheel_radius: 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
            );

            // Send motor velocity commands
            self.left_motor_pub.send(MotorCommand::velocity(0, diff.left_velocity));
            self.right_motor_pub.send(MotorCommand::velocity(1, diff.right_velocity));
        }
    }
}