Skip to content

Commit

Permalink
Merge pull request #16 from kscalelabs/robstridev2
Browse files Browse the repository at this point in the history 
Robstridev2
  • Loading branch information
@hatomist
hatomist authored Dec 4, 2024
2 parents a915939 + c59bd75 commit 3a7489f
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 18 deletions.
2 changes: 1 addition & 1 deletion platforms/kbot/Cargo.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
33 changes: 26 additions & 7 deletions platforms/kbot/src/actuator.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,11 @@ impl KBotActuator {
pub async fn new(
_operations_service: Arc<OperationsServiceImpl>,
ports: Vec<&str>,
motors_timeout: Duration,
actuator_timeout: Duration,
polling_interval: Duration,
desired_actuator_types: &[(u8, robstridev2::ActuatorType)],
) -> Result<Self> {
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") {
Expand All @@ -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 {
Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down
57 changes: 47 additions & 10 deletions platforms/kbot/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,21 @@ 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;
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;
Expand Down Expand Up @@ -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(
Expand All @@ -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)),
Expand Down

0 comments on commit 3a7489f

Please sign in to comment.