work in progress

.cargo/config.toml

@@ -4,3 +4,7 @@ rustflags = [
   "-C", "link-arg=-undefined",
   "-C", "link-arg=dynamic_lookup",
 ]
+
+[target.aarch64-unknown-linux-gnu]
+
+linker = "aarch64-linux-gnu-gcc"

Cargo.toml

@@ -2,7 +2,7 @@
 
 members = [
     "kscaleos/bindings",
-    "kscaleos/rust",
+    "kscaleos/kscaleos",
 ]
 resolver = "2"
 
@@ -14,5 +14,5 @@ edition = "2021"
 license = "MIT"
 repository = "https://github.com/kscalelabs/kscaleos"
 description = "The K-Scale Operating System"
-documentation = "https://docs.kscale.dev/software/actuators/overview"
+documentation = "https://docs.kscale.dev/kscaleos/intro"
 readme = "README.md"

configs/torso.yaml

@@ -0,0 +1,47 @@
+limbs:
+  LeftArm:
+    port_name: /dev/ttyCH341USB0
+    motor_configs:
+      1:
+        motor_type: TYPE03
+        kp: 50.0
+        kd: 1.0
+      2:
+        motor_type: TYPE03
+        kp: 50.0
+        kd: 1.0
+      3:
+        motor_type: TYPE02
+        kp: 50.0
+        kd: 1.0
+      4:
+        motor_type: TYPE02
+        kp: 50.0
+        kd: 1.0
+      5:
+        motor_type: TYPE01
+        kp: 50.0
+        kd: 1.0
+  RightArm:
+    port_name: /dev/ttyCH341USB0
+    motor_configs:
+      1:
+        motor_type: TYPE03
+        kp: 50.0
+        kd: 1.0
+      2:
+        motor_type: TYPE03
+        kp: 50.0
+        kd: 1.0
+      3:
+        motor_type: TYPE02
+        kp: 50.0
+        kd: 1.0
+      4:
+        motor_type: TYPE02
+        kp: 50.0
+        kd: 1.0
+      5:
+        motor_type: TYPE01
+        kp: 50.0
+        kd: 1.0

kscaleos/bindings/Cargo.toml

@@ -20,7 +20,7 @@ pyo3 = { version = ">= 0.21.0", features = ["extension-module"] }
 pyo3-stub-gen = ">= 0.6.0"
 
 # Other packages in the workspace.
-kscaleos = { path = "../rust" }
+kscaleos = { path = "../kscaleos" }
 
 [[bin]]
 

kscaleos/kscaleos/Cargo.toml

@@ -0,0 +1,35 @@
+[package]
+
+name = "kscaleos"
+version.workspace = true
+description.workspace = true
+readme.workspace = true
+authors.workspace = true
+edition.workspace = true
+repository.workspace = true
+license.workspace = true
+
+[lib]
+
+name = "kscaleos"
+crate-type = ["cdylib", "rlib"]
+
+[[bin]]
+
+name = "kscaleos"
+path = "src/bin.rs"
+
+[dependencies]
+robstride = "0.2.1"
+tonic = "0.12.3"
+tonic-web = "0.12.3"
+
+# Serde is used for parsing configuration files.
+serde = { version = "1.0", features = ["derive"] }
+serde_yaml = "0.9"
+
+[target.'cfg(any(target_os = "macos", target_os = "linux"))'.dependencies]
+ort = { version = "1.16.3", optional = true }
+
+[build-dependencies]
+tonic-build = "0.12.3"

kscaleos/kscaleos/src/config.rs

@@ -0,0 +1,37 @@
+use robstride::MotorType;
+use serde::{Deserialize, Serialize};
+use std::collections::HashMap;
+use std::fs;
+
+#[derive(Serialize, Deserialize, Debug)]
+pub struct MotorConfig {
+    pub motor_type: MotorType,
+    pub kp: f32,
+    pub kd: f32,
+}
+
+#[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Hash)]
+pub enum Limb {
+    LeftArm,
+    RightArm,
+    LeftLeg,
+    RightLeg,
+}
+
+#[derive(Serialize, Deserialize, Debug)]
+pub struct LimbConfig {
+    pub motor_configs: HashMap<u8, MotorConfig>,
+    pub port_name: String,
+}
+
+#[derive(Serialize, Deserialize, Debug)]
+pub struct Config {
+    pub limbs: HashMap<Limb, LimbConfig>,
+}
+
+impl Config {
+    pub fn new(config_path: &str) -> Self {
+        let file = fs::File::open(config_path).expect("Failed to open config file");
+        serde_yaml::from_reader(file).expect("Failed to parse config YAML")
+    }
+}

kscaleos/kscaleos/src/lib.rs

@@ -0,0 +1,83 @@
+use robstride::{MotorType, MotorsSupervisor};
+use std::collections::HashMap;
+use std::sync::{Arc, RwLock};
+
+mod config;
+mod runner;
+mod state;
+
+fn main() {
+    let config = config::Config::new("config.yaml");
+    let state = Arc::new(RwLock::new(State::new()));
+
+    // Four motor supervisors, one for each limb.
+    let mut motor_supervisors = vec![];
+    for i in 0..4 {
+        let port_name = format!("/dev/ttyCH341USB{}", i);
+
+        let motor_infos = match i {
+            // Left arm.
+            0 => HashMap::from([
+                (1, MotorType::Type03),
+                (2, MotorType::Type03),
+                (3, MotorType::Type02),
+                (4, MotorType::Type02),
+                (5, MotorType::Type01),
+            ]),
+            // Right arm.
+            1 => HashMap::from([
+                (1, MotorType::Type03),
+                (2, MotorType::Type03),
+                (3, MotorType::Type02),
+                (4, MotorType::Type02),
+                (5, MotorType::Type01),
+            ]),
+            // Left leg.
+            2 => HashMap::from([
+                (1, MotorType::Type04),
+                (2, MotorType::Type03),
+                (3, MotorType::Type03),
+                (4, MotorType::Type03),
+                (5, MotorType::Type01),
+            ]),
+            // Right leg.
+            3 => HashMap::from([
+                (1, MotorType::Type04),
+                (2, MotorType::Type03),
+                (3, MotorType::Type03),
+                (4, MotorType::Type03),
+                (5, MotorType::Type01),
+            ]),
+            _ => {
+                panic!("Invalid limb index: {}", i);
+            }
+        };
+
+        let motor_supervisor =
+            MotorsSupervisor::new(port_name.as_str(), &motor_infos, false, 1000.0, true).unwrap();
+        motor_supervisors.push(motor_supervisor);
+    }
+}
+
+struct State {}
+
+impl State {
+    fn new() -> Self {}
+
+    fn update(&mut self, _input: SensorData, _commands: Commands) {
+        // Update state logic
+    }
+}
+
+fn read_sensors() -> SensorData {
+    // Read from camera and motors
+    SensorData { /* fields */ }
+}
+
+fn send_commands(_commands: Commands) {
+    // Interface with motor drivers
+}
+
+struct SensorData {/* fields */}
+
+struct Commands {/* fields */}

kscaleos/kscaleos/src/runner.rs

@@ -0,0 +1,18 @@
+/**
+ * Defines the main control loop that runs in its own independent thread.
+ */
+use std::{
+    sync::{Arc, RwLock},
+    thread,
+    time::Duration,
+};
+
+use crate::state::State;
+
+pub fn run(state: Arc<RwLock<State>>) {
+    loop {
+        let state = state.read().unwrap();
+        println!("Running main control loop... State: {:?}", state);
+        thread::sleep(Duration::from_millis(10));
+    }
+}

kscaleos/kscaleos/src/state.rs

@@ -0,0 +1,13 @@
+/**
+ * Defines state variables that are passed between the runner loop and
+ * the calling process.
+ */
+
+#[derive(Debug)]
+pub struct State {}
+
+impl State {
+    pub fn new() -> Self {
+        State {}
+    }
+}