From c3ca8937dd0bd045f61bbff854ca412821ab2a4b Mon Sep 17 00:00:00 2001
From: Denys Bezmenov <denys@kscale.dev>
Date: Thu, 14 Nov 2024 16:42:31 -0800
Subject: [PATCH] working control

---
 platforms/kbot/src/actuator.rs | 99 ++++++++++++++++++++++++++++++----
 platforms/kbot/src/lib.rs      |  3 ++
 2 files changed, 93 insertions(+), 9 deletions(-)

diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs
index 76dd3d7..bef2336 100644
--- a/platforms/kbot/src/actuator.rs
+++ b/platforms/kbot/src/actuator.rs
@@ -3,7 +3,7 @@ use async_trait::async_trait;
 use eyre::{Result, WrapErr};
 use kos_core::{
     hal::{ActionResponse, Actuator, ActuatorCommand, CalibrateActuatorRequest},
-    kos_proto::{actuator::*, common::ActionResult},
+    kos_proto::{actuator::*, common::{ActionResult, ErrorCode}, common::Error as KosError },
 };
 use std::collections::HashMap;
 
@@ -48,19 +48,100 @@ impl KBotActuator {
 impl Actuator for KBotActuator {
     async fn command_actuators(
         &self,
-        _commands: Vec<ActuatorCommand>,
+        commands: Vec<ActuatorCommand>,
     ) -> Result<Vec<ActionResult>> {
-        Ok(vec![])
+        let mut results = vec![];
+        for command in commands {
+            let mut motor_result = vec![];
+            if let Some(position) = command.position {
+                let result = self.motors_supervisor.set_position(
+                    command.actuator_id as u8,
+                    position as f32 * std::f32::consts::PI / 180.0,
+                );
+                motor_result.push(result);
+            }
+            if let Some(velocity) = command.velocity {
+                let result = self.motors_supervisor.set_velocity(
+                    command.actuator_id as u8,
+                    velocity as f32,
+                );
+                motor_result.push(result);
+            }
+            if let Some(torque) = command.torque {
+                let result = self.motors_supervisor.set_torque(
+                    command.actuator_id as u8,
+                    torque as f32,
+                );
+                motor_result.push(result);
+            }
+
+            let success = motor_result.iter().all(|r| r.is_ok());
+            let error = if !success {
+                Some(KosError {
+                    code: if motor_result.iter().any(|r| matches!(r, Err(e) if e.kind() == std::io::ErrorKind::NotFound)) {
+                        ErrorCode::InvalidArgument as i32
+                    } else {
+                        ErrorCode::HardwareFailure as i32
+                    },
+                    message: motor_result
+                        .iter()
+                        .filter_map(|r| r.as_ref().err())
+                        .map(|e| e.to_string())
+                        .collect::<Vec<_>>()
+                        .join("; "),
+                })
+            } else {
+                None
+            };
+
+            results.push(ActionResult {
+                actuator_id: command.actuator_id,
+                success,
+                error,
+            });
+        }
+        Ok(results)
     }
 
     async fn configure_actuator(
         &self,
-        _config: ConfigureActuatorRequest,
+        config: ConfigureActuatorRequest,
     ) -> Result<ActionResponse> {
-        Ok(ActionResponse {
-            success: true,
-            error: None,
-        })
+        let motor_id = config.actuator_id as u8;
+        let mut results = vec![];
+
+        // Configure KP if provided
+        if let Some(kp) = config.kp {
+            let result = self.motors_supervisor.set_kp(motor_id, kp as f32);
+            results.push(result);
+        }
+
+        // Configure KD if provided
+        if let Some(kd) = config.kd {
+            let result = self.motors_supervisor.set_kd(motor_id, kd as f32);
+            results.push(result);
+        }
+
+        let success = results.iter().all(|r| r.is_ok());
+        let error = if !success {
+            Some(kos_core::kos_proto::common::Error {
+                code: if results.iter().any(|r| matches!(r, Err(e) if e.kind() == std::io::ErrorKind::NotFound)) {
+                    ErrorCode::InvalidArgument as i32
+                } else {
+                    ErrorCode::HardwareFailure as i32
+                },
+                message: results
+                    .iter()
+                    .filter_map(|r| r.as_ref().err())
+                    .map(|e| e.to_string())
+                    .collect::<Vec<_>>()
+                    .join("; "),
+            })
+        } else {
+            None
+        };
+
+        Ok(ActionResponse { success, error })
     }
 
     async fn calibrate_actuator(&self, _request: CalibrateActuatorRequest) -> Result<Operation> {
@@ -77,7 +158,7 @@ impl Actuator for KBotActuator {
             .map(|(id, state)| ActuatorStateResponse {
                 actuator_id: u32::from(*id),
                 online: matches!(state.mode, robstride::MotorMode::Motor),
-                position: Some(state.position as f64),
+                position: Some((state.position as f64) * 180.0 / std::f64::consts::PI),
                 velocity: Some(state.velocity as f64),
                 torque: Some(state.torque as f64),
                 temperature: None,
diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs
index 08255a8..d32ed3c 100644
--- a/platforms/kbot/src/lib.rs
+++ b/platforms/kbot/src/lib.rs
@@ -62,6 +62,9 @@ impl Platform for KbotPlatform {
                             (1, MotorType::Type04),
                             (2, MotorType::Type04),
                             (3, MotorType::Type04),
+                            (4, MotorType::Type04),
+                            (5, MotorType::Type04),
+                            (6, MotorType::Type01),
                         ]),
                         None,
                         None,