From d95d728eb2775bf2714b3a956eac9f06f22d262c Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 19:53:30 +0200 Subject: [PATCH 1/5] read/write torque --- src/fake_motor.rs | 22 ++++++++++++++++++++++ src/motors_controller.rs | 14 ++++++++++++++ src/motors_io.rs | 5 +++++ 3 files changed, 41 insertions(+) diff --git a/src/fake_motor.rs b/src/fake_motor.rs index 3e9135a..664e24e 100644 --- a/src/fake_motor.rs +++ b/src/fake_motor.rs @@ -76,6 +76,7 @@ pub struct FakeMotorsIO { current_torque: [f64; N], target_position: [f64; N], + target_torque: [f64; N], velocity_limit: [f64; N], torque_limit: [f64; N], @@ -92,6 +93,7 @@ impl Default for FakeMotorsIO { current_torque: [NAN; N], target_position: [0.0; N], + target_torque: [NAN; N], velocity_limit: [INFINITY; N], torque_limit: [INFINITY; N], @@ -145,6 +147,26 @@ impl RawMotorsIO for FakeMotorsIO { Ok(self.target_position) } + fn get_target_torque(&mut self) -> Result<[f64; N]> { + Ok(self.target_torque) + } + + fn set_target_torque(&mut self, target_torque: [f64; N]) -> Result<()> { + log::debug!(target: "fake_io::set_target_torque", "Setting target_torque to {:?}", target_torque); + self.target_torque = target_torque; + + for (cur, on, target) in izip!(&mut self.current_torque, self.torque_on, target_torque) { + if on { + log::debug!(target: "fake_io::set_target_torque", "Setting current torque to target torque {:?} (torque on)", target); + *cur = target; + } else { + log::debug!(target: "fake_io::set_target_torque", "Current torque unchanged (torque off)"); + } + } + + Ok(()) + } + fn set_target_position(&mut self, target_position: [f64; N]) -> Result<()> { log::debug!(target: "fake_io::set_target_position", "Setting target_position to {:?}", target_position); self.target_position = target_position; diff --git a/src/motors_controller.rs b/src/motors_controller.rs index 25c1be8..8f20d4f 100644 --- a/src/motors_controller.rs +++ b/src/motors_controller.rs @@ -119,6 +119,20 @@ pub trait MotorsController { self.io().set_target_position(limited_position) } + /// Set the current target torque of the motors (in Nm) + fn set_target_torque(&mut self, torque: [f64; N]) -> Result<()> { + log::debug!(target: "controller::set_target_torque", "real target_torque: {:?}", torque); + + self.io().set_target_torque(torque) + } + + /// Get the current target torque of the motors (in Nm) + fn get_target_torque(&mut self) -> Result<[f64; N]> { + let torque = self.io().get_target_torque()?; + log::debug!(target: "controller::get_target_torque", "raw target_torque: {:?}", torque); + Ok(torque) + } + /// Set the current target position and returns the motor feeback (position, velocity, torque) fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]> { log::debug!(target: "controller::set_target_position", "real target_position: {:?}", position); diff --git a/src/motors_io.rs b/src/motors_io.rs index 886e9e0..f8bba77 100644 --- a/src/motors_io.rs +++ b/src/motors_io.rs @@ -18,6 +18,11 @@ pub trait RawMotorsIO { /// Set the current target position of the motors (in radians) fn set_target_position(&mut self, position: [f64; N]) -> Result<()>; + /// Get the current target torque of the motors (in Nm) + fn get_target_torque(&mut self) -> Result<[f64; N]>; + /// Set the current target torque of the motors (in Nm) + fn set_target_torque(&mut self, torque: [f64; N]) -> Result<()>; + /// Set the current target position and returns the motor feeback (position, velocity, torque) fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]>; From 787627a65b32cb5f6eaa0b43c87065dc2b08035d Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 20:02:27 +0200 Subject: [PATCH 2/5] generic_consts? --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 71f81e8..6e9b71f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,4 +1,4 @@ -#![feature(generic_const_exprs)] +// #![feature(generic_const_exprs)] #![allow(incomplete_features)] mod fake_motor; From d25c94dd761ff328d5854e7c8280f5d1a1f7a07c Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 20:14:35 +0200 Subject: [PATCH 3/5] typo and stuffs --- src/lib.rs | 2 +- src/motors_controller.rs | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 6e9b71f..0b0f224 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -10,7 +10,7 @@ pub use limit::Limit; mod motors_io; pub use motors_io::RawMotorsIO; mod motors_controller; -pub use motors_controller::{MissingResisterErrror, MotorsController}; +pub use motors_controller::{MissingRegisterErrror, MotorsController}; mod pid; pub use pid::PID; diff --git a/src/motors_controller.rs b/src/motors_controller.rs index 8f20d4f..dbbd2d7 100644 --- a/src/motors_controller.rs +++ b/src/motors_controller.rs @@ -265,11 +265,11 @@ pub trait MotorsController { } #[derive(Debug)] -pub struct MissingResisterErrror(pub String); -impl std::fmt::Display for MissingResisterErrror { +pub struct MissingRegisterErrror(pub String); +impl std::fmt::Display for MissingRegisterErrror { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { let name = &self.0; write!(f, "(missing register \"{name}\")",) } } -impl std::error::Error for MissingResisterErrror {} +impl std::error::Error for MissingRegisterErrror {} From 9500c4433f9577d6f95b68d3b3b7dfb0bae1c06f Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Wed, 2 Oct 2024 10:37:18 +0200 Subject: [PATCH 4/5] add mode --- src/fake_motor.rs | 39 +++++++++++++++++++++++++++++++++++++++ src/motors_controller.rs | 28 ++++++++++++++++++++++++++++ src/motors_io.rs | 12 ++++++++++++ 3 files changed, 79 insertions(+) diff --git a/src/fake_motor.rs b/src/fake_motor.rs index 664e24e..f298b39 100644 --- a/src/fake_motor.rs +++ b/src/fake_motor.rs @@ -77,6 +77,8 @@ pub struct FakeMotorsIO { target_position: [f64; N], target_torque: [f64; N], + target_velocity: [f64; N], + control_mode: [u8; N], velocity_limit: [f64; N], torque_limit: [f64; N], @@ -94,6 +96,9 @@ impl Default for FakeMotorsIO { target_position: [0.0; N], target_torque: [NAN; N], + target_velocity: [NAN; N], + + control_mode: [0; N], velocity_limit: [INFINITY; N], torque_limit: [INFINITY; N], @@ -167,6 +172,40 @@ impl RawMotorsIO for FakeMotorsIO { Ok(()) } + fn get_target_velocity(&mut self) -> Result<[f64; N]> { + Ok(self.target_velocity) + } + + fn set_target_velocity(&mut self, target_velocity: [f64; N]) -> Result<()> { + log::debug!(target: "fake_io::set_target_velocity", "Setting target_velocity to {:?}", target_velocity); + self.target_velocity = target_velocity; + + for (cur, on, target) in izip!(&mut self.current_velocity, self.torque_on, target_velocity) + { + if on { + log::debug!(target: "fake_io::set_target_velocity", "Setting current velocity to target velocity {:?} (velocity on)", target); + *cur = target; + } else { + log::debug!(target: "fake_io::set_target_velocity", "Current velocity unchanged (velocity off)"); + } + } + + Ok(()) + } + + fn get_control_mode(&mut self) -> Result<[u8; N]> { + Ok(self.control_mode) + } + + fn set_control_mode(&mut self, control_mode: [u8; N]) -> Result<()> { + log::debug!(target: "fake_io::set_control_mode", "Setting control_mode to {:?}", control_mode); + self.control_mode = control_mode; + + log::debug!(target: "fake_io::set_control_mode", "Setting control_mode to {:?}",control_mode); + + Ok(()) + } + fn set_target_position(&mut self, target_position: [f64; N]) -> Result<()> { log::debug!(target: "fake_io::set_target_position", "Setting target_position to {:?}", target_position); self.target_position = target_position; diff --git a/src/motors_controller.rs b/src/motors_controller.rs index dbbd2d7..56b2a3d 100644 --- a/src/motors_controller.rs +++ b/src/motors_controller.rs @@ -126,6 +126,20 @@ pub trait MotorsController { self.io().set_target_torque(torque) } + /// Set the current target velocity of the motors (in rad/s) + fn set_target_velocity(&mut self, velocity: [f64; N]) -> Result<()> { + log::debug!(target: "controller::set_target_velocity", "real target_velocity: {:?}", velocity); + + self.io().set_target_velocity(velocity) + } + + /// Set control mode + fn set_control_mode(&mut self, mode: [u8; N]) -> Result<()> { + log::debug!(target: "controller::set_control_mode", "real control_mode: {:?}", mode); + + self.io().set_control_mode(mode) + } + /// Get the current target torque of the motors (in Nm) fn get_target_torque(&mut self) -> Result<[f64; N]> { let torque = self.io().get_target_torque()?; @@ -133,6 +147,20 @@ pub trait MotorsController { Ok(torque) } + /// Get the current target velocity of the motors (in rad/s) + fn get_target_velocity(&mut self) -> Result<[f64; N]> { + let velocity = self.io().get_target_velocity()?; + log::debug!(target: "controller::get_target_velocity", "raw target_velocity: {:?}", velocity); + Ok(velocity) + } + + /// Get the current control mode + fn get_control_mode(&mut self) -> Result<[u8; N]> { + let mode = self.io().get_control_mode()?; + log::debug!(target: "controller::get_control_mode", "raw control_mode: {:?}", mode); + Ok(mode) + } + /// Set the current target position and returns the motor feeback (position, velocity, torque) fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]> { log::debug!(target: "controller::set_target_position", "real target_position: {:?}", position); diff --git a/src/motors_io.rs b/src/motors_io.rs index f8bba77..689f3e4 100644 --- a/src/motors_io.rs +++ b/src/motors_io.rs @@ -23,6 +23,18 @@ pub trait RawMotorsIO { /// Set the current target torque of the motors (in Nm) fn set_target_torque(&mut self, torque: [f64; N]) -> Result<()>; + /// Set the current target velocity of the motors (in rad/s) + fn set_target_velocity(&mut self, velocity: [f64; N]) -> Result<()>; + + /// Get the current target velocity of the motors (in rad/s) + fn get_target_velocity(&mut self) -> Result<[f64; N]>; + + /// Set the control mode + fn set_control_mode(&mut self, mode: [u8; N]) -> Result<()>; + + /// Get the control mode + fn get_control_mode(&mut self) -> Result<[u8; N]>; + /// Set the current target position and returns the motor feeback (position, velocity, torque) fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]>; From e952f87440f69392bb753ea38b07eb7e29757133 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Mon, 7 Oct 2024 11:55:22 +0200 Subject: [PATCH 5/5] fix warn --- src/fake_motor.rs | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/fake_motor.rs b/src/fake_motor.rs index f298b39..06cdc79 100644 --- a/src/fake_motor.rs +++ b/src/fake_motor.rs @@ -1,5 +1,3 @@ -use std::f64::{INFINITY, NAN}; - use itertools::izip; use crate::motors_controller::MotorsController; @@ -91,21 +89,21 @@ impl Default for FakeMotorsIO { torque_on: [false; N], current_position: [0.0; N], - current_velocity: [NAN; N], - current_torque: [NAN; N], + current_velocity: [f64::NAN; N], + current_torque: [f64::NAN; N], target_position: [0.0; N], - target_torque: [NAN; N], - target_velocity: [NAN; N], + target_torque: [f64::NAN; N], + target_velocity: [f64::NAN; N], control_mode: [0; N], - velocity_limit: [INFINITY; N], - torque_limit: [INFINITY; N], + velocity_limit: [f64::INFINITY; N], + torque_limit: [f64::INFINITY; N], pid: [PID { - p: NAN, - i: NAN, - d: NAN, + p: f64::NAN, + i: f64::NAN, + d: f64::NAN, }; N], } }