diff --git a/lib/src/arcs.rs b/lib/src/arcs.rs new file mode 100644 index 0000000..146ac04 --- /dev/null +++ b/lib/src/arcs.rs @@ -0,0 +1,220 @@ +use glam::{DVec3 as Vec3, Vec4Swizzles}; + +use crate::gcode::GCodeTraditionalParams; +use crate::kind_tracker::Kind; +use crate::planner::{OperationSequence, PositionMode, ToolheadState}; + +#[derive(Debug, Default)] +pub struct ArcState { + plane: Plane, +} + +impl ArcState { + pub fn set_plane(&mut self, plane: Plane) { + self.plane = plane; + } + + pub fn generate_arc( + &self, + toolhead_state: &mut ToolheadState, + op_sequence: &mut OperationSequence, + move_kind: Option, + params: &GCodeTraditionalParams, + direction: ArcDirection, + ) -> usize { + let args = match self.get_args(toolhead_state, params) { + None => return 0, + Some(args) => args, + }; + + let (segments, arc) = args.plan_arc( + toolhead_state.position.xyz(), + direction, + args.mm_per_arc_segment, + ); + let e_base = toolhead_state.position.w; + let e_per_move = args.e.map(|e| (e - e_base) / (segments as f64)); + + toolhead_state.set_speed(args.velocity); + + let old_pos_mode = toolhead_state.position_modes; + toolhead_state.position_modes = [PositionMode::Absolute; 4]; + for (i, segment) in arc.enumerate() { + let coord = [ + Some(segment.x), + Some(segment.y), + Some(segment.z), + e_per_move.map(|e| e_base + (i as f64) * e), + ]; + let mut pm = toolhead_state.perform_move(coord); + pm.kind = move_kind; + op_sequence.add_move(pm, toolhead_state); + } + toolhead_state.position_modes = old_pos_mode; + + segments + } + + fn get_args( + &self, + toolhead_state: &mut ToolheadState, + params: &GCodeTraditionalParams, + ) -> Option { + let mm_per_arc_segment = toolhead_state.limits.mm_per_arc_segment?; + + let map_coord = |c: f64, axis: usize| { + ToolheadState::new_element( + c, + toolhead_state.position.as_ref()[axis], + toolhead_state.position_modes[axis], + ) + }; + + let (axes, offset) = match self.plane { + Plane::XY => ( + (0, 1, 2), + ( + params.get_number::('I').unwrap_or(0.0), + params.get_number::('J').unwrap_or(0.0), + ), + ), + Plane::XZ => ( + (0, 2, 1), + ( + params.get_number::('I').unwrap_or(0.0), + params.get_number::('K').unwrap_or(0.0), + ), + ), + Plane::YZ => ( + (1, 2, 0), + ( + params.get_number::('J').unwrap_or(0.0), + params.get_number::('K').unwrap_or(0.0), + ), + ), + }; + + if offset.0 == 0.0 && offset.1 == 0.0 { + return None; // We need at least one coordinate to work with + } + + Some(ArcArgs { + target: Vec3::new( + params + .get_number::('X') + .map_or(toolhead_state.position.x, |c| map_coord(c, 0)), + params + .get_number::('Y') + .map_or(toolhead_state.position.y, |c| map_coord(c, 1)), + params + .get_number::('Z') + .map_or(toolhead_state.position.z, |c| map_coord(c, 2)), + ), + e: params.get_number::('E').map(|c| map_coord(c, 3)), + velocity: params + .get_number::('F') + .map_or(toolhead_state.velocity, |v| v / 60.0), + axes, + offset, + mm_per_arc_segment, + }) + } +} + +#[derive(Debug, Copy, Clone, PartialEq)] +struct ArcArgs { + target: Vec3, + e: Option, + velocity: f64, + axes: (usize, usize, usize), + offset: (f64, f64), + mm_per_arc_segment: f64, +} + +impl ArcArgs { + // Ported from klipper, originally from Marlins plan-arc() function. + fn plan_arc( + &self, + start_position: Vec3, + direction: ArcDirection, + mm_per_arc_segment: f64, + ) -> (usize, impl Iterator + '_) { + let current_position = start_position.as_ref(); + let target_position = self.target.as_ref(); + let (alpha_axis, beta_axis, helical_axis) = self.axes; + + let r_p = -self.offset.0; + let r_q = -self.offset.1; + + let center_p = current_position[alpha_axis] - r_p; + let center_q = current_position[beta_axis] - r_q; + let rt_alpha = target_position[alpha_axis] - center_p; + let rt_beta = target_position[beta_axis] - center_q; + let mut angular_travel = + (r_p * rt_beta - r_q * rt_alpha).atan2(r_p * rt_alpha + r_q * rt_beta); + if angular_travel < 0.0 { + angular_travel += 2.0 * std::f64::consts::PI; + } + if direction == ArcDirection::Clockwise { + angular_travel -= 2.0 * std::f64::consts::PI; + } + + if angular_travel == 0.0 + && current_position[alpha_axis] == target_position[alpha_axis] + && current_position[beta_axis] == target_position[beta_axis] + { + angular_travel = 2.0 * std::f64::consts::PI + } + + let linear_travel = target_position[helical_axis] - current_position[helical_axis]; + let radius = r_p.hypot(r_q); + let flat_mm = radius * angular_travel; + let mm_of_travel = if linear_travel != 0.0 { + flat_mm.hypot(linear_travel) + } else { + flat_mm.abs() + }; + + let segments = ((mm_of_travel / mm_per_arc_segment).floor() as usize).max(1); + + let theta_per_segment = angular_travel / (segments as f64); + let linear_per_segment = linear_travel / (segments as f64); + ( + segments, + (1..segments) + .map(move |i| { + let i = i as f64; + let dist_helical = i * linear_per_segment; + let cos_ti = (i * theta_per_segment).cos(); + let sin_ti = (i * theta_per_segment).sin(); + let r_p = -self.offset.0 * cos_ti + self.offset.1 * sin_ti; + let r_q = -self.offset.0 * sin_ti - self.offset.1 * cos_ti; + let mut coord = [0.0f64; 3]; + coord[alpha_axis] = center_p + r_p; + coord[beta_axis] = center_q + r_q; + coord[helical_axis] = start_position.as_ref()[helical_axis] + dist_helical; + coord.into() + }) + .chain(std::iter::once(self.target)), + ) + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +pub enum ArcDirection { + Clockwise, + CounterClockwise, +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +pub enum Plane { + XY, + XZ, + YZ, +} + +impl Default for Plane { + fn default() -> Self { + Self::XY + } +} diff --git a/lib/src/kind_tracker.rs b/lib/src/kind_tracker.rs index 6786cf0..fa9c352 100644 --- a/lib/src/kind_tracker.rs +++ b/lib/src/kind_tracker.rs @@ -4,6 +4,7 @@ use std::collections::HashMap; pub struct KindTracker { pub i2k: HashMap, pub k2i: HashMap, + pub current_kind: Option, } impl KindTracker { @@ -26,6 +27,25 @@ impl KindTracker { pub fn resolve_kind(&self, k: Kind) -> &str { self.k2i.get(&k.0).expect("missing kind") } + + pub fn kind_from_comment(&mut self, comment: &Option) -> Option { + comment + .as_ref() + .map(|s| s.trim()) + .map(|s| { + if s.starts_with("move to next layer ") { + "move to next layer" + } else { + s + } + }) + .map(|s| self.get_kind(s)) + .or(self.current_kind) + } + + pub fn set_current(&mut self, kind: Option) { + self.current_kind = kind; + } } #[derive(Debug, Copy, Clone, Eq, PartialEq)] diff --git a/lib/src/lib.rs b/lib/src/lib.rs index e9ad168..e30700c 100644 --- a/lib/src/lib.rs +++ b/lib/src/lib.rs @@ -1,6 +1,7 @@ #[macro_use] extern crate lazy_static; +pub mod arcs; pub mod firmware_retraction; pub mod gcode; mod kind_tracker; diff --git a/lib/src/planner.rs b/lib/src/planner.rs index 1ee3d51..2185cc7 100644 --- a/lib/src/planner.rs +++ b/lib/src/planner.rs @@ -2,6 +2,7 @@ use std::collections::VecDeque; use std::f64::EPSILON; use std::time::Duration; +use crate::arcs::ArcState; pub use crate::firmware_retraction::FirmwareRetractionOptions; use crate::firmware_retraction::FirmwareRetractionState; use crate::gcode::{GCodeCommand, GCodeOperation}; @@ -16,8 +17,8 @@ pub struct Planner { operations: OperationSequence, pub toolhead_state: ToolheadState, pub kind_tracker: KindTracker, - pub current_kind: Option, pub firmware_retraction: Option, + pub arc_state: ArcState, } impl Planner { @@ -30,8 +31,8 @@ impl Planner { operations: OperationSequence::default(), toolhead_state: ToolheadState::from_limits(limits), kind_tracker: KindTracker::new(), - current_kind: None, firmware_retraction, + arc_state: ArcState::default(), } } @@ -46,19 +47,7 @@ impl Planner { self.toolhead_state.set_speed(v / 60.0); } - let move_kind = cmd - .comment - .as_ref() - .map(|s| s.trim()) - .map(|s| { - if s.starts_with("move to next layer ") { - "move to next layer" - } else { - s - } - }) - .map(|s| self.kind_tracker.get_kind(s)) - .or(self.current_kind); + let move_kind = self.kind_tracker.kind_from_comment(&cmd.comment); if x.is_some() || y.is_some() || z.is_some() || e.is_some() { let mut m = self.toolhead_state.perform_move([*x, *y, *z, *e]); @@ -90,6 +79,31 @@ impl Planner { return fr.unretract(kt, m, seq); } } + ('G', v @ 2 | v @ 3) => { + let move_kind = self.kind_tracker.kind_from_comment(&cmd.comment); + let m = &mut self.toolhead_state; + let seq = &mut self.operations; + return self.arc_state.generate_arc( + m, + seq, + move_kind, + params, + match v { + 2 => crate::arcs::ArcDirection::Clockwise, + 3 => crate::arcs::ArcDirection::CounterClockwise, + _ => unreachable!("v can only be 2 or 3"), + }, + ); + } + ('G', 17) => { + self.arc_state.set_plane(crate::arcs::Plane::XY); + } + ('G', 18) => { + self.arc_state.set_plane(crate::arcs::Plane::XZ); + } + ('G', 19) => { + self.arc_state.set_plane(crate::arcs::Plane::YZ); + } ('G', 92) => { if let Some(v) = params.get_number::('X') { self.toolhead_state.position.x = v; @@ -151,7 +165,8 @@ impl Planner { if let Some(comment) = comment.strip_prefix("TYPE:") { // IdeaMaker only gives us `TYPE:`s - self.current_kind = Some(self.kind_tracker.get_kind(comment)); + let kind = self.kind_tracker.get_kind(comment); + self.kind_tracker.set_current(Some(kind)); self.operations.add_fill(); } else if let Some(cmd) = comment.trim_start().strip_prefix("ESTIMATOR_ADD_TIME ") { if let Some((duration, kind)) = Self::parse_buffer_cmd(&mut self.kind_tracker, cmd) @@ -321,7 +336,7 @@ pub struct PlanningMove { impl PlanningMove { /// Create a new `PlanningMove` that travels between the two points `start` /// and `end`. - fn new(start: Vec4, end: Vec4, toolhead_state: &ToolheadState) -> PlanningMove { + pub(crate) fn new(start: Vec4, end: Vec4, toolhead_state: &ToolheadState) -> PlanningMove { if start.xyz() == end.xyz() { Self::new_extrude_move(start, end, toolhead_state) } else { @@ -724,9 +739,11 @@ pub struct PrinterLimits { #[serde(skip)] pub junction_deviation: f64, pub instant_corner_velocity: f64, - pub move_checkers: Vec, #[serde(default, skip_serializing_if = "Option::is_none")] pub firmware_retraction: Option, + #[serde(default, skip_serializing_if = "Option::is_none")] + pub mm_per_arc_segment: Option, + pub move_checkers: Vec, } impl Default for PrinterLimits { @@ -740,6 +757,7 @@ impl Default for PrinterLimits { instant_corner_velocity: 1.0, move_checkers: vec![], firmware_retraction: None, + mm_per_arc_segment: None, } } } @@ -847,7 +865,7 @@ impl ToolheadState { pm } - fn new_element(v: f64, old: f64, mode: PositionMode) -> f64 { + pub(crate) fn new_element(v: f64, old: f64, mode: PositionMode) -> f64 { match mode { PositionMode::Relative => old + v, PositionMode::Absolute => v, diff --git a/tool/src/main.rs b/tool/src/main.rs index d257790..9a4b34d 100644 --- a/tool/src/main.rs +++ b/tool/src/main.rs @@ -218,6 +218,7 @@ fn moonraker_config( printer: PrinterConfig, extruder: ExtruderConfig, firmware_retraction: Option, + gcode_arcs: Option, } #[derive(Debug, Deserialize)] @@ -252,6 +253,11 @@ fn moonraker_config( lift_z: f64, } + #[derive(Debug, Deserialize)] + struct GcodeArcsConfig { + resolution: Option, + } + let client = reqwest::blocking::Client::new(); let mut req = client.get(url); @@ -273,6 +279,8 @@ fn moonraker_config( target.set_square_corner_velocity(cfg.printer.square_corner_velocity); target.set_instant_corner_velocity(cfg.extruder.instantaneous_corner_velocity); + target.mm_per_arc_segment = cfg.gcode_arcs.and_then(|cfg| cfg.resolution); + target.firmware_retraction = cfg.firmware_retraction.map(|fr| FirmwareRetractionOptions { retract_length: fr.retract_length, unretract_extra_length: fr.unretract_extra_length,