Skip to content

Commit

Permalink
Version 0.7.1
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamuelNoesslboeck
SamuelNoesslboeck committed Feb 15, 2023
1 parent a256033 commit 8787e96
Show file tree
Hide file tree
Showing 11 changed files with 136 additions and 252 deletions.
2 changes: 1 addition & 1 deletion Cargo.lock
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

2 changes: 1 addition & 1 deletion Cargo.toml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
[package]
name = "sybot_lib"
version = "0.7.0"
version = "0.7.1"
edition = "2021"

# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
Expand Down
8 changes: 4 additions & 4 deletions bin/interpreter.rs
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
use stepper_lib::JsonConfig;
use sybot_lib::{init_interpreter, SyArm, SyArmError, ErrType, Robot};
use sybot_lib::{init_intpr, SyArm, Robot};

// use std::{time::Duration, thread::sleep, f32::consts::PI};

Expand All @@ -13,7 +13,7 @@ fn main() -> std::io::Result<()> {
syarm.set_tool_id(1);
//

let mut intpr = init_interpreter(syarm);
let mut intpr = init_intpr(syarm);

let args : Vec<String> = std::env::args().collect();

Expand All @@ -22,14 +22,14 @@ fn main() -> std::io::Result<()> {
if args.len() > 1 {
println!(" -> Interpreting file '{}'", args[1]);
intpr.interpret_file(args[1].as_str(),
|_| { Err(SyArmError::new_simple(ErrType::GCodeFuncNotFound)) });
|_| { Err(std::io::Error::new(std::io::ErrorKind::InvalidInput, "Invalid GCode input")) });
}

loop {
let mut line = String::new();
std::io::stdin().read_line(&mut line).unwrap();

match intpr.interpret(line.as_str(), |_| { Err(SyArmError::new_simple(ErrType::GCodeFuncNotFound)) }).first().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());
},
Expand Down
23 changes: 19 additions & 4 deletions src/arm.rs
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
// Imports
use std::vec;
use std::f32::consts::PI;
use std::io::Error;
use std::vec;

use glam::{Vec3, Mat3};

Expand All @@ -10,7 +10,7 @@ use stepper_lib::math::{inertia_point, inertia_rod_constr, forces_segment, inert

use stepper_lib::{JsonConfig, MachineConfig};

use crate::{Robot, RobotVars, Phis, Vectors};
use crate::{Robot, RobotVars, Phis, Vectors, SafeRobot};
use crate::types::CylVectors;

// Constants
Expand Down Expand Up @@ -42,7 +42,7 @@ fn law_of_cosines(a : f32, b : f32, c : f32) -> f32 {

impl Robot<4> for SyArm
{
type Error = crate::SyArmError;
type Error = Error;

// Conf
fn from_conf(conf : JsonConfig) -> Result<Self, std::io::Error> {
Expand Down Expand Up @@ -371,4 +371,19 @@ impl SyArm
(a_1 / 2.0 + a_2 / 2.0, a_2 / 2.0)
)
}
}

impl SafeRobot<4> for SyArm {
fn valid_gammas(&self, gammas : &Gammas<4>) -> Result<(), ([bool; 4], Self::Error)> {
let valids = self.comps.valid_dist_verb(gammas);

for valid in valids {
if !valid {
return Err((valids, Error::new(std::io::ErrorKind::InvalidInput,
format!("The gammas given are not valid {:?}", valids))))
}
}

Ok(())
}
}
216 changes: 65 additions & 151 deletions src/intpr.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,26 +3,34 @@ use std::{thread, time::Duration};
use serde_json::Value;
use stepper_lib::gcode::*;

use crate::{SyArm, SyArmResult, Robot};
use crate::SafeRobot;

mod funcs
{
use super::*;

// General Functions
/// G0 X{Position} Y{Position} Z{Position} D{Angle} \
/// Rapid positioning
pub fn g0(arm : &mut SyArm, _code : &GCode, _ : &Args) -> SyArmResult<Value> {
let angles = [0.0; 4]; // arm.get_with_fixed_dec_s(
// get_arg_letter(args, 'X'),
// get_arg_letter(args, 'Y'),
// get_arg_letter(args, 'Z'),
// get_arg_letter(args, 'D')
// )?;
arm.drive_abs(arm.gammas_from_phis(angles));
arm.update(None);
Ok(serde_json::json!(angles))
pub fn g0<R : SafeRobot<N>, const N : usize>(robot : &mut R, _ : &GCode, args : &Args) -> Result<serde_json::Value, R::Error> {
let angles = match robot.safe_phis_for_vec(robot.safe_pos(
get_arg_letter(args, 'X'),
get_arg_letter(args, 'Y'),
get_arg_letter(args, 'Z')
),
robot.safe_deco(get_arg_letter(args, 'D'))
) {
Ok(ang) => ang,
Err((_, err)) => return Err(err)
};

robot.drive_abs(robot.gammas_from_phis(angles));
robot.update(None);
Ok(serde_json::json!(Vec::from(angles)))
}

/// G4 X{Seconds} P{Milliseconds}
/// Dwell (sleeping)
pub fn g4(_ : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
pub fn g4<R : SafeRobot<N>, const N : usize>(_ : &mut R, _ : &GCode, args : &Args) -> Result<serde_json::Value, R::Error> {
let seconds =
get_arg_letter(args, 'X').unwrap_or(0.0) // Seconds
+ get_arg_letter(args, 'P').unwrap_or(0.0)/1000.0; // Milliseconds
Expand All @@ -32,82 +40,28 @@ use crate::{SyArm, SyArmResult, Robot};

/// G8 X{Position} Y{Position} Z{Position} D{Angle} \
/// Rapid positioning async
pub fn g8(arm : &mut SyArm, _code : &GCode, _ : &Args) -> SyArmResult<Value> {
let angles = [0.0; 4]; // arm.get_with_fixed_dec_s(
// get_arg_letter(args, 'X'),
// get_arg_letter(args, 'Y'),
// get_arg_letter(args, 'Z'),
// get_arg_letter(args, 'D')
// )?;
arm.drive_abs_async(arm.gammas_from_phis(angles));
arm.await_inactive();
arm.update(None);
Ok(serde_json::json!(angles))
pub fn g8<R : SafeRobot<N>, const N : usize>(robot : &mut R, _ : &GCode, args : &Args) -> Result<serde_json::Value, R::Error> {
let angles = match robot.safe_phis_for_vec(robot.safe_pos(
get_arg_letter(args, 'X'),
get_arg_letter(args, 'Y'),
get_arg_letter(args, 'Z')
),
robot.safe_deco(get_arg_letter(args, 'D'))
) {
Ok(ang) => ang,
Err((_, err)) => return Err(err)
};
robot.drive_abs_async(robot.gammas_from_phis(angles));
robot.await_inactive();
robot.update(None);
Ok(serde_json::json!(Vec::from(angles)))
}

// Move single axis
// /// G10 C<Angle>
// /// Moves the base with a relative angle
// pub fn g10(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_base_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G11 A<Angle>
// /// Moves the first arm segment with a relative angle
// pub fn g11(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_a1_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G12 A<Angle>
// /// Moves the second arm segment with a relative angle
// pub fn g12(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_a2_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G13 A<Angle>
// /// Moves the third arm segment with a relative angle
// pub fn g13(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_a3_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G20 C<Angle>
// /// Moves the base with an absolute angle
// pub fn g20(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_base_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G21 A<Angle>
// /// Moves the first arm segment with an absolute angle
// pub fn g21(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_a1_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G22 A<Angle>
// /// Moves the second arm segment with an absolute angle
// pub fn g22(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_a2_rel(args[0].value);
// Ok(Value::Null)
// }

// /// G23 A<Angle>
// /// Moves the third arm segment with an absolute angle
// pub fn g23(arm : &mut SyArm, _code : &GCode, args : &Args) -> SyArmResult<Value> {
// arm.drive_a3_rel(args[0].value);
// Ok(Value::Null)
// }
//

/// G28 \
/// G28 \
/// Return to home position
pub fn g28(arm : &mut SyArm, _ : &GCode, _ : &Args) -> SyArmResult<Value> {
pub fn g28<R : SafeRobot<N>, const N : usize>(robot : &mut R, _ : &GCode, _ : &Args) -> Result<serde_json::Value, R::Error> {
// arm.measure(2);
match arm.measure(2) {
match robot.measure(2) {
Ok(_) => { },
Err(meas) => println!(" -> Problems with measurement! {:?}", meas)
};
Expand All @@ -116,81 +70,41 @@ use crate::{SyArm, SyArmResult, Robot};

/// G29 \
/// Return to home position async
pub fn g29(arm : &mut SyArm, _ : &GCode, _ : &Args) -> SyArmResult<Value> {
pub fn g29<R : SafeRobot<N>, const N : usize>(robot : &mut R, _ : &GCode, _ : &Args) -> Result<serde_json::Value, R::Error> {
// arm.measure(2);
arm.measure_async(2);
arm.await_inactive();
arm.update(None);
let home = *arm.home_pos();
arm.set_endpoint(&home);
Ok(Value::Null)
}
//

// Misc Functions
pub fn m0(_ : &mut SyArm, _ : &GCode, _ : &Args) -> SyArmResult<Value> {
// arm.debug_pins();
Ok(Value::Null)
}

pub fn m1(arm : &mut SyArm, _ : &GCode, _ : &Args) -> SyArmResult<Value> {
println!("{}", arm.points_from_phis(&arm.all_phis())[3]);
robot.measure_async(2);
robot.await_inactive();
robot.update(None);
let home = *robot.home_pos();
robot.set_endpoint(&home);
Ok(Value::Null)
}
//

// Tool change
//
// Misc Functions
pub fn m0<R : SafeRobot<N>, const N : usize>(_ : &mut R, _ : &GCode, _ : &Args) -> Result<serde_json::Value, R::Error> {
// arm.debug_pins();
Ok(Value::Null)
}


pub fn init_interpreter(syarm : SyArm) -> Interpreter<SyArm, SyArmResult<Value>> {
let funcs = LetterEntries::from([
(Letter::General, NumEntries::from([
(0, g0 as GCodeFunc<SyArm, SyArmResult<Value>>),
(4, g4),
(8, g8),
// (10, g10),
// (11, g11),
// (12, g12),
// (13, g13),
(28, g28),
(29, g29)
])),
(Letter::Miscellaneous, NumEntries::from([
(0, m0 as GCodeFunc<SyArm, SyArmResult<Value>>),
(1, m1)
]))
]);

Interpreter::new(syarm, funcs)
}


// New interpreter
mod funcs
{
use super::*;

/// G0 X{Position} Y{Position} Z{Position} D{Angle} \
/// Rapid positioning
pub fn g0<R : Robot<N>, const N : usize>(arm : &mut R, _code : &GCode, _ : &Args) -> Result<serde_json::Value, R::Error> {
let angles = [0.0 as f32; N]; // arm.get_with_fixed_dec_s(
// get_arg_letter(args, 'X'),
// get_arg_letter(args, 'Y'),
// get_arg_letter(args, 'Z'),
// get_arg_letter(args, 'D')
// )?;
arm.drive_abs(arm.gammas_from_phis(angles));
arm.update(Some(&angles));

Ok(serde_json::json!(Vec::from(angles)))
}
pub fn m1<R : SafeRobot<N>, const N : usize>(robot : &mut R, _ : &GCode, _ : &Args) -> Result<serde_json::Value, R::Error> {
println!("{}", robot.points_from_phis(&robot.all_phis())[3]);
Ok(Value::Null)
}
//
}

pub fn init_intpr<R : Robot<N>, const N : usize>(rob : R) -> Interpreter<R, Result<serde_json::Value, R::Error>> {
pub fn init_intpr<R : SafeRobot<N>, const N : usize>(rob : R) -> Interpreter<R, Result<serde_json::Value, R::Error>> {
let funcs = LetterEntries::from([
(Letter::General, NumEntries::from([
(0, funcs::g0::<R, N> as GCodeFunc<R, Result<serde_json::Value, R::Error>>)
(0, funcs::g0::<R, N> as GCodeFunc<R, Result<serde_json::Value, R::Error>>),
(4, funcs::g4::<R, N> ),
(8, funcs::g8::<R, N> ),
(28, funcs::g28::<R, N> ),
(29, funcs::g29::<R, N> )
])),
(Letter::Miscellaneous, NumEntries::from([
(0, funcs::m0::<R, N> as GCodeFunc<R, Result<serde_json::Value, R::Error>>),
(1, funcs::m1::<R, N> )
]))
]);

Expand Down
3 changes: 1 addition & 2 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,14 @@
pub use arm::*;

pub mod intpr;
pub use intpr::init_interpreter;
pub use intpr::init_intpr;

mod robot;
pub use robot::*;

pub mod server;

pub mod types;
pub use types::{SyArmError, SyArmResult, ErrType};

#[cfg(test)]
mod tests;
Expand Down
12 changes: 0 additions & 12 deletions src/robot.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,18 +128,6 @@ pub trait Robot<const N : usize>
fn phis_from_vec(&self, pos : Vec3, dec_ang : f32) -> Phis<N>;
//

// Correction
#[inline]
fn valid_gammas(&self, gammas : &Gammas<N>) -> bool {
self.comps().valid_dist(gammas)
}

#[inline]
fn valid_phis(&self, phis : &Phis<N>) -> bool {
self.comps().valid_dist(&self.gammas_from_phis(*phis))
}
//

// Load
#[inline]
fn inertias_from_phis(&self, phis : &Phis<N>) -> Inertias<N> {
Expand Down
Loading

0 comments on commit 8787e96

Please sign in to comment.