#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
#define segment_z_moves_checksum CHECKSUM("segment_z_moves")
#define save_g92_checksum CHECKSUM("save_g92")
+#define save_g54_checksum CHECKSUM("save_g54")
#define set_g92_checksum CHECKSUM("set_g92")
// arm solutions
#define dir_pin_checksum CHEKCSUM("dir_pin")
#define en_pin_checksum CHECKSUM("en_pin")
-#define steps_per_mm_checksum CHECKSUM("steps_per_mm")
-#define max_rate_checksum CHECKSUM("max_rate")
+#define max_speed_checksum CHECKSUM("max_speed")
#define acceleration_checksum CHECKSUM("acceleration")
#define z_acceleration_checksum CHECKSUM("z_acceleration")
#define ymax_checksum CHECKSUM("y_max")
#define zmax_checksum CHECKSUM("z_max")
-
-#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
#define PI 3.14159265358979323846F // force to be float, do not use M_PI
// The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
this->max_speeds[X_AXIS] = THEKERNEL->config->value(x_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
this->max_speeds[Y_AXIS] = THEKERNEL->config->value(y_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
this->max_speeds[Z_AXIS] = THEKERNEL->config->value(z_axis_max_speed_checksum )->by_default( 300.0F)->as_number() / 60.0F;
+ this->max_speed = THEKERNEL->config->value(max_speed_checksum )->by_default( -60.0F)->as_number() / 60.0F;
this->segment_z_moves = THEKERNEL->config->value(segment_z_moves_checksum )->by_default(true)->as_bool();
this->save_g92 = THEKERNEL->config->value(save_g92_checksum )->by_default(false)->as_bool();
+ this->save_g54 = THEKERNEL->config->value(save_g54_checksum )->by_default(THEKERNEL->is_grbl_mode())->as_bool();
string g92 = THEKERNEL->config->value(set_g92_checksum )->by_default("")->as_string();
if(!g92.empty()) {
// optional setting for a fixed G92 offset
// so the first move can be correct if homing is not performed
ActuatorCoordinates actuator_pos;
arm_solution->cartesian_to_actuator(machine_position, actuator_pos);
- for (size_t i = 0; i < n_motors; i++)
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
actuators[i]->change_last_milestone(actuator_pos[i]);
+ }
+
+ // initialize any extra axis to machine position
+ for (size_t i = A_AXIS; i < n_motors; i++) {
+ actuators[i]->change_last_milestone(machine_position[i]);
+ }
//this->clearToolOffset();
arm_solution->actuator_to_cartesian(current_position, pos);
}
-int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const
+void Robot::print_position(uint8_t subcode, std::string& res, bool ignore_extruders) const
{
// M114.1 is a new way to do this (similar to how GRBL does it).
// it returns the realtime position based on the current step position of the actuators.
// this does require a FK to get a machine position from the actuator position
// and then invert all the transforms to get a workspace position from machine position
// M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
- int n = 0;
+ uint32_t n = 0;
+ char buf[64];
if(subcode == 0) { // M114 print WCS
wcs_t pos= mcs2wcs(machine_position);
- n = snprintf(buf, bufsize, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
+ n = snprintf(buf, sizeof(buf), "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
} else if(subcode == 4) {
// M114.4 print last milestone
- n = snprintf(buf, bufsize, "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position[X_AXIS], machine_position[Y_AXIS], machine_position[Z_AXIS]);
+ n = snprintf(buf, sizeof(buf), "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position[X_AXIS], machine_position[Y_AXIS], machine_position[Z_AXIS]);
} else if(subcode == 5) {
// M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
// will differ from LMS by the compensation at the current position otherwise
- n = snprintf(buf, bufsize, "CMP: X:%1.4f Y:%1.4f Z:%1.4f", compensated_machine_position[X_AXIS], compensated_machine_position[Y_AXIS], compensated_machine_position[Z_AXIS]);
+ n = snprintf(buf, sizeof(buf), "CMP: X:%1.4f Y:%1.4f Z:%1.4f", compensated_machine_position[X_AXIS], compensated_machine_position[Y_AXIS], compensated_machine_position[Z_AXIS]);
} else {
// get real time positions
if(subcode == 1) { // M114.1 print realtime WCS
wcs_t pos= mcs2wcs(mpos);
- n = snprintf(buf, bufsize, "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
+ n = snprintf(buf, sizeof(buf), "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
} else if(subcode == 2) { // M114.2 print realtime Machine coordinate system
- n = snprintf(buf, bufsize, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
+ n = snprintf(buf, sizeof(buf), "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
} else if(subcode == 3) { // M114.3 print realtime actuator position
// get real time current actuator position in mm
actuators[Y_AXIS]->get_current_position(),
actuators[Z_AXIS]->get_current_position()
};
- n = snprintf(buf, bufsize, "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+ n = snprintf(buf, sizeof(buf), "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
}
}
+ if(n > sizeof(buf)) n= sizeof(buf);
+ res.append(buf, n);
+
#if MAX_ROBOT_ACTUATORS > 3
// deal with the ABC axis
for (int i = A_AXIS; i < n_motors; ++i) {
- if(actuators[i]->is_extruder()) continue; // don't show an extruder as that will be E
+ n= 0;
+ if(ignore_extruders && actuators[i]->is_extruder()) continue; // don't show an extruder as that will be E
if(subcode == 4) { // M114.4 print last milestone
- n += snprintf(&buf[n], bufsize-n, " %c:%1.4f", 'A'+i-A_AXIS, machine_position[i]);
+ n= snprintf(buf, sizeof(buf), " %c:%1.4f", 'A'+i-A_AXIS, machine_position[i]);
}else if(subcode == 2 || subcode == 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
// current actuator position
- n += snprintf(&buf[n], bufsize-n, " %c:%1.4f", 'A'+i-A_AXIS, actuators[i]->get_current_position());
+ n= snprintf(buf, sizeof(buf), " %c:%1.4f", 'A'+i-A_AXIS, actuators[i]->get_current_position());
}
+ if(n > sizeof(buf)) n= sizeof(buf);
+ if(n > 0) res.append(buf, n);
}
#endif
-
- return n;
}
// converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
actuators[selected_extruder]->change_last_milestone(get_e_scale_fnc ? e*get_e_scale_fnc() : e);
}
}
+ if(gcode->subcode == 0 && gcode->get_num_args() > 0) {
+ for (int i = A_AXIS; i < n_motors; i++) {
+ // ABC just need to set machine_position and compensated_machine_position if specified
+ char axis= 'A'+i-3;
+ if(!actuators[i]->is_extruder() && gcode->has_letter(axis)) {
+ float ap= gcode->get_value(axis);
+ machine_position[i]= compensated_machine_position[i]= ap;
+ actuators[i]->change_last_milestone(ap); // this updates the last_milestone in the actuator
+ }
+ }
+ }
#endif
return;
return;
case 114:{
- char buf[128];
- int n= print_position(gcode->subcode, buf, sizeof buf);
- if(n > 0) gcode->txt_after_ok.append(buf, n);
+ std::string buf;
+ print_position(gcode->subcode, buf, true); // ignore extruders as they will print E themselves
+ gcode->txt_after_ok.append(buf);
return;
}
if(actuators[i]->is_extruder()) continue; //extruders handle this themselves
gcode->stream->printf(" %c: %g ", 'A' + i - A_AXIS, actuators[i]->get_max_rate());
}
+ }else{
+ gcode->stream->printf(" S: %g ", this->max_speed);
}
gcode->add_nl = true;
actuators[i]->set_max_rate(v);
}
}
+
+ }else{
+ if(gcode->has_letter('S')) max_speed= gcode->get_value('S');
}
break;
case 211: // M211 Sn turns soft endstops on/off
- soft_endstop_enabled= gcode->get_uint('S') == 1;
+ if(gcode->has_letter('S')) {
+ soft_endstop_enabled= gcode->get_uint('S') == 1;
+ }else{
+ gcode->stream->printf("Soft endstops are %s", soft_endstop_enabled ? "Enabled" : "Disabled");
+ for (int i = X_AXIS; i <= Z_AXIS; ++i) {
+ if(isnan(soft_endstop_min[i])) {
+ gcode->stream->printf(",%c min is disabled", 'X'+i);
+ }
+ if(isnan(soft_endstop_max[i])) {
+ gcode->stream->printf(",%c max is disabled", 'X'+i);
+ }
+ if(!is_homed(i)) {
+ gcode->stream->printf(",%c axis is not homed", 'X'+i);
+ }
+ }
+ gcode->stream->printf("\n");
+ }
break;
case 220: // M220 - speed override percentage
gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, isnan(THEKERNEL->planner->z_junction_deviation)?-1:THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
- gcode->stream->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS]);
+ gcode->stream->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f S%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS], this->max_speed);
gcode->stream->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
for (int i = 0; i < n_motors; ++i) {
// save wcs_offsets and current_wcs
// TODO this may need to be done whenever they change to be compliant
- gcode->stream->printf(";WCS settings\n");
- gcode->stream->printf("%s\n", wcs2gcode(current_wcs).c_str());
- int n = 1;
- for(auto &i : wcs_offsets) {
- if(i != wcs_t(0, 0, 0)) {
- float x, y, z;
- std::tie(x, y, z) = i;
- gcode->stream->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n, x, y, z, wcs2gcode(n-1).c_str());
+ if(save_g54) {
+ gcode->stream->printf(";WCS settings\n");
+ gcode->stream->printf("%s\n", wcs2gcode(current_wcs).c_str());
+ int n = 1;
+ for(auto &i : wcs_offsets) {
+ if(i != wcs_t(0, 0, 0)) {
+ float x, y, z;
+ std::tie(x, y, z) = i;
+ gcode->stream->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n, x, y, z, wcs2gcode(n-1).c_str());
+ }
+ ++n;
}
- ++n;
}
if(save_g92) {
// linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
}
}else{
- // already in machine coordinates, we do not add tool offset for that
+ // already in machine coordinates, we do not add wcs or tool offset for that
for(int i= X_AXIS; i <= Z_AXIS; ++i) {
if(!isnan(param[i])) target[i] = param[i];
}
break;
}
+ // needed to act as start of next arc command
+ memcpy(arc_milestone, target, sizeof(arc_milestone));
+
if(moved) {
// set machine_position to the calculated target
memcpy(machine_position, target, n_motors*sizeof(float));
if(!is_homed(i)) continue;
if( (!isnan(soft_endstop_min[i]) && transformed_target[i] < soft_endstop_min[i]) || (!isnan(soft_endstop_max[i]) && transformed_target[i] > soft_endstop_max[i]) ) {
if(soft_endstop_halt) {
- THEKERNEL->streams->printf("Soft Endstop %c was exceeded - reset or M999 required\n", i+'X');
+ if(THEKERNEL->is_grbl_mode()) {
+ THEKERNEL->streams->printf("error:");
+ }else{
+ THEKERNEL->streams->printf("Error: ");
+ }
+
+ THEKERNEL->streams->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i+'X');
THEKERNEL->call_event(ON_HALT, nullptr);
return false;
//} else if(soft_endstop_truncate) {
// TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
+ // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
} else {
// ignore it
- THEKERNEL->streams->printf("WARNING Soft Endstop %c was exceeded - entire move ignored\n", i+'X');
+ if(THEKERNEL->is_grbl_mode()) {
+ THEKERNEL->streams->printf("error:");
+ }else{
+ THEKERNEL->streams->printf("Error: ");
+ }
+ THEKERNEL->streams->printf("Soft Endstop %c was exceeded - entire move ignored\n", i+'X');
return false;
}
}
// as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move
if(!auxilliary_move && distance < 0.00001F) return false;
-
if(!auxilliary_move) {
for (size_t i = X_AXIS; i < N_PRIMARY_AXIS; i++) {
// find distance unit vector for primary axis only
unit_vec[i] = deltas[i] / distance;
// Do not move faster than the configured cartesian limits for XYZ
- if ( max_speeds[i] > 0 ) {
+ if ( i <= Z_AXIS && max_speeds[i] > 0 ) {
float axis_speed = fabsf(unit_vec[i] * rate_mm_s);
if (axis_speed > max_speeds[i])
rate_mm_s *= ( max_speeds[i] / axis_speed );
}
}
+
+ if(this->max_speed > 0 && rate_mm_s > this->max_speed) {
+ rate_mm_s= this->max_speed;
+ }
}
// find actuator position given the machine position, use actual adjusted target
}
}
+ // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
+ while(THEKERNEL->get_feed_hold()) {
+ THEKERNEL->call_event(ON_IDLE, this);
+ // if we also got a HALT then break out of this
+ if(THEKERNEL->is_halted()) return false;
+ }
+
// Append the block to the planner
// NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
+ // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
if(THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration, s_value, is_g123)) {
// this is the new compensated machine position
memcpy(this->compensated_machine_position, transformed_target, n_motors*sizeof(float));
// We always add another point after this loop so we stop at segments-1, ie i < segments
for (int i = 1; i < segments; i++) {
if(THEKERNEL->is_halted()) return false; // don't queue any more segments
- for (int i = 0; i < n_motors; i++)
- segment_end[i] += segment_delta[i];
+ for (int j = 0; j < n_motors; j++)
+ segment_end[j] += segment_delta[j];
// Append the end of this segment to the queue
+ // this can block waiting for free block queue or if in feed hold
bool b= this->append_milestone(segment_end, rate_mm_s);
moved= moved || b;
}
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 current location
+ // Scary math.
+ // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements
+ float center_axis0 = this->arc_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
+ float center_axis1 = this->arc_milestone[this->plane_axis_1] + offset[this->plane_axis_1];
+ float linear_travel = target[this->plane_axis_2] - this->arc_milestone[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] - center_axis0;
- float rt_axis1 = target[this->plane_axis_1] - center_axis1;
-
- // Patch from GRBL Firmware - Christoph Baumann 04072015
- // CCW angle between position and target from circle center. Only one atan2() trig computation required.
- float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
- if (is_clockwise) { // Correct atan2 output per direction
- if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); }
+ float rt_axis0 = target[this->plane_axis_0] - this->arc_milestone[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->arc_milestone[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->arc_milestone[this->plane_axis_0]==target[this->plane_axis_0]) && (this->arc_milestone[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 * PI);
+ } else { // set angular_travel to 2pi for a counterclockwise full circle
+ angular_travel = (2 * PI);
+ }
} else {
- if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * PI); }
+ // 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 * PI); }
+ } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs
+ if (angular_travel < 0) { angular_travel += (2 * PI); }
+ }
}
// Find the distance for this gcode
float millimeters_of_travel = hypotf(angular_travel * radius, fabsf(linear_travel));
// We don't care about non-XYZ moves ( for example the extruder produces some of those )
- if( millimeters_of_travel < 0.00001F ) {
+ if( millimeters_of_travel < 0.000001F ) {
return false;
}
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 = ceilf(millimeters_of_travel / arc_segment);
-
- //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
- float theta_per_segment = angular_travel / segments;
- float linear_per_segment = linear_travel / segments;
-
- /* 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;
-
- // TODO we need to handle the ABC axis here by segmenting them
- float arc_target[3];
- float sin_Ti;
- float cos_Ti;
- float r_axisi;
- uint16_t i;
- int8_t count = 0;
+ uint16_t segments = floorf(millimeters_of_travel / arc_segment);
+ bool moved= false;
- // Initialize the linear axis
- arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2];
+ if(segments > 1) {
+ float theta_per_segment = angular_travel / segments;
+ float linear_per_segment = linear_travel / segments;
+
+ /* 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;
+
+ // TODO we need to handle the ABC axis here by segmenting them
+ 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
+ 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
- bool moved= false;
- for (i = 1; i < segments; i++) { // Increment (segments-1)
- if(THEKERNEL->is_halted()) return false; // don't queue any more segments
-
- if (count < this->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 = cosf(i * theta_per_segment);
- sin_Ti = 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;
- }
+ if (count < this->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 = cosf(i * theta_per_segment);
+ sin_Ti = 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;
+ // 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;
- // Append this segment to the queue
- bool b= this->append_milestone(arc_target, rate_mm_s);
- moved= moved || b;
+ // 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.