diff --git a/Cargo.lock b/Cargo.lock index 99f9b8ca..c902218b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -371,6 +371,17 @@ dependencies = [ "nalgebra", ] +[[package]] +name = "hyped_motors" +version = "0.1.0" +dependencies = [ + "embassy-sync 0.6.0", + "embassy-time", + "heapless", + "hyped_can", + "hyped_core", +] + [[package]] name = "hyped_sensors" version = "0.1.0" diff --git a/Cargo.toml b/Cargo.toml index 71004727..39a80236 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,6 +5,7 @@ members = [ "lib/control", "lib/io/*", "lib/localisation", + "lib/motors", "lib/sensors" ] exclude = [ diff --git a/lib/motors/Cargo.toml b/lib/motors/Cargo.toml new file mode 100644 index 00000000..71698e1d --- /dev/null +++ b/lib/motors/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "hyped_motors" +version = "0.1.0" +edition = "2021" + +[dependencies] +embassy-sync = { version = "0.6.0", features = ["defmt"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} +embassy-time = { version = "0.3.1", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"} +heapless = { version = "0.8", default-features = false, features = ["serde"] } +hyped_core = { path = "../core" } +hyped_can = { path = "../io/hyped_can" } \ No newline at end of file diff --git a/lib/motors/src/can_open_message.rs b/lib/motors/src/can_open_message.rs new file mode 100644 index 00000000..51c1c1f8 --- /dev/null +++ b/lib/motors/src/can_open_message.rs @@ -0,0 +1,98 @@ +/// Basic structure for a CanOpen Message +#[derive(Debug, PartialEq, Clone)] +pub struct CanOpenMessage { + pub id: u32, + pub index: u16, + pub sub_index: u8, + pub command: u8, + pub data: u32, +} + +pub mod config_messages { + use super::CanOpenMessage; + + pub const TEST_STEPPER_ENABLE: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x6040, + sub_index: 0x09, + data: 0x00000001, + }; + + pub const TEST_MODE_COMMAND: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x2031, + sub_index: 0x00, + data: 0x00000060, + }; +} + +pub mod messages { + use super::CanOpenMessage; + + pub const ENTER_STOP_STATE: CanOpenMessage = CanOpenMessage { + id: 0x000, + command: 0x02, + index: 0x0000, + sub_index: 0x00, + data: 0x00000000, + }; + + pub const ENTER_PREOPERATIONAL_STATE: CanOpenMessage = CanOpenMessage { + id: 0x000, + command: 0x03, + index: 0x0000, + sub_index: 0x00, + data: 0x00000000, + }; + + pub const ENTER_OPERATIONAL_STATE: CanOpenMessage = CanOpenMessage { + id: 0x000, + command: 0x01, + index: 0x0000, + sub_index: 0x00, + data: 0x00000000, + }; + + // Data will be overwritten at runtime depending on the frequency desired + pub const SET_FREQUENCY: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x2040, + sub_index: 0x04, + data: 0x00000000, + }; + + pub const SHUTDOWN: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x6040, + sub_index: 0x00, + data: 0x00000006, + }; + + pub const SWITCH_ON: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x6040, + sub_index: 0x00, + data: 0x00000007, + }; + + pub const START_DRIVE: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x6040, + sub_index: 0x00, + data: 0x0000000F, + }; + + pub const QUICK_STOP: CanOpenMessage = CanOpenMessage { + id: 0x601, + command: 0x2B, + index: 0x6040, + sub_index: 0x00, + data: 0x00000002, + }; +} diff --git a/lib/motors/src/can_open_processor.rs b/lib/motors/src/can_open_processor.rs new file mode 100644 index 00000000..cdac7463 --- /dev/null +++ b/lib/motors/src/can_open_processor.rs @@ -0,0 +1,115 @@ +use crate::can_open_message::{config_messages, messages, CanOpenMessage}; +use hyped_can::{CanError, HypedCan, HypedCanFrame}; + +/// All types of messages that can be sent to the motor controller +pub enum Messages { + TestStepperEnable, + TestModeCommand, + EnterStopState, + EnterPreoperationalState, + EnterOperationalState, + SetFrequency(u32), + Shutdown, + SwitchOn, + StartDrive, + QuickStop, +} + +// TODOLater consider adding a ReceivedMessages so we can decide what we do depending on the message we receive + +/// Convert a CanOpenMessage to a HypedCanFrame +impl From for HypedCanFrame { + fn from(msg: CanOpenMessage) -> Self { + let mut data: [u8; 8] = [0; 8]; + + data[0] = msg.command; + data[1] = (msg.index & 0xFF) as u8; + data[2] = ((msg.index >> 8) & 0xFF) as u8; + data[3] = msg.sub_index; + data[4] = (msg.data & 0xFF) as u8; + data[5] = ((msg.data >> 8) & 0xFF) as u8; + data[6] = ((msg.data >> 16) & 0xFF) as u8; + data[7] = ((msg.data >> 24) & 0xFF) as u8; + + HypedCanFrame { + can_id: msg.id, + data, + } + } +} + +/// Convert a HypedCanFrame to a CanOpenMessage +impl From for CanOpenMessage { + fn from(frame: HypedCanFrame) -> Self { + CanOpenMessage { + id: frame.can_id, + command: frame.data[0], + index: u16::from(frame.data[1]) | (u16::from(frame.data[2]) << 8), + sub_index: frame.data[3], + data: u32::from_le_bytes([frame.data[4], frame.data[5], frame.data[6], frame.data[7]]), + } + } +} + +/// Convert a Message to a CanOpenMessage +impl From for CanOpenMessage { + fn from(message: Messages) -> Self { + match message { + Messages::TestStepperEnable => config_messages::TEST_STEPPER_ENABLE, + Messages::TestModeCommand => config_messages::TEST_MODE_COMMAND, + Messages::EnterStopState => messages::ENTER_STOP_STATE, + Messages::EnterPreoperationalState => messages::ENTER_PREOPERATIONAL_STATE, + Messages::EnterOperationalState => messages::ENTER_OPERATIONAL_STATE, + Messages::SetFrequency(f) => CanOpenMessage { + id: messages::SET_FREQUENCY.id, + command: messages::SET_FREQUENCY.command, + index: messages::SET_FREQUENCY.index, + sub_index: messages::SET_FREQUENCY.sub_index, + data: f, + }, + Messages::Shutdown => messages::SHUTDOWN, + Messages::SwitchOn => messages::SWITCH_ON, + Messages::StartDrive => messages::START_DRIVE, + Messages::QuickStop => messages::QUICK_STOP, + } + } +} + +/// A wrapper around a HypedCan that turns a CanOpenMessage into a HypedCanFrame and sends it over the HypedCan +/// Also reads a HypedCanFrame and turns it into a CanOpenMessage +pub struct CanOpen { + can: T, +} + +impl CanOpen { + pub fn new(can: T) -> Self { + CanOpen { can } + } + + /// Send a message to the motor controller + pub fn send_message(&mut self, message: Messages) -> Result<(), CanError> { + let frame: HypedCanFrame = CanOpenMessage::from(message).into(); + self.can.write_frame(&frame) + } + + /// Read a message from the motor controller and return it + pub fn read_message(&mut self) -> Result { + let envelope = self.can.read_frame()?; + let frame = envelope.frame; + let message = CanOpenMessage::from(frame); + Ok(message) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn convert_to_and_back() { + let og_message = CanOpenMessage::from(Messages::SetFrequency(1000)); + let frame: HypedCanFrame = og_message.clone().into(); + let message = CanOpenMessage::from(frame); + assert_eq!(og_message, message); + } +} diff --git a/lib/motors/src/lib.rs b/lib/motors/src/lib.rs new file mode 100644 index 00000000..37a44b99 --- /dev/null +++ b/lib/motors/src/lib.rs @@ -0,0 +1,4 @@ +#![no_std] + +pub mod can_open_message; +pub mod can_open_processor;