diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index b613239..71b61f4 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = "0.1" +robstridev2 = "0.2" gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 22066f0..41a2d0d 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -22,9 +22,11 @@ impl KBotActuator { pub async fn new( _operations_service: Arc, ports: Vec<&str>, - motors_timeout: Duration, + actuator_timeout: Duration, + polling_interval: Duration, + desired_actuator_types: &[(u8, robstridev2::ActuatorType)], ) -> Result { - let mut supervisor = Supervisor::new(motors_timeout)?; + let mut supervisor = Supervisor::new(actuator_timeout)?; for port in ports.clone() { if port.starts_with("/dev/tty") { @@ -44,13 +46,15 @@ impl KBotActuator { let mut supervisor_runner = supervisor.clone_controller(); let _supervisor_handle = tokio::spawn(async move { - if let Err(e) = supervisor_runner.run().await { + if let Err(e) = supervisor_runner.run(polling_interval).await { tracing::error!("Supervisor task failed: {}", e); } }); for port in ports.clone() { - supervisor.scan_bus(0xFD, port).await?; + supervisor + .scan_bus(0xFD, port, desired_actuator_types) + .await?; } Ok(KBotActuator { @@ -73,7 +77,10 @@ impl Actuator for KBotActuator { .position .map(|p| p.to_radians() as f32) .unwrap_or(0.0), - command.velocity.map(|v| v as f32).unwrap_or(0.0), + command + .velocity + .map(|v| v.to_radians() as f32) + .unwrap_or(0.0), command.torque.map(|t| t as f32).unwrap_or(0.0), ) .await; @@ -115,6 +122,18 @@ impl Actuator for KBotActuator { } } + if let Some(zero_position) = config.zero_position { + if zero_position { + supervisor.zero(motor_id).await?; + } + } + + if let Some(new_actuator_id) = config.new_actuator_id { + supervisor + .change_id(motor_id, new_actuator_id as u8) + .await?; + } + let success = result.is_ok(); let error = result.err().map(|e| KosError { code: ErrorCode::HardwareFailure as i32, @@ -139,8 +158,8 @@ impl Actuator for KBotActuator { responses.push(ActuatorStateResponse { actuator_id: id, online: ts.elapsed().unwrap_or(Duration::from_secs(1)) < Duration::from_secs(1), - position: Some(feedback.angle as f64), - velocity: Some(feedback.velocity as f64), + position: Some(feedback.angle.to_degrees() as f64), + velocity: Some(feedback.velocity.to_degrees() as f64), torque: Some(feedback.torque as f64), temperature: Some(feedback.temperature as f64), voltage: None, diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 6c8a91b..92afb8d 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -5,13 +5,14 @@ mod process_manager; mod hexmove; pub use actuator::*; +pub use robstridev2::ActuatorType; #[cfg(target_os = "linux")] pub use hexmove::*; pub use process_manager::*; use async_trait::async_trait; -use eyre::{Result, WrapErr}; +use eyre::WrapErr; use kos_core::hal::Operation; use kos_core::kos_proto::actuator::actuator_service_server::ActuatorServiceServer; use kos_core::kos_proto::process_manager::process_manager_service_server::ProcessManagerServiceServer; @@ -19,7 +20,6 @@ use kos_core::{ services::{ActuatorServiceImpl, OperationsServiceImpl, ProcessManagerServiceImpl}, Platform, ServiceEnum, }; -use std::collections::HashMap; use std::future::Future; use std::pin::Pin; use std::sync::Arc; @@ -65,10 +65,42 @@ impl Platform for KbotPlatform { KBotProcessManager::new(self.name().to_string(), self.serial()) .wrap_err("Failed to initialize GStreamer process manager")?; - let actuator = - KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) - .await - .wrap_err("Failed to create actuator")?; + let actuator = KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[ + // Right Leg + (10, ActuatorType::RobStride04), + (11, ActuatorType::RobStride03), + (12, ActuatorType::RobStride03), + (13, ActuatorType::RobStride04), + (14, ActuatorType::RobStride02), + // Left Leg + (20, ActuatorType::RobStride04), + (21, ActuatorType::RobStride03), + (22, ActuatorType::RobStride03), + (23, ActuatorType::RobStride04), + (24, ActuatorType::RobStride02), + // Right Arm + (30, ActuatorType::RobStride03), + (31, ActuatorType::RobStride03), + (32, ActuatorType::RobStride02), + (33, ActuatorType::RobStride02), + (34, ActuatorType::RobStride02), + (35, ActuatorType::RobStride00), + // Left Arm + (40, ActuatorType::RobStride03), + (41, ActuatorType::RobStride03), + (42, ActuatorType::RobStride02), + (43, ActuatorType::RobStride02), + (44, ActuatorType::RobStride02), + (45, ActuatorType::RobStride00), + ], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( @@ -79,10 +111,15 @@ impl Platform for KbotPlatform { )), ]) } else { - let actuator = - KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) - .await - .wrap_err("Failed to create actuator")?; + let actuator = KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04)], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( ActuatorServiceImpl::new(Arc::new(actuator)),