Skip to content

Commit 3fd629d

Browse files
bdringbdring
authored
Merge pull request #1586 from Popeyef5/fix/invalid_arc-pword
Fix invalid_arc routine to check for full rotations in G2/G3 arc commands
2 parents 86074f6 + b24150d commit 3fd629d

7 files changed

Lines changed: 107 additions & 95 deletions

File tree

‎FluidNC/src/Kinematics/Cartesian.cpp‎

Lines changed: 97 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ namespace Kinematics {
2424
// circle plane axis, caxes[1] is the second circle plane axis, and caxes[2] is the
2525
// orthogonal plane. So for G17 mode, caxes[] is { 0, 1, 2} for { X, Y, Z}. G18 is {2, 0, 1} i.e. {Z, X, Y}, and G19 is {1, 2, 0} i.e. {Y, Z, X}
2626
bool Cartesian::invalid_arc(
27-
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) {
27+
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc, int pword_rotations) {
2828
pl_data->limits_checked = true;
2929

3030
auto axes = config->_axes;
@@ -50,108 +50,118 @@ namespace Kinematics {
5050
return false;
5151
}
5252

53-
// The origin for this calculation's coordinate system is at the center of the arc.
54-
// The 0 and 1 entries are for the circle plane
55-
// and the 2 entry is the orthogonal (linear) direction
56-
57-
float s[2], e[2]; // Start and end of arc in the circle plane, relative to center
58-
59-
// Depending on the arc direction, set the arc start and end points relative
60-
// to the arc center. Afterwards, end is always counterclockwise relative to
61-
// start, thus simplifying the following decision tree.
62-
if (is_clockwise_arc) {
63-
s[0] = target[caxes[0]] - center[0];
64-
s[1] = target[caxes[1]] - center[1];
65-
e[0] = position[caxes[0]] - center[0];
66-
e[1] = position[caxes[1]] - center[1];
67-
} else {
68-
s[0] = position[caxes[0]] - center[0];
69-
s[1] = position[caxes[1]] - center[1];
70-
e[0] = target[caxes[0]] - center[0];
71-
e[1] = target[caxes[1]] - center[1];
72-
}
73-
7453
// Axis crossings - plus and minus caxes[0] and caxes[1]
7554
bool p[2] = { false, false };
7655
bool m[2] = { false, false };
7756

78-
// The following decision tree determines whether the arc crosses
79-
// the horizontal and vertical axes of the circular plane in the
80-
// positive and negative half planes. There are ways to express
81-
// it in fewer lines of code by converting to alternate
82-
// representations like angles, but this way is computationally
83-
// efficient since it avoids any use of transcendental functions.
84-
// Every path through this decision tree is either 4 or 5 simple
85-
// comparisons.
86-
if (e[1] >= 0) { // End in upper half plane
87-
if (e[0] > 0) { // End in quadrant 0 - X+ Y+
88-
if (s[1] >= 0) { // Start in upper half plane
89-
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
90-
if (s[0] <= e[0]) { // wraparound
91-
p[0] = p[1] = m[0] = m[1] = true;
57+
if (pword_rotations > 1) {
58+
// The arc does at least a full circle around the center before arriving at
59+
// the target position, so we need to check all axis crossings.
60+
p[0] = p[1] = m[0] = m[1] = true;
61+
} else {
62+
// The arc does less than a full circle before arriving at the target position,
63+
// so the axis crossings are determined by the arc direction and the start and end points.
64+
65+
// The origin for this calculation's coordinate system is at the center of the arc.
66+
// The 0 and 1 entries are for the circle plane
67+
// and the 2 entry is the orthogonal (linear) direction
68+
69+
float s[2], e[2]; // Start and end of arc in the circle plane, relative to center
70+
71+
// Depending on the arc direction, set the arc start and end points relative
72+
// to the arc center. Afterwards, end is always counterclockwise relative to
73+
// start, thus simplifying the following decision tree.
74+
if (is_clockwise_arc) {
75+
s[0] = target[caxes[0]] - center[0];
76+
s[1] = target[caxes[1]] - center[1];
77+
e[0] = position[caxes[0]] - center[0];
78+
e[1] = position[caxes[1]] - center[1];
79+
} else {
80+
s[0] = position[caxes[0]] - center[0];
81+
s[1] = position[caxes[1]] - center[1];
82+
e[0] = target[caxes[0]] - center[0];
83+
e[1] = target[caxes[1]] - center[1];
84+
}
85+
86+
// The following decision tree determines whether the arc crosses
87+
// the horizontal and vertical axes of the circular plane in the
88+
// positive and negative half planes. There are ways to express
89+
// it in fewer lines of code by converting to alternate
90+
// representations like angles, but this way is computationally
91+
// efficient since it avoids any use of transcendental functions.
92+
// Every path through this decision tree is either 4 or 5 simple
93+
// comparisons.
94+
if (e[1] >= 0) { // End in upper half plane
95+
if (e[0] > 0) { // End in quadrant 0 - X+ Y+
96+
if (s[1] >= 0) { // Start in upper half plane
97+
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
98+
if (s[0] <= e[0]) { // wraparound
99+
p[0] = p[1] = m[0] = m[1] = true;
100+
}
101+
} else { // Start in quadrant 1 - X- Y+
102+
m[0] = m[1] = p[0] = true;
92103
}
93-
} else { // Start in quadrant 1 - X- Y+
94-
m[0] = m[1] = p[0] = true;
95-
}
96-
} else { // Start in lower half plane
97-
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
98-
p[0] = true;
99-
} else { // Start in quadrant 2 - X- Y-
100-
m[1] = p[0] = true;
101-
}
102-
}
103-
} else { // End in quadrant 1 - X- Y+
104-
if (s[1] >= 0) { // Start in upper half plane
105-
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
106-
p[1] = true;
107-
} else { // Start in quadrant 1 - X- Y+
108-
if (s[0] <= e[0]) { // wraparound
109-
p[0] = p[1] = m[0] = m[1] = true;
104+
} else { // Start in lower half plane
105+
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
106+
p[0] = true;
107+
} else { // Start in quadrant 2 - X- Y-
108+
m[1] = p[0] = true;
110109
}
111110
}
112-
} else { // Start in lower half plane
113-
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
114-
p[0] = p[1] = true;
115-
} else { // Start in quadrant 2 - X- Y-
116-
m[1] = p[0] = p[1] = true;
117-
}
118-
}
119-
}
120-
} else { // e[1] < 0 - end in lower half plane
121-
if (e[0] > 0) { // End in quadrant 3 - X+ Y+
122-
if (s[1] >= 0) { // Start in upper half plane
123-
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
124-
p[1] = m[0] = m[1] = true;
125-
} else { // Start in quadrant 1 - X- Y+
126-
m[0] = m[1] = true;
127-
}
128-
} else { // Start in lower half plane
129-
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
130-
if (s[0] >= e[0]) { // wraparound
131-
p[0] = p[1] = m[0] = m[1] = true;
111+
} else { // End in quadrant 1 - X- Y+
112+
if (s[1] >= 0) { // Start in upper half plane
113+
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
114+
p[1] = true;
115+
} else { // Start in quadrant 1 - X- Y+
116+
if (s[0] <= e[0]) { // wraparound
117+
p[0] = p[1] = m[0] = m[1] = true;
118+
}
119+
}
120+
} else { // Start in lower half plane
121+
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
122+
p[0] = p[1] = true;
123+
} else { // Start in quadrant 2 - X- Y-
124+
m[1] = p[0] = p[1] = true;
132125
}
133-
} else { // Start in quadrant 2 - X- Y-
134-
m[1] = true;
135126
}
136127
}
137-
} else { // End in quadrant 2 - X- Y+
138-
if (s[1] >= 0) { // Start in upper half plane
139-
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
140-
p[1] = m[0] = true;
141-
} else { // Start in quadrant 1 - X- Y+
142-
m[0] = true;
128+
} else { // e[1] < 0 - end in lower half plane
129+
if (e[0] > 0) { // End in quadrant 3 - X+ Y+
130+
if (s[1] >= 0) { // Start in upper half plane
131+
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
132+
p[1] = m[0] = m[1] = true;
133+
} else { // Start in quadrant 1 - X- Y+
134+
m[0] = m[1] = true;
135+
}
136+
} else { // Start in lower half plane
137+
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
138+
if (s[0] >= e[0]) { // wraparound
139+
p[0] = p[1] = m[0] = m[1] = true;
140+
}
141+
} else { // Start in quadrant 2 - X- Y-
142+
m[1] = true;
143+
}
143144
}
144-
} else { // Start in lower half plane
145-
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
146-
p[0] = p[1] = m[0] = true;
147-
} else { // Start in quadrant 2 - X- Y-
148-
if (s[0] >= e[0]) { // wraparound
149-
p[0] = p[1] = m[0] = m[1] = true;
145+
} else { // End in quadrant 2 - X- Y+
146+
if (s[1] >= 0) { // Start in upper half plane
147+
if (s[0] > 0) { // Start in quadrant 0 - X+ Y+
148+
p[1] = m[0] = true;
149+
} else { // Start in quadrant 1 - X- Y+
150+
m[0] = true;
151+
}
152+
} else { // Start in lower half plane
153+
if (s[0] > 0) { // Start in quadrant 3 - X+ Y-
154+
p[0] = p[1] = m[0] = true;
155+
} else { // Start in quadrant 2 - X- Y-
156+
if (s[0] >= e[0]) { // wraparound
157+
p[0] = p[1] = m[0] = m[1] = true;
158+
}
150159
}
151160
}
152161
}
153162
}
154163
}
164+
155165
// Now check limits based on arc endpoints and axis crossings
156166
for (size_t a = 0; a < 2; ++a) {
157167
the_axis = caxes[a];

‎FluidNC/src/Kinematics/Cartesian.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,8 @@ namespace Kinematics {
3131
float center[3],
3232
float radius,
3333
size_t caxes[3],
34-
bool is_clockwise_arc) override;
34+
bool is_clockwise_arc,
35+
int pword_rotations) override;
3536

3637
virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override;
3738
virtual void init() override;

‎FluidNC/src/Kinematics/Kinematics.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,9 @@ namespace Kinematics {
1919
}
2020

2121
bool Kinematics::invalid_arc(
22-
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) {
22+
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc, int pword_rotations) {
2323
Assert(_system != nullptr, "No kinematic system");
24-
return _system->invalid_arc(target, pl_data, position, center, radius, caxes, is_clockwise_arc);
24+
return _system->invalid_arc(target, pl_data, position, center, radius, caxes, is_clockwise_arc, pword_rotations);
2525
}
2626

2727
bool Kinematics::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) {

‎FluidNC/src/Kinematics/Kinematics.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ namespace Kinematics {
4343
void constrain_jog(float* target, plan_line_data_t* pl_data, float* position);
4444
bool invalid_line(float* target);
4545
bool invalid_arc(
46-
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc);
46+
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc, int pword_rotations);
4747

4848
bool canHome(AxisMask axisMask);
4949
bool kinematics_homing(AxisMask axisMask);
@@ -73,7 +73,7 @@ namespace Kinematics {
7373
virtual void constrain_jog(float* cartesian, plan_line_data_t* pl_data, float* position) {}
7474
virtual bool invalid_line(float* cartesian) { return false; }
7575
virtual bool invalid_arc(
76-
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) {
76+
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc, int pword_rotations) {
7777
return false;
7878
}
7979

‎FluidNC/src/Kinematics/ParallelDelta.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -123,7 +123,7 @@ namespace Kinematics {
123123

124124
// TO DO. This is not supported yet. Other levels of protection will prevent "damage"
125125
bool ParallelDelta::invalid_arc(
126-
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc) {
126+
float* target, plan_line_data_t* pl_data, float* position, float center[3], float radius, size_t caxes[3], bool is_clockwise_arc, int pword_rotations) {
127127
return false;
128128
}
129129

‎FluidNC/src/Kinematics/ParallelDelta.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,8 @@ namespace Kinematics {
4747
float center[3],
4848
float radius,
4949
size_t caxes[3],
50-
bool is_clockwise_arc) override;
50+
bool is_clockwise_arc,
51+
int pword_rotations) override;
5152

5253
void releaseMotors(AxisMask axisMask, MotorMask motors) override;
5354

‎FluidNC/src/MotionControl.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -137,7 +137,7 @@ void mc_arc(float* target,
137137

138138
// The first two axes are the circle plane and the third is the orthogonal plane
139139
size_t caxes[3] = { axis_0, axis_1, axis_linear };
140-
if (config->_kinematics->invalid_arc(target, pl_data, position, center, radius, caxes, is_clockwise_arc)) {
140+
if (config->_kinematics->invalid_arc(target, pl_data, position, center, radius, caxes, is_clockwise_arc, pword_rotations)) {
141141
return;
142142
}
143143

0 commit comments

Comments
 (0)