use std::time::{Duration, Instant};
use crate::bus::{CanBus, CanFrame, CanId};
use crate::error::{Error, Result};
use crate::protocol::{
DEFAULT_CONTROL_RATE_HZ, DEFAULT_MOTOR_ID, DirectCommand, FeedbackFilter, MitCommand,
MitHelperCommand, MotorFeedback, MotorFeedbackUpdate, MotorModel, OriginMode,
Position32Feedback, clamp_erpm, degrees_to_radians, direct_command_frame, erpm_to_rad_s,
is_position_32bit_feedback_frame, is_servo_feedback_frame, mit_command_frame, mit_helper_frame,
parse_position_32bit_feedback, parse_servo_feedback,
};
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum HelperPolicy {
Strict,
FcFd,
Legacy,
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MotorConfig {
pub motor_id: u8,
pub model: MotorModel,
pub feedback_id: Option<CanId>,
pub helper_policy: HelperPolicy,
pub allow_legacy_feedback_ids: bool,
pub control_rate_hz: f64,
pub mit_velocity_kd: Option<f64>,
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct RefreshStats {
pub iterations: u64,
pub feedback_frames: u64,
pub missed_feedback: u64,
pub expected_period: Duration,
pub elapsed: Duration,
}
impl RefreshStats {
pub const fn new(expected_period: Duration) -> Self {
Self {
iterations: 0,
feedback_frames: 0,
missed_feedback: 0,
expected_period,
elapsed: Duration::ZERO,
}
}
pub fn effective_hz(&self) -> Option<f64> {
if self.iterations == 0 || self.elapsed.is_zero() {
return None;
}
Some(self.iterations as f64 / self.elapsed.as_secs_f64())
}
pub fn feedback_ratio(&self) -> Option<f64> {
if self.iterations == 0 {
return None;
}
Some(self.feedback_frames as f64 / self.iterations as f64)
}
}
impl MotorConfig {
pub fn feedback_filter(&self) -> FeedbackFilter {
let mut filter = FeedbackFilter::for_motor(self.model, self.motor_id);
if let Some(feedback_id) = self.feedback_id {
filter = filter.with_explicit_id(feedback_id);
}
if self.allow_legacy_feedback_ids {
filter = filter.with_legacy_ids(self.motor_id);
}
filter
}
pub fn refresh_period(&self) -> Result<Duration> {
if self.control_rate_hz <= 0.0 || !self.control_rate_hz.is_finite() {
return Err(Error::InvalidRefreshRate(self.control_rate_hz));
}
Ok(Duration::from_secs_f64(1.0 / self.control_rate_hz))
}
pub fn velocity_kd(&self) -> f64 {
self.mit_velocity_kd
.unwrap_or_else(|| self.model.spec().mit_velocity_kd)
}
}
impl Default for MotorConfig {
fn default() -> Self {
Self {
motor_id: DEFAULT_MOTOR_ID,
model: MotorModel::Ak60_6,
feedback_id: None,
helper_policy: HelperPolicy::FcFd,
allow_legacy_feedback_ids: false,
control_rate_hz: DEFAULT_CONTROL_RATE_HZ,
mit_velocity_kd: None,
}
}
}
#[derive(Debug)]
pub struct MotorController<B> {
bus: B,
config: MotorConfig,
feedback_filter: FeedbackFilter,
last_feedback: Option<MotorFeedback>,
last_position_32bit_feedback: Option<Position32Feedback>,
active_command: Option<MitCommand>,
mit_enabled: bool,
}
impl<B: CanBus> MotorController<B> {
pub fn new(bus: B, config: MotorConfig) -> Result<Self> {
config.refresh_period()?;
Ok(Self {
feedback_filter: config.feedback_filter(),
bus,
config,
last_feedback: None,
last_position_32bit_feedback: None,
active_command: None,
mit_enabled: false,
})
}
pub fn config(&self) -> &MotorConfig {
&self.config
}
pub fn bus(&self) -> &B {
&self.bus
}
pub fn bus_mut(&mut self) -> &mut B {
&mut self.bus
}
pub fn into_inner(self) -> B {
self.bus
}
pub fn last_feedback(&self) -> Option<&MotorFeedback> {
self.last_feedback.as_ref()
}
pub fn last_position_32bit_feedback(&self) -> Option<&Position32Feedback> {
self.last_position_32bit_feedback.as_ref()
}
pub const fn mit_enabled(&self) -> bool {
self.mit_enabled
}
pub fn active_command(&self) -> Option<MitCommand> {
self.active_command
}
pub fn mit_frame(&self, command: MitCommand) -> Result<CanFrame> {
mit_command_frame(self.config.model, self.config.motor_id, command)
}
pub fn helper_frame(&self, helper: MitHelperCommand) -> Result<CanFrame> {
mit_helper_frame(self.config.model, self.config.motor_id, helper)
}
pub fn direct_frame(&self, command: DirectCommand) -> Result<CanFrame> {
direct_command_frame(self.config.motor_id, command)
}
pub fn enable_mit_mode(&mut self) -> Result<()> {
match self.config.helper_policy {
HelperPolicy::Strict => {}
HelperPolicy::FcFd => self.send_helper(MitHelperCommand::Enable)?,
HelperPolicy::Legacy => {
self.send_helper(MitHelperCommand::Enable)?;
self.send_helper(MitHelperCommand::EnableLegacy)?;
}
}
self.send_mit_frame(MitCommand::neutral())?;
self.mit_enabled = true;
Ok(())
}
pub fn disable_mit_mode(&mut self) -> Result<()> {
for _ in 0..3 {
self.send_mit_frame(MitCommand::neutral())?;
}
match self.config.helper_policy {
HelperPolicy::Strict => {}
HelperPolicy::FcFd => self.send_helper(MitHelperCommand::Disable)?,
HelperPolicy::Legacy => {
self.send_helper(MitHelperCommand::Disable)?;
self.send_helper(MitHelperCommand::DisableLegacy)?;
}
}
self.active_command = None;
self.mit_enabled = false;
Ok(())
}
pub fn zero_position(&mut self) -> Result<()> {
self.send_helper(MitHelperCommand::ZeroPosition)?;
self.clear_position_feedback();
Ok(())
}
pub fn set_mit_mode(&mut self, mut command: MitCommand) -> Result<()> {
if !self.mit_enabled {
self.enable_mit_mode()?;
}
let spec = self.config.model.spec();
let requested_erpm = crate::protocol::rad_s_to_erpm(command.velocity_rad_s, spec);
let clamped_erpm = clamp_erpm(requested_erpm, spec);
command.velocity_rad_s = erpm_to_rad_s(clamped_erpm, spec);
self.send_mit_frame(command)?;
self.active_command = Some(command);
Ok(())
}
pub fn set_velocity_erpm(&mut self, velocity_erpm: i32) -> Result<()> {
if velocity_erpm == 0 {
return self.stop();
}
let spec = self.config.model.spec();
let clamped_erpm = clamp_erpm(velocity_erpm, spec);
self.set_mit_mode(MitCommand {
position_rad: 0.0,
velocity_rad_s: erpm_to_rad_s(clamped_erpm, spec),
kp: 0.0,
kd: self.config.velocity_kd(),
torque_nm: 0.0,
})
}
pub fn set_position_degrees(&mut self, position_degrees: f64) -> Result<()> {
let spec = self.config.model.spec();
self.set_mit_mode(MitCommand {
position_rad: degrees_to_radians(position_degrees),
velocity_rad_s: 0.0,
kp: spec.mit_position_kp,
kd: spec.mit_position_kd,
torque_nm: 0.0,
})
}
pub fn set_torque_nm(&mut self, torque_nm: f64) -> Result<()> {
self.set_mit_mode(MitCommand {
position_rad: 0.0,
velocity_rad_s: 0.0,
kp: 0.0,
kd: 0.0,
torque_nm,
})
}
pub fn send_direct_command(&mut self, command: DirectCommand) -> Result<()> {
let frame = self.direct_frame(command)?;
self.bus.send(&frame)?;
match command {
DirectCommand::SetOrigin(_) => self.clear_position_feedback(),
DirectCommand::ConfigureFeedback(_) => {
self.last_position_32bit_feedback = None;
}
_ => {}
}
Ok(())
}
pub fn set_duty_cycle(&mut self, duty: f64) -> Result<()> {
self.send_direct_command(DirectCommand::DutyCycle(duty))
}
pub fn set_current_amps(&mut self, current_amps: f64) -> Result<()> {
self.send_direct_command(DirectCommand::Current(current_amps))
}
pub fn set_brake_current_amps(&mut self, current_amps: f64) -> Result<()> {
self.send_direct_command(DirectCommand::BrakeCurrent(current_amps))
}
pub fn set_direct_velocity_erpm(&mut self, velocity_erpm: i32) -> Result<()> {
self.send_direct_command(DirectCommand::Velocity(velocity_erpm))
}
pub fn set_direct_position_degrees(&mut self, position_degrees: f64) -> Result<()> {
self.send_direct_command(DirectCommand::Position(position_degrees))
}
pub fn set_origin(&mut self, origin_mode: OriginMode) -> Result<()> {
self.send_direct_command(DirectCommand::SetOrigin(origin_mode))
}
pub fn send_keepalive(&mut self) -> Result<()> {
let command = self.active_command.unwrap_or_else(MitCommand::neutral);
self.send_mit_frame(command)
}
pub fn refresh_once(&mut self, feedback_timeout: Duration) -> Result<Option<MotorFeedback>> {
self.send_keepalive()?;
self.receive_feedback(feedback_timeout)
}
pub fn refresh_for(
&mut self,
duration: Duration,
feedback_timeout: Duration,
) -> Result<RefreshStats> {
let period = self.config.refresh_period()?;
let started = Instant::now();
let mut stats = RefreshStats::new(period);
while started.elapsed() < duration {
let loop_started = Instant::now();
match self.refresh_once(feedback_timeout)? {
Some(_) => stats.feedback_frames += 1,
None => stats.missed_feedback += 1,
}
stats.iterations += 1;
let loop_elapsed = loop_started.elapsed();
if loop_elapsed < period {
std::thread::sleep(period - loop_elapsed);
}
}
stats.elapsed = started.elapsed();
Ok(stats)
}
pub fn receive_feedback(&mut self, timeout: Duration) -> Result<Option<MotorFeedback>> {
let deadline = Instant::now() + timeout;
loop {
match self.receive_feedback_update_until(deadline)? {
Some(MotorFeedbackUpdate::Status(feedback)) => return Ok(Some(feedback)),
Some(MotorFeedbackUpdate::Position32(_)) => {}
None => return Ok(None),
}
}
}
pub fn receive_feedback_update(
&mut self,
timeout: Duration,
) -> Result<Option<MotorFeedbackUpdate>> {
self.receive_feedback_update_until(Instant::now() + timeout)
}
pub fn check_communication(&mut self, timeout: Duration) -> Result<bool> {
self.enable_mit_mode()?;
self.send_keepalive()?;
Ok(self.receive_feedback(timeout)?.is_some())
}
pub fn stop(&mut self) -> Result<()> {
for _ in 0..4 {
self.send_mit_frame(MitCommand::neutral())?;
}
self.disable_mit_mode()
}
fn send_mit_frame(&mut self, command: MitCommand) -> Result<()> {
let frame = self.mit_frame(command)?;
self.bus.send(&frame)
}
fn send_helper(&mut self, helper: MitHelperCommand) -> Result<()> {
let frame = self.helper_frame(helper)?;
self.bus.send(&frame)
}
fn receive_feedback_update_until(
&mut self,
deadline: Instant,
) -> Result<Option<MotorFeedbackUpdate>> {
loop {
let now = Instant::now();
if now >= deadline {
return Ok(None);
}
let Some(frame) = self.bus.receive(deadline.saturating_duration_since(now))? else {
return Ok(None);
};
if !self.feedback_filter.matches(&frame) {
continue;
}
if is_position_32bit_feedback_frame(&frame, self.config.motor_id) {
let position = parse_position_32bit_feedback(frame.data())?;
self.last_position_32bit_feedback = Some(position);
return Ok(Some(MotorFeedbackUpdate::Position32(position)));
}
let feedback = if is_servo_feedback_frame(&frame, self.config.motor_id) {
parse_servo_feedback(frame.data())?
} else {
self.config.model.parse_feedback(frame.data())?
};
self.last_feedback = Some(feedback.clone());
return Ok(Some(MotorFeedbackUpdate::Status(feedback)));
}
}
fn clear_position_feedback(&mut self) {
self.last_feedback = None;
self.last_position_32bit_feedback = None;
}
}