|
| 1 | +use glam::{DVec3 as Vec3, Vec4Swizzles}; |
| 2 | + |
| 3 | +use crate::gcode::GCodeTraditionalParams; |
| 4 | +use crate::kind_tracker::Kind; |
| 5 | +use crate::planner::{OperationSequence, PositionMode, ToolheadState}; |
| 6 | + |
| 7 | +#[derive(Debug, Default)] |
| 8 | +pub struct ArcState { |
| 9 | + plane: Plane, |
| 10 | +} |
| 11 | + |
| 12 | +impl ArcState { |
| 13 | + pub fn set_plane(&mut self, plane: Plane) { |
| 14 | + self.plane = plane; |
| 15 | + } |
| 16 | + |
| 17 | + pub fn generate_arc( |
| 18 | + &self, |
| 19 | + toolhead_state: &mut ToolheadState, |
| 20 | + op_sequence: &mut OperationSequence, |
| 21 | + move_kind: Option<Kind>, |
| 22 | + params: &GCodeTraditionalParams, |
| 23 | + direction: ArcDirection, |
| 24 | + ) -> usize { |
| 25 | + let args = match self.get_args(toolhead_state, params) { |
| 26 | + None => return 0, |
| 27 | + Some(args) => args, |
| 28 | + }; |
| 29 | + |
| 30 | + let (segments, arc) = args.plan_arc( |
| 31 | + toolhead_state.position.xyz(), |
| 32 | + direction, |
| 33 | + args.mm_per_arc_segment, |
| 34 | + ); |
| 35 | + let e_base = toolhead_state.position.w; |
| 36 | + let e_per_move = args.e.map(|e| (e - e_base) / (segments as f64)); |
| 37 | + |
| 38 | + toolhead_state.set_speed(args.velocity); |
| 39 | + |
| 40 | + let old_pos_mode = toolhead_state.position_modes; |
| 41 | + toolhead_state.position_modes = [PositionMode::Absolute; 4]; |
| 42 | + for (i, segment) in arc.enumerate() { |
| 43 | + let coord = [ |
| 44 | + Some(segment.x), |
| 45 | + Some(segment.y), |
| 46 | + Some(segment.z), |
| 47 | + e_per_move.map(|e| e_base + (i as f64) * e), |
| 48 | + ]; |
| 49 | + let mut pm = toolhead_state.perform_move(coord); |
| 50 | + pm.kind = move_kind; |
| 51 | + op_sequence.add_move(pm, toolhead_state); |
| 52 | + } |
| 53 | + toolhead_state.position_modes = old_pos_mode; |
| 54 | + |
| 55 | + segments |
| 56 | + } |
| 57 | + |
| 58 | + fn get_args( |
| 59 | + &self, |
| 60 | + toolhead_state: &mut ToolheadState, |
| 61 | + params: &GCodeTraditionalParams, |
| 62 | + ) -> Option<ArcArgs> { |
| 63 | + let mm_per_arc_segment = toolhead_state.limits.mm_per_arc_segment?; |
| 64 | + |
| 65 | + let map_coord = |c: f64, axis: usize| { |
| 66 | + ToolheadState::new_element( |
| 67 | + c, |
| 68 | + toolhead_state.position.as_ref()[axis], |
| 69 | + toolhead_state.position_modes[axis], |
| 70 | + ) |
| 71 | + }; |
| 72 | + |
| 73 | + let (axes, offset) = match self.plane { |
| 74 | + Plane::XY => ( |
| 75 | + (0, 1, 2), |
| 76 | + ( |
| 77 | + params.get_number::<f64>('I').unwrap_or(0.0), |
| 78 | + params.get_number::<f64>('J').unwrap_or(0.0), |
| 79 | + ), |
| 80 | + ), |
| 81 | + Plane::XZ => ( |
| 82 | + (0, 2, 1), |
| 83 | + ( |
| 84 | + params.get_number::<f64>('I').unwrap_or(0.0), |
| 85 | + params.get_number::<f64>('K').unwrap_or(0.0), |
| 86 | + ), |
| 87 | + ), |
| 88 | + Plane::YZ => ( |
| 89 | + (1, 2, 0), |
| 90 | + ( |
| 91 | + params.get_number::<f64>('J').unwrap_or(0.0), |
| 92 | + params.get_number::<f64>('K').unwrap_or(0.0), |
| 93 | + ), |
| 94 | + ), |
| 95 | + }; |
| 96 | + |
| 97 | + if offset.0 == 0.0 && offset.1 == 0.0 { |
| 98 | + return None; // We need at least one coordinate to work with |
| 99 | + } |
| 100 | + |
| 101 | + Some(ArcArgs { |
| 102 | + target: Vec3::new( |
| 103 | + params |
| 104 | + .get_number::<f64>('X') |
| 105 | + .map_or(toolhead_state.position.x, |c| map_coord(c, 0)), |
| 106 | + params |
| 107 | + .get_number::<f64>('Y') |
| 108 | + .map_or(toolhead_state.position.y, |c| map_coord(c, 1)), |
| 109 | + params |
| 110 | + .get_number::<f64>('Z') |
| 111 | + .map_or(toolhead_state.position.z, |c| map_coord(c, 2)), |
| 112 | + ), |
| 113 | + e: params.get_number::<f64>('E').map(|c| map_coord(c, 3)), |
| 114 | + velocity: params |
| 115 | + .get_number::<f64>('F') |
| 116 | + .map_or(toolhead_state.velocity, |v| v / 60.0), |
| 117 | + axes, |
| 118 | + offset, |
| 119 | + mm_per_arc_segment, |
| 120 | + }) |
| 121 | + } |
| 122 | +} |
| 123 | + |
| 124 | +#[derive(Debug, Copy, Clone, PartialEq)] |
| 125 | +struct ArcArgs { |
| 126 | + target: Vec3, |
| 127 | + e: Option<f64>, |
| 128 | + velocity: f64, |
| 129 | + axes: (usize, usize, usize), |
| 130 | + offset: (f64, f64), |
| 131 | + mm_per_arc_segment: f64, |
| 132 | +} |
| 133 | + |
| 134 | +impl ArcArgs { |
| 135 | + // Ported from klipper, originally from Marlins plan-arc() function. |
| 136 | + fn plan_arc( |
| 137 | + &self, |
| 138 | + start_position: Vec3, |
| 139 | + direction: ArcDirection, |
| 140 | + mm_per_arc_segment: f64, |
| 141 | + ) -> (usize, impl Iterator<Item = Vec3> + '_) { |
| 142 | + let current_position = start_position.as_ref(); |
| 143 | + let target_position = self.target.as_ref(); |
| 144 | + let (alpha_axis, beta_axis, helical_axis) = self.axes; |
| 145 | + |
| 146 | + let r_p = -self.offset.0; |
| 147 | + let r_q = -self.offset.1; |
| 148 | + |
| 149 | + let center_p = current_position[alpha_axis] - r_p; |
| 150 | + let center_q = current_position[beta_axis] - r_q; |
| 151 | + let rt_alpha = target_position[alpha_axis] - center_p; |
| 152 | + let rt_beta = target_position[beta_axis] - center_q; |
| 153 | + let mut angular_travel = |
| 154 | + (r_p * rt_beta - r_q * rt_alpha).atan2(r_p * rt_alpha + r_q * rt_beta); |
| 155 | + if angular_travel < 0.0 { |
| 156 | + angular_travel += 2.0 * std::f64::consts::PI; |
| 157 | + } |
| 158 | + if direction == ArcDirection::Clockwise { |
| 159 | + angular_travel -= 2.0 * std::f64::consts::PI; |
| 160 | + } |
| 161 | + |
| 162 | + if angular_travel == 0.0 |
| 163 | + && current_position[alpha_axis] == target_position[alpha_axis] |
| 164 | + && current_position[beta_axis] == target_position[beta_axis] |
| 165 | + { |
| 166 | + angular_travel = 2.0 * std::f64::consts::PI |
| 167 | + } |
| 168 | + |
| 169 | + let linear_travel = target_position[helical_axis] - current_position[helical_axis]; |
| 170 | + let radius = r_p.hypot(r_q); |
| 171 | + let flat_mm = radius * angular_travel; |
| 172 | + let mm_of_travel = if linear_travel != 0.0 { |
| 173 | + flat_mm.hypot(linear_travel) |
| 174 | + } else { |
| 175 | + flat_mm.abs() |
| 176 | + }; |
| 177 | + |
| 178 | + let segments = ((mm_of_travel / mm_per_arc_segment).floor() as usize).max(1); |
| 179 | + |
| 180 | + let theta_per_segment = angular_travel / (segments as f64); |
| 181 | + let linear_per_segment = linear_travel / (segments as f64); |
| 182 | + ( |
| 183 | + segments, |
| 184 | + (1..segments) |
| 185 | + .map(move |i| { |
| 186 | + let i = i as f64; |
| 187 | + let dist_helical = i * linear_per_segment; |
| 188 | + let cos_ti = (i * theta_per_segment).cos(); |
| 189 | + let sin_ti = (i * theta_per_segment).sin(); |
| 190 | + let r_p = -self.offset.0 * cos_ti + self.offset.1 * sin_ti; |
| 191 | + let r_q = -self.offset.0 * sin_ti - self.offset.1 * cos_ti; |
| 192 | + let mut coord = [0.0f64; 3]; |
| 193 | + coord[alpha_axis] = center_p + r_p; |
| 194 | + coord[beta_axis] = center_q + r_q; |
| 195 | + coord[helical_axis] = start_position.as_ref()[helical_axis] + dist_helical; |
| 196 | + coord.into() |
| 197 | + }) |
| 198 | + .chain(std::iter::once(self.target)), |
| 199 | + ) |
| 200 | + } |
| 201 | +} |
| 202 | + |
| 203 | +#[derive(Debug, Copy, Clone, Eq, PartialEq)] |
| 204 | +pub enum ArcDirection { |
| 205 | + Clockwise, |
| 206 | + CounterClockwise, |
| 207 | +} |
| 208 | + |
| 209 | +#[derive(Debug, Copy, Clone, Eq, PartialEq)] |
| 210 | +pub enum Plane { |
| 211 | + XY, |
| 212 | + XZ, |
| 213 | + YZ, |
| 214 | +} |
| 215 | + |
| 216 | +impl Default for Plane { |
| 217 | + fn default() -> Self { |
| 218 | + Self::XY |
| 219 | + } |
| 220 | +} |
0 commit comments