Add rustypot

node-hub/dora-rustypot/Cargo.toml

@@ -0,0 +1,12 @@
+[package]
+name = "dora-rustypot"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+dora-node-api = "0.3.11"
+eyre = "0.6.12"
+rustypot = { git = "https://github.com/pollen-robotics/rustypot", branch = "next-release-1.0" }
+serialport = "4.7.1"

node-hub/dora-rustypot/src/main.rs

@@ -0,0 +1,76 @@
+use dora_node_api::dora_core::config::DataId;
+use dora_node_api::{into_vec, DoraNode, Event, IntoArrow, Parameter};
+use eyre::{Context, Result};
+use rustypot::servo::feetech::sts3215::Sts3215Controller;
+use std::collections::BTreeMap;
+use std::time::Duration;
+
+fn main() -> Result<()> {
+    let (mut node, mut events) = DoraNode::init_from_env()?;
+    let serialportname: String = std::env::var("PORT").context("Serial port name not provided")?;
+    let baudrate: u32 = std::env::var("BAUDRATE")
+        .unwrap_or_else(|_| "1000000".to_string())
+        .parse()
+        .context("Invalid baudrate")?;
+    let ids = std::env::var("IDS")
+        .unwrap_or_else(|_| "1,2,3,4,5,6".to_string())
+        .split(&[',', ' '][..])
+        .map(|s| s.parse::<u8>().unwrap())
+        .collect::<Vec<u8>>();
+    let torque = std::env::var("TORQUE")
+        .unwrap_or_else(|_| "false".to_string())
+        .parse::<bool>()
+        .context("Invalid torque")?;
+
+    let serial_port = serialport::new(serialportname, baudrate)
+        .timeout(Duration::from_millis(1000))
+        .open()?;
+
+    let mut c = Sts3215Controller::new()
+        .with_protocol_v1()
+        .with_serial_port(serial_port);
+
+    if torque {
+        let truthies = vec![true; ids.len()];
+        c.write_torque_enable(&ids, &truthies)
+            .expect("could not enable torque");
+    } else {
+        let falsies = vec![false; ids.len()];
+        c.write_torque_enable(&ids, &falsies)
+            .expect("could not enable torque");
+    }
+
+    while let Some(event) = events.recv() {
+        match event {
+            Event::Input {
+                id,
+                metadata: _,
+                data,
+            } => match id.as_str() {
+                "tick" => {
+                    if let Ok(joints) = c.read_present_position(&ids) {
+                        let mut parameter = BTreeMap::new();
+                        parameter.insert(
+                            "encoding".to_string(),
+                            Parameter::String("jointstate".to_string()),
+                        );
+                        node.send_output(
+                            DataId::from("pose".to_string()),
+                            parameter,
+                            joints.into_arrow(),
+                        )
+                        .unwrap();
+                    };
+                }
+                "pose" => {
+                    let data: Vec<f64> = into_vec(&data).expect("could not cast values");
+                    c.write_goal_position(&ids, &data).unwrap();
+                }
+                other => eprintln!("Received input `{other}`"),
+            },
+            _ => {}
+        }
+    }
+
+    Ok(())
+}