Merge pull request #6 from SamuelNoesslboeck/logging

Improved logging

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "sybot_lib"
-version = "0.7.4"
+version = "0.7.5"
 edition = "2021"
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
@@ -21,10 +21,16 @@ path = "bin/webserver.rs"
 actix = "0.13.0"
 actix-web = "4"
 actix-web-actors = "4.1.0"
+colored = "2.0.0"
 
+colored_json = "3.0.1"
 gcode = "0.6.1"
 glam = "0.21.3"
 serde = { version = "1.0.104", features = ["derive"] }
 serde_json = "1.0.48"
 
-stepper_lib = { path = "../stepper_lib" } # { git = "https://github.com/SamuelNoesslboeck/stepper_lib" }
+stepper_lib = { path = "../stepper_lib" } # { git = "https://github.com/SamuelNoesslboeck/stepper_lib" }
+
+[features]
+default = [ "dbg-funcs" ]
+dbg-funcs = [ ]

bin/interpreter.rs

@@ -1,41 +1,77 @@
-use stepper_lib::JsonConfig;
-use sybot_lib::{init_intpr, SyArm, ConfRobot};
+use colored::Colorize; 
+use colored_json::to_colored_json_auto;
 
-// use std::{time::Duration, thread::sleep, f32::consts::PI};
+use sybot_lib::{init_intpr, SyArm, ConfRobot, JsonConfig};
 
 fn main() -> std::io::Result<()> {
     let mut syarm = SyArm::from_conf(
         JsonConfig::read_from_file("res/SyArm_Mk1.conf.json")
     )?;
 
+    // Print Header
+    println!("{}", "\n[SyArm - GCode Interpreter]".bold()); 
+    println!("Version: {}, (c) {}\n", "0.0.1".truecolor(0xEA, 0x8C, 0x43), "Samuel Nösslböck (Sy)".truecolor(0xEA, 0x8C, 0x43));
+
+    if cfg!(feature = "dbg-funcs") {
+        syarm.print_conf_header();
+        println!("");   // Newline for style
+    }
+
     // DEBUG
-        // Select pencil
-        syarm.set_tool_id(4);
+        // Select "NoTool"
+        syarm.set_tool_id(2);
     // 
 
     let mut intpr = init_intpr(syarm);
 
     let args : Vec<String> = std::env::args().collect();
+    let mut lines = Vec::new();
+    let mut lines_init_len = 0;
 
-    println!("SyArm - GCode Interpreter"); 
+    #[allow(unused_assignments)]
+    let mut file = String::new();
 
     if args.len() > 1 {
-        println!(" -> Interpreting file '{}'", args[1]);
-        intpr.interpret_file(args[1].as_str(), 
-            |_| { Err(std::io::Error::new(std::io::ErrorKind::InvalidInput, "Invalid GCode input")) });
+        println!(" -> Interpreting file {}", format!("\"{}\"", args[1]).green());
+
+        file = std::fs::read_to_string(args[1].as_str()).unwrap();
+        lines = file.split('\n').collect();
+        lines_init_len = lines.len();
+    } else {
+        println!("{}", "\nInterpreter Terminal".bold())
     }
 
+    println!("");
+
     loop {
         let mut line = String::new();
-        std::io::stdin().read_line(&mut line).unwrap();
 
-        match intpr.interpret(line.as_str(), |_| { Err(std::io::Error::new(std::io::ErrorKind::InvalidInput, "Invalid GCode input")) }).first().unwrap() {
-            Ok(j) => { 
-                println!("{}", j.to_string());
+        let lines_len = lines.len();
+        if lines_len > 0 {
+            line = String::from(lines.remove(0));
+        } else {
+            std::io::stdin().read_line(&mut line).unwrap();
+        }
+
+        match intpr.interpret(line.as_str(), 
+        |_| { 
+            Err(std::io::Error::new(std::io::ErrorKind::InvalidInput, "Invalid GCode input")) 
+        }).first() {
+            Some(res) => {
+                if lines_len > 0 {
+                    println!("{} {}", format!("{} |", lines_init_len - lines_len).bold(), line);
+                }
+
+                match res {
+                    Ok(j) => { 
+                        println!("{}\n", to_colored_json_auto(&j).unwrap());
+                    },
+                    Err(err) => {
+                        println!("{}\n", err);
+                    }
+                }
             },
-            Err(err) => {
-                println!("{}", err);
-            }
+            None => { }
         }
     }
 }

res/SyArm_Mk1.conf.json

@@ -1,6 +1,7 @@
 {
   "name": "SyArm_Mk1",
   "conf_version": "0.0.1/2023/02/21",
+  "author": "Samuel Nösslböck",
 
   "lk": {
     "u": 12,
@@ -63,7 +64,7 @@
         "l_b": 203.5
       },
       "ang": {
-        "offset": -2.282,
+        "offset": 2.282,
         "counter": true
       },
       "sim": {
@@ -142,7 +143,7 @@
       "obj": {
         "servo": {
           "data": "MG996R",
-          "pwm": 12
+          "pwm": 23
         }, 
         "length": 50.0,
         "mass": 0.1
@@ -155,7 +156,7 @@
         "axis": {
           "servo": {
             "data": "MG996R",
-            "pwm": 12
+            "pwm": 23
           }, 
           "length": 50.0,
           "mass": 0.1

res/basicYZpos.gcode → res/gcode/basicYZpos.gcode

@@ -2,4 +2,4 @@
 ; 18/12/2022
 
 G28; Measuring
-G0 X0 Y380 Z400
+G0 X0 Y330 Z400

res/gcode/pickUp.gcode

@@ -0,0 +1,19 @@
+; Basic Pickup script
+; 06/03/2023
+
+; Setup
+G28; Measure
+T1; Select Axisbearing tool
+M5; Make sure that the spindle is deactivated
+
+; Program
+G0 X-50 Y400 Z50; Drive to low position
+M3; Grab
+
+G0 X-50 Y400 Z50; Lift object
+G0 X50 Y400 Z50; Move object to the right
+
+G0 X50 Y400 Z0; Put object down
+M5; Release
+
+M30; Exit program

src/conf.rs

@@ -0,0 +1,120 @@
+extern crate alloc;
+use alloc::sync::Arc;
+
+use glam::{Mat3, Vec3};
+
+use stepper_lib::data::LinkedData;
+use stepper_lib::comp::Tool;
+use stepper_lib::units::*;
+
+// Submodules
+mod elem;
+use elem::*;
+
+mod json;
+pub use json::*;
+
+
+#[derive(Debug)]
+pub struct MachineConfig<const N : usize, const D : usize, const A : usize>
+{
+    pub name: String,
+
+    pub lk : Arc<LinkedData>,
+
+    pub anchor : Vec3,
+    pub dims : [Vec3; D],
+    pub axes : [Vec3; A],
+
+    pub tools : Vec<Box<dyn Tool + Send>>,
+
+    pub vels : [Omega; N],
+    pub home : [Gamma; N],
+    pub meas_dist : [Delta; N],
+
+    pub ang : [AngleData; N],
+    pub sim : [SimData; N],
+    pub meas : [MeasInstance; N],
+    pub limit : [LimitDecl; N]
+}
+
+impl<const N : usize, const D : usize, const A : usize> Default for MachineConfig<N, D, A> 
+{
+    fn default() -> Self {
+        Self {
+            name: String::new(),
+            lk: Default::default(),
+
+            anchor: Default::default(),
+            dims: [Default::default(); D],
+            axes: [Default::default(); A], 
+
+            tools: vec![],
+
+            vels: [Omega::ZERO; N],
+            home: [Gamma::ZERO; N],
+            meas_dist: [Delta::ZERO; N],
+
+            ang: [Default::default(); N],
+            sim: [Default::default(); N],
+            meas: [Default::default(); N],
+            limit: [Default::default(); N]
+        }
+    }
+}
+
+impl<const N : usize, const D : usize, const A : usize> MachineConfig<N, D, A>
+{
+    pub fn get_axes(&self, angles : &[Phi; A]) -> Vec<Mat3> {
+        let mut matr = vec![];
+
+        for i in 0 .. A {
+            let axis_vec = Vec3::from(self.axes[i]).normalize();
+
+            matr.push(
+                if axis_vec == Vec3::X {
+                    Mat3::from_rotation_x(angles[i].0)
+                } else if axis_vec == Vec3::Y {
+                    Mat3::from_rotation_y(angles[i].0)
+                } else if axis_vec == Vec3::Z {
+                    Mat3::from_rotation_z(angles[i].0)
+                } else {
+                    Mat3::ZERO
+                }
+            );
+        }
+
+        matr
+    }
+}
+
+impl<const N : usize, const D : usize, const A : usize> MachineConfig<N, D, A>
+{
+    pub fn gammas_from_phis(&self, phis : [Phi; N]) -> [Gamma; N] {
+        let mut gammas = [Gamma::ZERO; N];
+
+        for i in 0 .. N {
+            gammas[i] = if self.ang[i].counter { 
+                -phis[i].force_to_gamma() + self.ang[i].offset
+            } else { 
+                phis[i].force_to_gamma() - self.ang[i].offset
+            };
+        }
+
+        gammas
+    }
+
+    pub fn phis_from_gammas(&self, gammas : [Gamma; N]) -> [Phi; N] {
+        let mut phis = [Phi::ZERO; N];
+
+        for i in 0 .. N {
+            phis[i] = (if self.ang[i].counter { 
+                -gammas[i]
+            } else { 
+                gammas[i]
+            } + self.ang[i].offset).force_to_phi();
+        }
+
+        phis
+    }
+}