fix bindings + logging

actuator/bindings/src/lib.rs

@@ -19,6 +19,12 @@ impl From<eyre::Report> for ErrReportWrapper {
     }
 }
 
+impl From<PyErr> for ErrReportWrapper {
+    fn from(err: PyErr) -> Self {
+        ErrReportWrapper(err.into())
+    }
+}
+
 impl From<ErrReportWrapper> for PyErr {
     fn from(err: ErrReportWrapper) -> PyErr {
         PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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<PyRobstrideActuatorConfig> for robstride::ActuatorConfiguration {
 }
 
 #[pymodule]
-fn robstride_bindings(m: &Bound<PyModule>) -> PyResult<()> {
+fn bindings(m: &Bound<PyModule>) -> PyResult<()> {
     m.add_function(wrap_pyfunction!(get_version, m)?)?;
     m.add_class::<PyRobstrideSupervisor>()?;
     m.add_class::<PyRobstrideActuatorCommand>()?;

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?,
             )),

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()