1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
|
#include "smoothieware.h"
#include "utilities.h"
smoothieware::smoothieware(firmware_arguments args) : firmware(args) {
feed_rate = 0;
for (int i = 0; i < REPETIER_XYZE; i++)
{
machine_position[i] = 0;
}
THEKERNEL = new SmoothiewareKernel();
apply_arguments();
};
void smoothieware::apply_arguments()
{
static const std::vector<std::string> smoothieware_firmware_version_names{ "2021-06-19" };
set_versions(smoothieware_firmware_version_names, "2021-06-19");
smoothieware_version_ = (smoothieware::smoothieware_firmware_versions)version_index_;
std::vector<std::string> used_arguments;
// Add switch back in if we ever add more versions
//switch (smoothieware_version_)
//{
//default:
append_arc_ = &smoothieware::append_arc_2021_06_19;
used_arguments = { "mm_per_arc_segment", "mm_max_arc_error", "n_arc_correction", "g90_g91_influences_extruder" };
//break;
//}
args_.set_used_arguments(used_arguments);
}
smoothieware::~smoothieware()
{
delete THEKERNEL;
}
firmware_arguments smoothieware::get_default_arguments_for_current_version() const
{
// Start off with the current args so they are set up correctly for this firmware type and version
firmware_arguments default_args = args_;
// firmware defaults
default_args.g90_g91_influences_extruder = true;
// Add the switch back if we ever need to add more versions
//switch (smoothieware_version_)
//{
//default:
// Active Settings
default_args.mm_per_arc_segment = 0.0f;
default_args.mm_max_arc_error = 0.01;
default_args.n_arc_correction = 5;
//break;
//}
return default_args;
}
std::string smoothieware::interpolate_arc(firmware_position& target, double i, double j, double r, bool is_clockwise)
{
// Clear the current list of gcodes
gcodes_.clear();
// Setup the current position
machine_position[X_AXIS] = static_cast<float>(position_.x);
machine_position[Y_AXIS] = static_cast<float>(position_.y);
machine_position[Z_AXIS] = static_cast<float>(position_.z);
machine_position[E_AXIS] = static_cast<float>(position_.e);
float smoothieware_target[k_max_actuators];
smoothieware_target[X_AXIS] = static_cast<float>(target.x);
smoothieware_target[Y_AXIS] = static_cast<float>(target.y);
smoothieware_target[Z_AXIS] = static_cast<float>(target.z);
smoothieware_target[E_AXIS] = static_cast<float>(target.e);
float smoothieware_offset[2];
smoothieware_offset[0] = static_cast<float>(i);
smoothieware_offset[1] = static_cast<float>(j);
float radius = static_cast<float>(r);
// Set the feedrate
feed_rate = static_cast<float>(target.f);
uint8_t smoothieware_isclockwise = is_clockwise ? 1 : 0;
(this->*append_arc_)(&gcode_, smoothieware_target, smoothieware_offset, radius, smoothieware_isclockwise);
return gcodes_;
}
// Append an arc to the queue ( cutting it into segments as needed )
bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float target[], const float offset[], float radius, bool is_clockwise)
{
float rate_mm_s = this->feed_rate / seconds_per_minute;
// catch negative or zero feed rates and return the same error as GRBL does
if (rate_mm_s <= 0.0F) {
gcode->is_error = true;
gcode->txt_after_ok = (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0");
return false;
}
// Scary math.
float center_axis0 = this->machine_position[this->plane_axis_0] + offset[this->plane_axis_0];
float center_axis1 = this->machine_position[this->plane_axis_1] + offset[this->plane_axis_1];
float linear_travel = target[this->plane_axis_2] - this->machine_position[this->plane_axis_2];
float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to start position
float r_axis1 = -offset[this->plane_axis_1];
float rt_axis0 = target[this->plane_axis_0] - this->machine_position[this->plane_axis_0] - offset[this->plane_axis_0]; // Radius vector from center to target position
float rt_axis1 = target[this->plane_axis_1] - this->machine_position[this->plane_axis_1] - offset[this->plane_axis_1];
float angular_travel = 0;
//check for condition where atan2 formula will fail due to everything canceling out exactly
if ((this->machine_position[this->plane_axis_0] == target[this->plane_axis_0]) && (this->machine_position[this->plane_axis_1] == target[this->plane_axis_1])) {
if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle
angular_travel = (-2 * (float)PI);
}
else { // set angular_travel to 2pi for a counterclockwise full circle
angular_travel = (2 * (float)PI);
}
}
else {
// Patch from GRBL Firmware - Christoph Baumann 04072015
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
// Only run if not a full circle or angular travel will incorrectly result in 0.0f
angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; } //Math for XZ plane is reverse of other 2 planes
if (is_clockwise) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs
if (angular_travel > 0) { angular_travel -= (2 * (float)PI); }
}
else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
if (angular_travel < 0) { angular_travel += (2 * (float)PI); }
}
}
// initialize linear travel for ABC
#if SMOOTHIEWARE_MAX_ROBOT_ACTUATORS > 3
float abc_travel[n_motors - 3];
for (int i = A_AXIS; i < n_motors; i++) {
abc_travel[i - 3] = target[i] - this->machine_position[i];
}
#endif
// Find the distance for this gcode
float millimeters_of_travel = (float)utilities::hypot(angular_travel * radius, utilities::fabsf(linear_travel));
// We don't care about non-XYZ moves ( for example the extruder produces some of those )
if (millimeters_of_travel < 0.000001F) {
return false;
}
// limit segments by maximum arc error
float arc_segment = (float)args_.mm_per_arc_segment;
if ((args_.mm_max_arc_error > 0) && (2 * radius > args_.mm_max_arc_error)) {
float min_err_segment = 2 * (float)utilities::sqrtf((args_.mm_max_arc_error * (2 * radius - args_.mm_max_arc_error)));
if (args_.mm_per_arc_segment < min_err_segment) {
arc_segment = min_err_segment;
}
}
// catch fall through on above
if (arc_segment < 0.0001F) {
arc_segment = 0.5F; /// the old default, so we avoid the divide by zero
}
// Figure out how many segments for this gcode
// TODO for deltas we need to make sure we are at least as many segments as requested, also if mm_per_line_segment is set we need to use the
uint16_t segments = (uint16_t)floorf(millimeters_of_travel / arc_segment);
bool moved = false;
if (segments > 1) {
float theta_per_segment = angular_travel / segments;
float linear_per_segment = linear_travel / segments;
#if SMOOTHIEWARE_MAX_ROBOT_ACTUATORS > 3
float abc_per_segment[n_motors - 3];
for (int i = 0; i < n_motors - 3; i++) {
abc_per_segment[i] = abc_travel[i] / segments;
}
#endif
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
r_T = [cos(phi) -sin(phi);
sin(phi) cos(phi] * r ;
For arc generation, the center of the circle is the axis of rotation and the radius vector is
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. This requires only two cos() and sin() computations to form the rotation
matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
all float numbers are single precision on the Arduino. (True float precision will not have
round off issues for CNC applications.) Single precision error can accumulate to be greater than
tool precision in some cases. Therefore, arc path correction is implemented.
Small angle approximation may be used to reduce computation overhead further. This approximation
holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
issue for CNC machines with the single precision Arduino calculations.
This approximation also allows mc_arc to immediately insert a line segment into the planner
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
float cos_T = 1 - 0.5F * theta_per_segment * theta_per_segment; // Small angle approximation
float sin_T = theta_per_segment;
float arc_target[n_motors];
float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
int8_t count = 0;
// init array for all axis
utilities::memcpy(arc_target, machine_position, n_motors * sizeof(float));
// Initialize the linear axis
arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2];
for (i = 1; i < segments; i++) { // Increment (segments-1)
if (THEKERNEL->is_halted()) return false; // don't queue any more segments
if (count < args_.n_arc_correction) {
// Apply vector rotation matrix
r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
r_axis1 = r_axisi;
count++;
}
else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = (float)utilities::cosf(i * theta_per_segment);
sin_Ti = (float)utilities::sinf(i * theta_per_segment);
r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti;
r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti;
count = 0;
}
// Update arc_target location
arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
arc_target[this->plane_axis_2] += linear_per_segment;
#if SMOOTHIEWARE_MAX_ROBOT_ACTUATORS > 3
for (int a = A_AXIS; a < n_motors; a++) {
arc_target[a] += abc_per_segment[a - 3];
}
#endif
// Append this segment to the queue
bool b = this->append_milestone(arc_target, rate_mm_s);
moved = moved || b;
}
}
// Ensure last segment arrives at target location.
if (this->append_milestone(target, rate_mm_s)) moved = true;
return moved;
}
bool smoothieware::append_milestone(const float target[], double rate_mm_s)
{
double rate_mm_min = rate_mm_s * 60;
// create the target position
firmware_position gcode_target;
gcode_target.x = target[AxisEnum::X_AXIS];
gcode_target.y = target[AxisEnum::Y_AXIS];
gcode_target.z = target[AxisEnum::Z_AXIS];
gcode_target.e = target[AxisEnum::E_AXIS];
gcode_target.f = rate_mm_min;
if (gcodes_.size() > 0)
{
gcodes_ += "\n";
}
// Generate the gcode
gcodes_ += g1_command(gcode_target);
return true;
return true;
}
|