Skip to content

Commit 56cbd04

Browse files
hatomisthatomist
authored
Instant commands (#67)
* instant commands
1 parent 53510b3 commit 56cbd04

6 files changed

Lines changed: 89 additions & 42 deletions

File tree

‎Cargo.toml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@ resolver = "2"
88

99
[workspace.package]
1010

11-
version = "0.3.3"
12-
authors = ["Wesley Maa <wesley@kscale.dev>", "Pawel Budzianowski <pawel@kscale.dev>", "Benjamin Bolte <ben@kscale.dev>"]
11+
version = "0.3.4"
12+
authors = ["Denys Bezmenov <denys@kscale.dev>", "Wesley Maa <wesley@kscale.dev>", "Pawel Budzianowski <pawel@kscale.dev>", "Benjamin Bolte <ben@kscale.dev>"]
1313
edition = "2021"
1414
description = "Actuator package"
1515
license = "MIT"

‎actuator/robstride/Cargo.toml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,10 @@ license.workspace = true
1515
name = "robstride"
1616
crate-type = ["cdylib", "rlib"]
1717

18+
[features]
19+
default = []
20+
instant_command = []
21+
1822
[dependencies]
1923

2024
async-trait = "0.1"

‎actuator/robstride/src/actuator.rs‎

Lines changed: 32 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ impl Command {
134134
// Parse fault data from the command
135135
let fault_values = u32::from_le_bytes(self.data[0..4].try_into().unwrap());
136136
let warning_values = u32::from_le_bytes(self.data[4..8].try_into().unwrap());
137-
137+
138138
let fault_feedback = FaultFeedback {
139139
phase_a_overcurrent: (fault_values & (1 << 13)) != 0,
140140
overload_fault: (fault_values & (1 << 14)) != 0,
@@ -147,7 +147,7 @@ impl Command {
147147
motor_over_temp_fault: (fault_values & 1) != 0,
148148
motor_over_temp_warning: (warning_values & 1) != 0,
149149
};
150-
150+
151151
Ok(Frame::Fault(fault_feedback))
152152
}
153153
_ => Err("Invalid communication type".to_string()),
@@ -536,16 +536,36 @@ impl CommandData for FaultFeedback {
536536
let mut fault_values: u32 = 0;
537537
let mut warning_values: u32 = 0;
538538

539-
if self.phase_a_overcurrent { fault_values |= 1 << 13; }
540-
if self.overload_fault { fault_values |= 1 << 14; }
541-
if self.encoder_not_calibrated { fault_values |= 1 << 7; }
542-
if self.phase_c_overcurrent { fault_values |= 1 << 12; }
543-
if self.phase_b_overcurrent { fault_values |= 1 << 11; }
544-
if self.overvoltage_fault { fault_values |= 1 << 3; }
545-
if self.undervoltage_fault { fault_values |= 1 << 2; }
546-
if self.driver_chip_failure { fault_values |= 1 << 1; }
547-
if self.motor_over_temp_fault { fault_values |= 1; }
548-
if self.motor_over_temp_warning { warning_values |= 1; }
539+
if self.phase_a_overcurrent {
540+
fault_values |= 1 << 13;
541+
}
542+
if self.overload_fault {
543+
fault_values |= 1 << 14;
544+
}
545+
if self.encoder_not_calibrated {
546+
fault_values |= 1 << 7;
547+
}
548+
if self.phase_c_overcurrent {
549+
fault_values |= 1 << 12;
550+
}
551+
if self.phase_b_overcurrent {
552+
fault_values |= 1 << 11;
553+
}
554+
if self.overvoltage_fault {
555+
fault_values |= 1 << 3;
556+
}
557+
if self.undervoltage_fault {
558+
fault_values |= 1 << 2;
559+
}
560+
if self.driver_chip_failure {
561+
fault_values |= 1 << 1;
562+
}
563+
if self.motor_over_temp_fault {
564+
fault_values |= 1;
565+
}
566+
if self.motor_over_temp_warning {
567+
warning_values |= 1;
568+
}
549569

550570
let mut data = [0u8; 8];
551571
data[0..4].copy_from_slice(&fault_values.to_le_bytes());

‎actuator/robstride/src/bin/test_motor.rs‎

Lines changed: 21 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
11
use eyre::Result;
22
use robstride::robstride03::{RobStride03, RobStride03Command};
33
use robstride::SocketCanTransport;
4+
use robstride::{Actuator, ControlCommand, Protocol, Transport, TxCommand, TypedCommandData};
5+
use std::sync::Arc;
46
use std::time::Duration;
57
use tokio::sync::mpsc;
6-
use std::sync::Arc;
7-
use tracing::{info, error, trace};
8-
use robstride::{TxCommand, Protocol, ControlCommand, Actuator, Transport, TypedCommandData};
8+
use tracing::{error, info, trace};
99
use tracing_subscriber::{fmt, EnvFilter};
1010

1111
#[tokio::main]
@@ -18,11 +18,11 @@ async fn main() -> Result<()> {
1818

1919
// Create channel for sending commands
2020
let (tx, mut rx) = mpsc::channel(32);
21-
21+
2222
// Initialize CAN transport
2323
let transport = SocketCanTransport::new("can1".to_string()).await?;
24-
let mut transport_clone = transport.clone(); // Make this mutable
25-
24+
let mut transport_clone = transport.clone(); // Make this mutable
25+
2626
// Spawn transport handling task
2727
let _transport_task = tokio::spawn(async move {
2828
info!("Starting transport handling task");
@@ -45,44 +45,45 @@ async fn main() -> Result<()> {
4545

4646
// Create motor instance (using ID 1 and host ID 0)
4747
let motor = RobStride03::new(33, 0, tx);
48-
48+
4949
// Enable the motor
5050
println!("Enabling motor...");
5151
motor.enable().await?;
5252
tokio::time::sleep(Duration::from_millis(500)).await;
53-
53+
5454
// Set some reasonable limits
5555
println!("Setting limits...");
56-
motor.set_max_torque(20.0).await?; // 20 Nm max torque
57-
motor.set_max_velocity(5.0).await?; // 5 rad/s max velocity
58-
56+
motor.set_max_torque(20.0).await?; // 20 Nm max torque
57+
motor.set_max_velocity(5.0).await?; // 5 rad/s max velocity
58+
5959
// Create a position command (1 radian)
6060
let target_position = 0.0;
6161
println!("Moving to position: {} radians", target_position);
6262

6363
let command = RobStride03Command {
6464
target_angle_rad: target_position,
6565
target_velocity_rads: 0.0,
66-
kp: 25.0, // Position gain
67-
kd: 5.0, // Velocity gain
66+
kp: 25.0, // Position gain
67+
kd: 5.0, // Velocity gain
6868
torque_nm: 0.0,
69-
}.to_control_command();
70-
69+
}
70+
.to_control_command();
71+
7172
// Send the command
7273
motor.control(command).await?;
73-
74+
7475
// Wait for movement
7576
tokio::time::sleep(Duration::from_secs(2)).await;
76-
77+
7778
// Get feedback (position)
7879
motor.get_feedback().await?;
79-
80+
8081
// Disable the motor
8182
println!("Disabling motor...");
8283
motor.disable(false).await?;
83-
84+
8485
// Wait a moment before shutting down
8586
tokio::time::sleep(Duration::from_millis(100)).await;
86-
87+
8788
Ok(())
8889
}

‎actuator/robstride/src/lib.rs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,4 +10,4 @@ pub use actuator_types::*;
1010
pub use actuators::*;
1111
pub use protocol::Protocol;
1212
pub use supervisor::*;
13-
pub use transport::{CH341Transport, SocketCanTransport, StubTransport, TransportType, Transport};
13+
pub use transport::{CH341Transport, SocketCanTransport, StubTransport, Transport, TransportType};

‎actuator/robstride/src/supervisor.rs‎

Lines changed: 29 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -542,6 +542,10 @@ impl Supervisor {
542542
}
543543
}
544544

545+
if cfg!(feature = "instant_command") {
546+
command_valid = false;
547+
}
548+
545549
if command_valid {
546550
if let Err(e) = record
547551
.actuator
@@ -637,27 +641,32 @@ impl Supervisor {
637641
kp: config.kp,
638642
kd: config.kd,
639643
..Default::default()
640-
}.to_control_command(),
644+
}
645+
.to_control_command(),
641646
ActuatorType::RobStride01 => RobStride01Command {
642647
kp: config.kp,
643648
kd: config.kd,
644649
..Default::default()
645-
}.to_control_command(),
650+
}
651+
.to_control_command(),
646652
ActuatorType::RobStride02 => RobStride02Command {
647653
kp: config.kp,
648654
kd: config.kd,
649655
..Default::default()
650-
}.to_control_command(),
656+
}
657+
.to_control_command(),
651658
ActuatorType::RobStride03 => RobStride03Command {
652659
kp: config.kp,
653660
kd: config.kd,
654661
..Default::default()
655-
}.to_control_command(),
662+
}
663+
.to_control_command(),
656664
ActuatorType::RobStride04 => RobStride04Command {
657665
kp: config.kp,
658666
kd: config.kd,
659667
..Default::default()
660-
}.to_control_command(),
668+
}
669+
.to_control_command(),
661670
};
662671

663672
record.state.control_config = config.clone();
@@ -734,6 +743,13 @@ impl Supervisor {
734743
record.state.control_command.target_velocity = cmd.target_velocity;
735744
record.state.control_command.torque = cmd.torque;
736745

746+
if cfg!(feature = "instant_command") {
747+
record
748+
.actuator
749+
.control(record.state.control_command.clone())
750+
.await?;
751+
}
752+
737753
Ok(())
738754
}
739755

@@ -801,8 +817,14 @@ impl Supervisor {
801817
debug!(" Faults:");
802818
debug!(" Uncalibrated: {:?}", feedback.fault_uncalibrated);
803819
debug!(" Hall encoding: {:?}", feedback.fault_hall_encoding);
804-
debug!(" Magnetic encoding: {:?}", feedback.fault_magnetic_encoding);
805-
debug!(" Over temperature: {:?}", feedback.fault_over_temperature);
820+
debug!(
821+
" Magnetic encoding: {:?}",
822+
feedback.fault_magnetic_encoding
823+
);
824+
debug!(
825+
" Over temperature: {:?}",
826+
feedback.fault_over_temperature
827+
);
806828
debug!(" Overcurrent: {:?}", feedback.fault_overcurrent);
807829
debug!(" Undervoltage: {:?}", feedback.fault_undervoltage);
808830

0 commit comments

Comments
 (0)