diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 4c54a13..cd3727f 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -19,6 +19,12 @@ impl From for ErrReportWrapper { } } +impl From for ErrReportWrapper { + fn from(err: PyErr) -> Self { + ErrReportWrapper(err.into()) + } +} + impl From for PyErr { fn from(err: ErrReportWrapper) -> PyErr { PyErr::new::(err.0.to_string()) @@ -163,10 +169,17 @@ impl PyRobstrideSupervisor { let rt = Runtime::new().map_err(|e| ErrReportWrapper(e.into()))?; let supervisor = rt.block_on(async { - let mut supervisor = - Supervisor::new(Duration::from_secs(1)).map_err(|e| ErrReportWrapper(e))?; + let mut supervisor = Supervisor::new(Duration::from_secs(1)) + .map_err(|e| ErrReportWrapper(e))?; + let mut found_motors = vec![false; actuators_config.len()]; + // Add transports for port in &ports { + Python::with_gil(|py| { + py.run_bound(&format!("print('Adding transport for port: {}')", port), None, None)?; + Ok::<_, PyErr>(()) + })?; + if port.starts_with("/dev/tty") { let serial = CH341Transport::new(port.clone()) .await @@ -188,19 +201,63 @@ impl PyRobstrideSupervisor { } } - // Scan for motors + // Start supervisor runner + let mut supervisor_runner = supervisor.clone_controller(); + Python::with_gil(|py| { + py.run_bound("print('Starting supervisor runner...')", None, None)?; + Ok::<_, PyErr>(()) + })?; + + tokio::spawn(async move { + if let Err(e) = supervisor_runner.run(Duration::from_secs_f64(polling_interval)).await { + let _ = Python::with_gil(|py| { + py.run_bound(&format!("print('ERROR: Supervisor task failed: {}')", e), None, None) + }); + } + }); + + // Scan for motors on each port for port in &ports { + Python::with_gil(|py| { + py.run_bound(&format!("print('Scanning for motors on port: {}')", port), None, None)?; + Ok::<_, PyErr>(()) + })?; + let discovered_ids = supervisor .scan_bus(0xFD, port, &actuators_config) .await .map_err(|e| ErrReportWrapper(e))?; - for (motor_id, _) in &actuators_config { - if !discovered_ids.contains(motor_id) { - tracing::warn!("Configured motor not found - ID: {}", motor_id); + + Python::with_gil(|py| { + py.run_bound(&format!("print('Discovered IDs on {}: {:?}')", port, discovered_ids), None, None)?; + Ok::<_, PyErr>(()) + })?; + + for (idx, (motor_id, _)) in actuators_config.iter().enumerate() { + if discovered_ids.contains(motor_id) { + found_motors[idx] = true; } } } + // Log warnings for missing motors + for (idx, (motor_id, config)) in actuators_config.iter().enumerate() { + if !found_motors[idx] { + Python::with_gil(|py| { + py.run_bound(&format!("print('WARNING: Configured motor not found - ID: {}, Type: {:?}')", motor_id, config.actuator_type), None, None)?; + Ok::<_, PyErr>(()) + })?; + } else { + Python::with_gil(|py| { + py.run_bound(&format!( + "print('Found configured motor - ID: {}, Type: {:?}')", + motor_id, config.actuator_type + ), None, None)?; + Ok::<_, PyErr>(()) + })?; + } + } + Ok(supervisor) })?; @@ -326,7 +383,7 @@ impl From for robstride::ActuatorConfiguration { } #[pymodule] -fn robstride_bindings(m: &Bound) -> PyResult<()> { +fn bindings(m: &Bound) -> PyResult<()> { m.add_function(wrap_pyfunction!(get_version, m)?)?; m.add_class::()?; m.add_class::()?; diff --git a/actuator/robstride/src/main.rs b/actuator/robstride/src/main.rs index b8e26bf..1e17c42 100644 --- a/actuator/robstride/src/main.rs +++ b/actuator/robstride/src/main.rs @@ -56,7 +56,7 @@ async fn main() -> eyre::Result<()> { supervisor .add_actuator( Box::new(RobStride02::new( - 1, + 14, 0xFE, supervisor.get_transport_tx("stub").await?, )), diff --git a/examples/supervisor.py b/examples/supervisor.py index da7efba..df5c9ec 100644 --- a/examples/supervisor.py +++ b/examples/supervisor.py @@ -1,36 +1,71 @@ """Example of moving a motor using the supervisor.""" import argparse +import logging import time -from actuator import RobstrideActuatorConfig, RobstrideSupervisor +from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand +def setup_logging() -> None: + """Set up logging configuration.""" + logging.basicConfig( + level=logging.INFO, + format='%(asctime)s - %(levelname)s - %(message)s', + datefmt='%Y-%m-%d %H:%M:%S' + ) def main() -> None: + setup_logging() + logger = logging.getLogger(__name__) + parser = argparse.ArgumentParser() - parser.add_argument("--port-name", type=str, default="/dev/ttyCH341USB0") - parser.add_argument("--motor-id", type=int, default=1) - parser.add_argument("--motor-type", type=str, default="04") - parser.add_argument("--sleep", type=float, default=0.0) - parser.add_argument("--period", type=float, default=10.0) - parser.add_argument("--amplitude", type=float, default=1.0) + parser.add_argument( + "--port-names", + type=str, + default="can0,can1", + help="Comma-separated list of port names" + ) + parser.add_argument("--motor-id", type=int, default=23) + parser.add_argument("--motor-type", type=int, default=2) + parser.add_argument("--polling-interval", type=float, default=0.001) parser.add_argument("--verbose", action="store_true") args = parser.parse_args() - _amplitude = args.amplitude - _period = args.period + if args.verbose: + logging.getLogger().setLevel(logging.DEBUG) - supervisor = RobstrideSupervisor( - ports=[args.port_name], - py_actuators_config=[(args.motor_id, RobstrideActuatorConfig(args.motor_type))], - polling_interval=args.sleep, - ) + ports = [port.strip() for port in args.port_names.split(",")] + logger.info(f"Initializing supervisor with ports: {ports}") + + try: + supervisor = RobstrideSupervisor( + ports=ports, + py_actuators_config=[(args.motor_id, RobstrideActuatorConfig(args.motor_type))], + polling_interval=args.polling_interval, + ) + logger.info("Supervisor initialized successfully") + except Exception as e: + logger.error(f"Failed to initialize supervisor: {e}") + return - while True: - print(supervisor.get_actuators_state([args.motor_id])) - time.sleep(1) + try: + while True: + states = supervisor.get_actuators_state([args.motor_id]) + if states: + for state in states: + status = "online" if state.online else "offline" + logger.info(f"Motor {state.actuator_id}: {status}") + if state.online: + logger.info(f" Position: {state.position:.2f}°") + logger.info(f" Velocity: {state.velocity:.2f}°/s") + logger.info(f" Torque: {state.torque:.2f}") + logger.info(f" Temperature: {state.temperature:.2f}°C") + else: + logger.warning("No motor states received") + time.sleep(1) + except KeyboardInterrupt: + logger.info("\nShutting down gracefully...") if __name__ == "__main__": - # python -m examples.supervisor main()