diff --git a/.cargo/config.toml b/.cargo/config.toml index 8dcb60c..48737ac 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -8,3 +8,21 @@ rustflags = [ [target.aarch64-unknown-linux-gnu] linker = "aarch64-linux-gnu-gcc" + +[target.riscv64gc-unknown-linux-musl] +rustflags = [ + "-C", "target-feature=-crt-static", + "-Clink-args=--sysroot=/sdk/host/riscv64-buildroot-linux-musl/sysroot", + "-L", "./firmware/cviwrapper", + "-L", "./firmware/sts3215", + "-L", "./firmware/tpu-sdk-sg200x/lib", + "-L", "./firmware/duo-sdk/rootfs/usr/lib", + "-L", "./models/tpu-mlir/lib", + "-Clink-arg=-Wl,-rpath,../models/tpu-mlir/lib", + "-Clink-arg=-Wl,-rpath,./firmware/tpu-sdk-sg200x/lib", + "-Clink-arg=-Wl,-rpath,./firmware/duo-sdk/rootfs/lib", + "-Clink-arg=-Wl,-rpath,./tpu-libs", + "-Clink-arg=-Wl,-rpath,./sysroot/lib", + "-Clink-arg=-Wl,-rpath,./sysroot/usr/lib", +] +linker = "/sdk/host/bin/riscv64-buildroot-linux-musl-gcc.br_real" diff --git a/daemon/.cargo/config.toml b/daemon/.cargo/config.toml new file mode 100644 index 0000000..38cc619 --- /dev/null +++ b/daemon/.cargo/config.toml @@ -0,0 +1,19 @@ +[target.riscv64gc-unknown-linux-musl] +rustflags = [ + "-C", "target-feature=-crt-static", + "-Clink-args=--sysroot=/sdk/host/riscv64-buildroot-linux-musl/sysroot", + "-L", "./firmware/cviwrapper", + "-L", "./firmware/sts3215", + "-L", "./firmware/tpu-sdk-sg200x/lib", + "-L", "./firmware/duo-sdk/rootfs/usr/lib", + "-L", "./models/tpu-mlir/lib", + "-Clink-arg=-Wl,-rpath,../models/tpu-mlir/lib", + "-Clink-arg=-Wl,-rpath,./firmware/tpu-sdk-sg200x/lib", + "-Clink-arg=-Wl,-rpath,./firmware/duo-sdk/rootfs/lib", + "-Clink-arg=-Wl,-rpath,./tpu-libs", + "-Clink-arg=-Wl,-rpath,./sysroot/lib", + "-Clink-arg=-Wl,-rpath,./sysroot/usr/lib", +] +linker = "/sdk/host/bin/riscv64-buildroot-linux-musl-gcc.br_real" + +[build] diff --git a/daemon/Cargo.toml b/daemon/Cargo.toml index fd6d01c..6b49041 100644 --- a/daemon/Cargo.toml +++ b/daemon/Cargo.toml @@ -12,14 +12,10 @@ readme.workspace = true [dependencies] kos_core = { version = "0.1.1", path = "../kos_core" } tokio = { version = "1", features = ["full"] } -tonic = { version = "0.12", features = ["transport"] } +tonic = { git = "https://github.com/hatomist/tonic-milkv" } eyre = "0.6" tracing = "0.1" tracing-subscriber = { version = "0.3", features = ["env-filter"] } -tower = "0.5" -gstreamer = "0.23" -gstreamer-base = "0.23" -glib = "0.17" tracing-appender = "0.2" clap = { version = "4.0", features = ["derive"] } chrono = "0.4" diff --git a/daemon/src/main.rs b/daemon/src/main.rs index 814ab75..4074e6a 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -26,7 +26,7 @@ use kos_stub::StubPlatform as PlatformImpl; use kos_sim::SimPlatform as PlatformImpl; #[cfg(feature = "kos-zeroth-01")] -use kos_zeroth_01::Zeroth01Platform as PlatformImpl; +use kos_zeroth_01::ZBotPlatform as PlatformImpl; #[cfg(feature = "kos-kbot")] use kos_kbot::KbotPlatform as PlatformImpl; diff --git a/kos_core/Cargo.toml b/kos_core/Cargo.toml index 665615a..efe80d0 100644 --- a/kos_core/Cargo.toml +++ b/kos_core/Cargo.toml @@ -16,11 +16,11 @@ bytes = "1" serde = { version = "1.0", features = ["derive"] } serde_json = "1.0" yaml-rust2 = "0.9" -tonic = { version = "0.12", features = ["transport"] } +tonic = { git = "https://github.com/hatomist/tonic-milkv" } prost = "0.13" prost-types = "0.13" async-trait = "0.1" -rumqttc = "0.24" +rumqttc = { version = "0.24", default-features = false } tokio = { version = "1", features = ["full"] } eyre = "0.6" hyper = "0.14" @@ -29,7 +29,7 @@ lazy_static = "1.4" krec = "0.2" [build-dependencies] -tonic-build = "0.12" +tonic-build = { git = "https://github.com/hatomist/tonic-milkv" } [lib] doctest = false diff --git a/platforms/zeroth-01/Cargo.toml b/platforms/zeroth-01/Cargo.toml index 1af576c..6ad6128 100644 --- a/platforms/zeroth-01/Cargo.toml +++ b/platforms/zeroth-01/Cargo.toml @@ -11,3 +11,7 @@ description = "KOS platform for Zeroth-01" kos_core = { version = "0.1.1", path = "../../kos_core" } async-trait = "0.1" eyre = "0.6" +serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" +tonic = { git = "https://github.com/hatomist/tonic-milkv" } +tokio = { version = "1", features = ["full"] } diff --git a/platforms/zeroth-01/src/actuator.rs b/platforms/zeroth-01/src/actuator.rs new file mode 100644 index 0000000..4020871 --- /dev/null +++ b/platforms/zeroth-01/src/actuator.rs @@ -0,0 +1,153 @@ +use crate::firmware::hal::{ServoDirection, ServoMode, ServoRegister, TorqueMode}; +use crate::Servo; +use kos_core::google_proto::longrunning::Operation; +use kos_core::hal::Actuator; +use kos_core::kos_proto::actuator::*; +use kos_core::kos_proto::common::{ActionResponse, ActionResult, Error as KosError, ErrorCode}; +use std::sync::{Arc, Mutex}; +use tokio::sync::RwLock; +use tonic::{Request, Response, Status}; +use eyre::Result; + +pub struct ZBotActuator { + servo: Arc>, +} + +impl ZBotActuator { + pub async fn new() -> Result { + let servo = Servo::new()?; + servo.enable_readout()?; + Ok(Self { + servo: Arc::new(Mutex::new(servo)), + }) + } +} + +#[tonic::async_trait] +impl Actuator for ZBotActuator { + async fn command_actuators( + &self, + commands: Vec, + ) -> Result> { + let servo = self.servo.lock().map_err(|_| Status::internal("Lock error"))?; + + let mut results = Vec::new(); + for cmd in commands { + let result = if let Some(position) = cmd.position { + // Convert degrees to raw servo position + let raw_position = Servo::degrees_to_raw(position as f32); + servo.move_servo(cmd.actuator_id as u8, raw_position as i16, 0, 0) + } else if let Some(velocity) = cmd.velocity { + // Set speed and direction + let speed = velocity.abs() as u16; + let direction = if velocity >= 0.0 { + ServoDirection::Clockwise + } else { + ServoDirection::Counterclockwise + }; + servo.set_speed(cmd.actuator_id as u8, speed, direction) + } else { + Ok(()) // No command specified + }; + + let success = result.is_ok(); + let error = result.err().map(|e| KosError { + code: ErrorCode::HardwareFailure as i32, + message: e.to_string(), + }); + + results.push(ActionResult { + actuator_id: cmd.actuator_id, + success, + error, + }); + } + + Ok(results) + } + + async fn configure_actuator( + &self, + config: ConfigureActuatorRequest, + ) -> Result { + let servo = self.servo.lock().map_err(|_| Status::internal("Lock error"))?; + + // Unlock EEPROM for writing + servo.write(config.actuator_id as u8, ServoRegister::LockMark, &[0]) + .map_err(|e| Status::internal(e.to_string()))?; + + let mut result = Ok(()); + + // Apply configurations + if let Some(kp) = config.kp { + result = result.and(servo.write(config.actuator_id as u8, ServoRegister::PProportionalCoeff, &[kp as u8])); + } + if let Some(ki) = config.ki { + result = result.and(servo.write(config.actuator_id as u8, ServoRegister::IIntegralCoeff, &[ki as u8])); + } + if let Some(kd) = config.kd { + result = result.and(servo.write(config.actuator_id as u8, ServoRegister::DDifferentialCoeff, &[kd as u8])); + } + if let Some(torque_enabled) = config.torque_enabled { + let mode = if torque_enabled { TorqueMode::Enabled } else { TorqueMode::Disabled }; + result = result.and(servo.set_torque_mode(config.actuator_id as u8, mode)); + } + + // Lock EEPROM after writing + servo.write(config.actuator_id as u8, ServoRegister::LockMark, &[1]) + .map_err(|e| Status::internal(e.to_string()))?; + + match result { + Ok(_) => Ok(ActionResponse { success: true, error: None }), + Err(e) => Ok(ActionResponse { success: false, error: Some(KosError { code: ErrorCode::HardwareFailure as i32, message: e.to_string() }) }), + } + } + + async fn calibrate_actuator( + &self, + request: CalibrateActuatorRequest, + ) -> Result { + Ok(Operation::default()) + } + + async fn get_actuators_state( + &self, + actuator_ids: Vec, + ) -> Result> { + let servo = self.servo.lock().map_err(|_| Status::internal("Lock error"))?; + + let mut states = Vec::new(); + for id in actuator_ids { + if let Ok(info) = servo.read_info(id as u8) { + states.push(ActuatorStateResponse { + actuator_id: id, + online: true, + position: Some(Servo::raw_to_degrees(info.current_location as u16) as f64), + velocity: Some({ + let speed_raw = info.current_speed as u16; + let speed_magnitude = speed_raw & 0x7FFF; + let speed_sign = if speed_raw & 0x8000 != 0 { -1.0 } else { 1.0 }; + speed_sign * (speed_magnitude as f32 * 360.0 / 4096.0) as f64 + }), + torque: None, + temperature: Some(info.current_temperature as f64), + voltage: Some(info.current_voltage as f32 / 10.0), + current: Some(info.current_current as f32 / 100.0), + }); + } else { + states.push(ActuatorStateResponse { + actuator_id: id, + online: false, + position: None, + velocity: None, + torque: None, + temperature: None, + voltage: None, + current: None, + }); + } + } + + Ok(states) + } +} diff --git a/platforms/zeroth-01/src/firmware/feetech.rs b/platforms/zeroth-01/src/firmware/feetech.rs new file mode 100644 index 0000000..4eb6396 --- /dev/null +++ b/platforms/zeroth-01/src/firmware/feetech.rs @@ -0,0 +1,262 @@ +use eyre::Result; +use std::os::raw::{c_int, c_short, c_uchar, c_ushort, c_uint}; +use crate::hal::{ServoInfo, ServoData, ServoMultipleWriteCommand, ServoMode, ServoDirection, ServoRegister, MemoryLockState, TorqueMode, MAX_SERVOS}; +use std::sync::{Arc, Mutex}; +use std::fmt; + +#[link(name = "sts3215")] +extern "C" { + fn servo_init() -> c_int; + fn servo_deinit(); + fn servo_write(id: c_uchar, address: c_uchar, data: *const c_uchar, length: c_uchar) -> c_int; + fn servo_read(id: c_uchar, address: c_uchar, length: c_uchar, data: *mut c_uchar) -> c_int; + fn servo_move(id: c_uchar, position: c_short, time: c_ushort, speed: c_ushort) -> c_int; + fn enable_servo_readout() -> c_int; + fn disable_servo_readout() -> c_int; + fn enable_servo_movement() -> c_int; + fn disable_servo_movement() -> c_int; + fn set_servo_mode(id: c_uchar, mode: c_uchar) -> c_int; + fn set_servo_speed(id: c_uchar, speed: c_ushort, direction: c_int) -> c_int; + fn servo_read_info(id: c_uchar, info: *mut ServoInfo) -> c_int; + fn read_servo_positions(servo_data: *mut ServoData) -> c_int; + fn servo_write_multiple(cmd: *const ServoMultipleWriteCommand) -> c_int; +} + +#[derive(Debug)] +pub struct Servo { + _private: (), // Prevent direct construction +} + +impl Servo { + pub fn new() -> Result { + let result = unsafe { servo_init() }; + if result != 0 { + eyre::bail!("Failed to initialize servo"); + } + Ok(Servo { _private: () }) + } + + pub fn write(&self, id: u8, register: ServoRegister, data: &[u8]) -> Result<()> { + let _result = unsafe { servo_write(id, register.clone() as u8, data.as_ptr(), data.len() as c_uchar) }; + let result = unsafe { servo_write(id, register as u8, data.as_ptr(), data.len() as c_uchar) }; + + if result != 0 { + eyre::bail!("Failed to write to servo"); + } + Ok(()) + } + + pub fn read(&self, id: u8, register: ServoRegister, length: u8) -> Result> { + let mut data = vec![0u8; length as usize]; + let result = unsafe { servo_read(id, register as u8, length, data.as_mut_ptr()) }; + if result != 0 { + eyre::bail!("Failed to read from servo"); + } + Ok(data) + } + + pub fn move_servo(&self, id: u8, position: i16, time: u16, speed: u16) -> Result<()> { + let result = unsafe { servo_move(id, position, time, speed) }; + if result != 0 { + eyre::bail!("Failed to move servo"); + } + Ok(()) + } + + pub fn enable_readout(&self) -> Result<()> { + let result = unsafe { enable_servo_readout() }; + if result != 0 { + eyre::bail!("Failed to enable servo readout"); + } + Ok(()) + } + + pub fn disable_readout(&self) -> Result<()> { + let result = unsafe { disable_servo_readout() }; + if result != 0 { + eyre::bail!("Failed to disable servo readout"); + } + Ok(()) + } + + pub fn enable_movement(&self) -> Result<()> { + let result = unsafe { enable_servo_movement() }; + if result != 0 { + eyre::bail!("Failed to enable servo movement"); + } + Ok(()) + } + + pub fn disable_movement(&self) -> Result<()> { + let result = unsafe { disable_servo_movement() }; + if result != 0 { + eyre::bail!("Failed to disable servo movement"); + } + Ok(()) + } + + pub fn set_mode(&self, id: u8, mode: ServoMode) -> Result<()> { + let result = unsafe { set_servo_mode(id, mode as u8) }; + if result != 0 { + eyre::bail!("Failed to set servo mode"); + } + Ok(()) + } + + pub fn set_speed(&self, id: u8, speed: u16, direction: ServoDirection) -> Result<()> { + let direction = if direction == ServoDirection::Clockwise { 1 } else { -1 }; + let result = unsafe { set_servo_speed(id, speed, direction as i32) }; + if result != 0 { + eyre::bail!("Failed to set servo speed"); + } + Ok(()) + } + + pub fn read_info(&self, id: u8) -> Result { + let mut info = ServoInfo { + torque_switch: 0, + acceleration: 0, + target_location: 0, + running_time: 0, + running_speed: 0, + torque_limit: 0, + reserved1: [0; 6], + lock_mark: 0, + current_location: 0, + current_speed: 0, + current_load: 0, + current_voltage: 0, + current_temperature: 0, + async_write_flag: 0, + servo_status: 0, + mobile_sign: 0, + reserved2: [0; 2], + current_current: 0, + }; + let result = unsafe { servo_read_info(id, &mut info) }; + if result != 0 { + eyre::bail!("Failed to read servo info"); + } + Ok(info) + } + + pub fn read_continuous(&self) -> Result { + let mut data = ServoData { + servo: [ServoInfo { + torque_switch: 0, + acceleration: 0, + target_location: 0, + running_time: 0, + running_speed: 0, + torque_limit: 0, + reserved1: [0; 6], + lock_mark: 0, + current_location: 0, + current_speed: 0, + current_load: 0, + current_voltage: 0, + current_temperature: 0, + async_write_flag: 0, + servo_status: 0, + mobile_sign: 0, + reserved2: [0; 2], + current_current: 0, + }; MAX_SERVOS], + task_run_count: 0, + }; + let result = unsafe { read_servo_positions(&mut data) }; + if result != 0 { + eyre::bail!("Failed to read continuous servo data"); + } + Ok(data) + } + + pub fn write_multiple(&self, cmd: &ServoMultipleWriteCommand) -> Result<()> { + let result = unsafe { servo_write_multiple(cmd) }; + if result != 0 { + eyre::bail!("Failed to write multiple servo positions"); + } + Ok(()) + } + + pub fn read_pid(&self, id: u8) -> Result<(u8, u8, u8)> { + let p = self.read(id, ServoRegister::PProportionalCoeff, 1)?[0]; + let i = self.read(id, ServoRegister::IIntegralCoeff, 1)?[0]; + let d = self.read(id, ServoRegister::DDifferentialCoeff, 1)?[0]; + Ok((p, i, d)) + } + + pub fn set_pid(&self, id: u8, p: u8, i: u8, d: u8) -> Result<()> { + // Unlock flash + self.write(id, ServoRegister::LockMark, &[MemoryLockState::Unlocked as u8])?; + + // Set PID parameters + self.write(id, ServoRegister::PProportionalCoeff, &[p])?; + self.write(id, ServoRegister::IIntegralCoeff, &[i])?; + self.write(id, ServoRegister::DDifferentialCoeff, &[d])?; + + // Lock flash + self.write(id, ServoRegister::LockMark, &[MemoryLockState::Locked as u8])?; + + Ok(()) + } + + pub fn set_memory_lock(&self, id: u8, state: MemoryLockState) -> Result<()> { + self.write(id, ServoRegister::LockMark, &[state as u8]) + } + + pub fn read_angle_limits(&self, id: u8) -> Result<(i16, i16)> { + let min_limit = i16::from_le_bytes(self.read(id, ServoRegister::MinAngleLimit, 2)?.try_into().unwrap()); + let max_limit = i16::from_le_bytes(self.read(id, ServoRegister::MaxAngleLimit, 2)?.try_into().unwrap()); + Ok((min_limit, max_limit)) + } + + pub fn set_torque_mode(&self, id: u8, mode: TorqueMode) -> Result<()> { + self.write(id, ServoRegister::TorqueSwitch, &[mode as u8]) + } + + pub fn write_servo_memory(&self, id: u8, register: ServoRegister, value: u16) -> Result<()> { + let data = [(value & 0xFF) as u8, ((value >> 8) & 0xFF) as u8]; + self.write(id, register, &data) + } + + pub fn scan(&self, id: u8) -> Result { + // Try to read the servo ID from memory address 0x5 (ServoRegister::ID) + match self.read(id, ServoRegister::ID, 1) { + Ok(data) if data.len() == 1 && data[0] == id => Ok(true), + Ok(_) => Ok(false), // Received data, but it doesn't match the ID + Err(_) => Ok(false), // No response, assume no servo at this ID + } + } + + pub fn degrees_to_raw(degrees: f32) -> u16 { + // Ensure the input is within the valid range + let clamped_degrees = degrees.max(-180.0).min(180.0); + + // Convert degrees to raw value + let raw = (clamped_degrees + 180.0) / 360.0 * 4096.0; + + // Round to nearest integer and ensure it's within the valid range + raw.round().max(0.0).min(4095.0) as u16 + } + + pub fn raw_to_degrees(raw: u16) -> f32 { + // Ensure the input is within the valid range + let clamped_raw = raw.max(0).min(4095); + + // Convert raw value to degrees + let degrees = (clamped_raw as f32 / 4096.0) * 360.0 - 180.0; + + // Round to two decimal places + (degrees * 100.0).round() / 100.0; + + // clamp to -180.0 to 180.0 + degrees.max(-180.0).min(180.0) + } +} + +impl Drop for Servo { + fn drop(&mut self) { + unsafe { servo_deinit() }; + } +} \ No newline at end of file diff --git a/platforms/zeroth-01/src/firmware/mod.rs b/platforms/zeroth-01/src/firmware/mod.rs new file mode 100644 index 0000000..f23c246 --- /dev/null +++ b/platforms/zeroth-01/src/firmware/mod.rs @@ -0,0 +1,152 @@ +mod feetech; + +pub use feetech::Servo; + +pub mod hal { + use std::os::raw::{c_short, c_uchar, c_ushort, c_uint}; + use serde::{Serialize, Deserialize}; + + + pub const MAX_SERVOS: usize = 16; + + #[repr(C)] + #[derive(Debug, Copy, Clone, Default, Serialize, Deserialize)] + pub struct ServoInfo { + pub torque_switch: c_uchar, + pub acceleration: c_uchar, + pub target_location: c_short, + pub running_time: c_ushort, + pub running_speed: c_ushort, + pub torque_limit: c_ushort, + pub reserved1: [c_uchar; 6], + pub lock_mark: c_uchar, + pub current_location: c_short, + pub current_speed: c_short, + pub current_load: c_short, + pub current_voltage: c_uchar, + pub current_temperature: c_uchar, + pub async_write_flag: c_uchar, + pub servo_status: c_uchar, + pub mobile_sign: c_uchar, + pub reserved2: [c_uchar; 2], + pub current_current: c_ushort, + } + + #[repr(u8)] + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub enum ServoRegister { + FirmwareMajorVersion = 0x00, + FirmwareSubVersion = 0x01, + ServoMainVersion = 0x03, + ServoSubVersion = 0x04, + ID = 0x05, + BaudRate = 0x06, + ReturnDelay = 0x07, + ResponseStatusLevel = 0x08, + MinAngleLimit = 0x09, + MaxAngleLimit = 0x0B, + MaxTemperatureLimit = 0x0D, + MaxInputVoltage = 0x0E, + MinInputVoltage = 0x0F, + MaxTorque = 0x10, + Phase = 0x12, + UnloadingCondition = 0x13, + LEDAlarmCondition = 0x14, + PProportionalCoeff = 0x15, + DDifferentialCoeff = 0x16, + IIntegralCoeff = 0x17, + MinStartupForce = 0x18, + ClockwiseInsensitiveArea = 0x1A, + CounterclockwiseInsensitiveArea = 0x1B, + ProtectionCurrent = 0x1C, + AngularResolution = 0x1E, + PositionCorrection = 0x1F, + OperationMode = 0x21, + ProtectiveTorque = 0x22, + ProtectionTime = 0x23, + OverloadTorque = 0x24, + SpeedClosedLoopPCoeff = 0x25, + OverCurrentProtectionTime = 0x26, + VelocityClosedLoopICoeff = 0x27, + TorqueSwitch = 0x28, + Acceleration = 0x29, + TargetLocation = 0x2A, + RunningTime = 0x2C, + RunningSpeed = 0x2E, + TorqueLimit = 0x30, + LockMark = 0x37, + CurrentLocation = 0x38, + CurrentSpeed = 0x3A, + CurrentLoad = 0x3C, + CurrentVoltage = 0x3E, + CurrentTemperature = 0x3F, + AsyncWriteFlag = 0x40, + ServoStatus = 0x41, + MobileSign = 0x42, + CurrentCurrent = 0x45, + } + + #[repr(u8)] + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub enum MemoryLockState { + Unlocked = 0, + Locked = 1, + } + + #[repr(C)] + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub struct ServoData { + pub servo: [ServoInfo; MAX_SERVOS], + pub task_run_count: c_uint, + } + + #[repr(C)] + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub struct ServoMultipleWriteCommand { + pub only_write_positions: c_uchar, + pub ids: [c_uchar; MAX_SERVOS], + pub positions: [c_short; MAX_SERVOS], + pub times: [c_ushort; MAX_SERVOS], + pub speeds: [c_ushort; MAX_SERVOS], + } + + #[repr(u8)] + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub enum ServoMode { + Position = 0, + ConstantSpeed = 1, + PWMOpenLoop = 2, + StepServo = 3, + } + + #[repr(i32)] + #[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)] + pub enum ServoDirection { + Clockwise = 0, + Counterclockwise = 1, + } + + #[repr(u8)] + #[derive(Debug, Copy, Clone, Serialize, Deserialize)] + pub enum TorqueMode { + Disabled = 0, + Enabled = 1, + Stiff = 2, + } + + impl PartialEq for TorqueMode { + fn eq(&self, other: &Self) -> bool { + self == other + } + } + + #[derive(Debug, Clone, Copy, Serialize, Deserialize)] + pub struct IMUData { + pub acc_x: f32, + pub acc_y: f32, + pub acc_z: f32, + pub gyro_x: f32, + pub gyro_y: f32, + pub gyro_z: f32, + } +} \ No newline at end of file diff --git a/platforms/zeroth-01/src/lib.rs b/platforms/zeroth-01/src/lib.rs index 8b13789..e6a9a0c 100644 --- a/platforms/zeroth-01/src/lib.rs +++ b/platforms/zeroth-01/src/lib.rs @@ -1 +1,59 @@ +mod actuator; +mod firmware; +pub use actuator::*; +pub use firmware::*; + +use kos_core::{Platform, ServiceEnum}; +use tonic::async_trait; +use std::sync::Arc; +use kos_core::services::{ActuatorServiceImpl, OperationsServiceImpl}; +use kos_core::kos_proto::actuator::actuator_service_server::ActuatorServiceServer; +use std::future::Future; +use std::pin::Pin; +use std::time::Duration; + +pub struct ZBotPlatform {} + +impl ZBotPlatform { + pub fn new() -> Self { + Self {} + } +} + +impl Default for ZBotPlatform { + fn default() -> Self { + ZBotPlatform::new() + } +} + +#[async_trait] +impl Platform for ZBotPlatform { + fn name(&self) -> &'static str { + "ZBot" + } + + fn serial(&self) -> String { + "00000000".to_string() + } + + fn initialize(&mut self, _operations_service: Arc) -> eyre::Result<()> { + Ok(()) + } + + fn create_services<'a>( + &'a self, + _operations_service: Arc, + ) -> Pin>> + Send + 'a>> { + Box::pin(async move { + let actuator = ZBotActuator::new().await?; + Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( + ActuatorServiceImpl::new(Arc::new(actuator)) + ))]) + }) + } + + fn shutdown(&mut self) -> eyre::Result<()> { + Ok(()) + } +}