From 328952659a64576ec7200a07c524d79c6af02496 Mon Sep 17 00:00:00 2001
From: budzianowski <budzianowski@gmail.com>
Date: Thu, 10 Oct 2024 16:39:57 -0700
Subject: [PATCH 1/2] add initial setup

---
 actuator/rust/bindings/src/lib.rs         |  5 +++++
 actuator/rust/robstride/src/bin/motors.rs |  4 ++--
 actuator/rust/robstride/src/lib.rs        | 27 +++++++++++++++++++++++
 3 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs
index 5755446..be37c14 100644
--- a/actuator/rust/bindings/src/lib.rs
+++ b/actuator/rust/bindings/src/lib.rs
@@ -99,6 +99,11 @@ impl PyRobstrideMotors {
             .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
     }
 
+    fn set_new_id(&mut self, motor_id: u8, new_motor_id: u8) -> PyResult<()> {
+        self.inner.set_new_id(motor_id, new_motor_id);
+        Ok(())
+    }
+
     fn __repr__(&self) -> PyResult<String> {
         let motor_count = self.inner.get_latest_feedback().len();
         Ok(format!("PyRobstrideMotors(motor_count={})", motor_count))
diff --git a/actuator/rust/robstride/src/bin/motors.rs b/actuator/rust/robstride/src/bin/motors.rs
index 68a0edf..dbed306 100644
--- a/actuator/rust/robstride/src/bin/motors.rs
+++ b/actuator/rust/robstride/src/bin/motors.rs
@@ -95,7 +95,7 @@ fn main() -> Result<(), Box<dyn Error>> {
     let motor_type = motor_type_from_str(motor_type_input.as_str())?;
 
     // Create motor instances
-    let mut motors = Motors::new(&port_name, HashMap::from([(test_id, motor_type)]))?;
+    let mut motors = Motors::new(&port_name, &HashMap::from([(test_id, motor_type)]))?;
 
     let mut last_command: i32 = -1;
 
@@ -136,4 +136,4 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     println!("Exiting program.");
     Ok(())
-}
+}
\ No newline at end of file
diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs
index 42e7fc0..429b201 100644
--- a/actuator/rust/robstride/src/lib.rs
+++ b/actuator/rust/robstride/src/lib.rs
@@ -667,6 +667,33 @@ impl Motors {
             .get(&motor_id)
             .ok_or_else(|| std::io::Error::new(std::io::ErrorKind::NotFound, "No feedback found"))
     }
+
+    pub fn set_new_id(&mut self, motor_id: u8, new_motor_id: u8) -> Result<HashMap<u8, MotorFeedback>, std::io::Error> {
+        let motor_ids = self.motor_configs.keys().cloned().collect::<Vec<u8>>();
+
+        for id in motor_ids {
+            if id == motor_id {
+                let mut pack = CanPack {
+                    ex_id: ExId {
+                    id: motor_id,
+                    data: 0,
+                    mode: CanComMode::MotorId,
+                    res: 0,
+                },
+                len: 8,
+                data: vec![0; 8],
+                };
+                
+                pack.ex_id.data = ((new_motor_id as u16) << 8) | (motor_id as u16);
+                self.send_command(&pack);
+                break;
+            } 
+        };
+
+        // After sending the reset command, sleep for a short time.
+        std::thread::sleep(self.sleep_time);
+        self.read_all_pending_responses()
+    }
 }
 
 fn get_default_kp_kd_values(motor_infos: &HashMap<u8, MotorType>) -> HashMap<u8, (f32, f32)> {

From c58270ad58f969e993a1b0a16a173761487259a5 Mon Sep 17 00:00:00 2001
From: budzianowski <budzianowski@gmail.com>
Date: Thu, 10 Oct 2024 17:43:56 -0700
Subject: [PATCH 2/2] add example

---
 examples/supervisor.py | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/examples/supervisor.py b/examples/supervisor.py
index c3b0367..c9e1bf3 100644
--- a/examples/supervisor.py
+++ b/examples/supervisor.py
@@ -4,27 +4,27 @@
 import math
 import time
 
-from actuator import RobstrideMotorsSupervisor
+from actuator import RobstrideMotors
 
 
 def main() -> None:
     parser = argparse.ArgumentParser()
-    parser.add_argument("--port-name", type=str, default="/dev/ttyUSB0")
-    parser.add_argument("--motor-id", type=int, default=1)
-    parser.add_argument("--motor-type", type=str, default="01")
+    parser.add_argument("--port-name", type=str, default="/dev/tty.usbserial-210")
+    parser.add_argument("--motor-id", type=int, default=127)
+    parser.add_argument("--motor-type", type=str, default="04")
     args = parser.parse_args()
 
-    supervisor = RobstrideMotorsSupervisor(args.port_name, {args.motor_id: args.motor_type})
-    supervisor.add_motor_to_zero(args.motor_id)
+    supervisor = RobstrideMotors(args.port_name, {args.motor_id: args.motor_type})
+    supervisor.send_get_mode()
     start_time = time.time()
 
     try:
         while True:
             time.sleep(0.25)
             target_position = 0.5 * math.sin(time.time() - start_time)
-            supervisor.set_target_position(args.motor_id, target_position)
-            # feedback = supervisor.get_latest_feedback()
-            # print(feedback)
+            # supervisor.set_target_position(args.motor_id, target_position)
+            feedback = supervisor.get_latest_feedback()
+            print(feedback)
 
     except KeyboardInterrupt:
         supervisor.stop()