First arcs version. (Arcs not working ok)
This commit is contained in:
Erik van der Zalm
parent
2e8e8878e5
commit
0b82465168
@ -3,6 +3,9 @@
|
||||
|
||||
//#define DEBUG_STEPS
|
||||
|
||||
#define MM_PER_ARC_SEGMENT 1
|
||||
#define N_ARC_CORRECTION 25
|
||||
|
||||
// BASIC SETTINGS: select your board type, thermistor type, axis scaling, and endstop configuration
|
||||
|
||||
//// The following define selects which electronics board you have. Please choose the one that matches your setup
|
||||
|
||||
@ -35,6 +35,7 @@
|
||||
#include "planner.h"
|
||||
#include "stepper.h"
|
||||
#include "temperature.h"
|
||||
#include "motion_control.h"
|
||||
|
||||
#ifdef SIMPLE_LCD
|
||||
#include "Simplelcd.h"
|
||||
@ -113,6 +114,7 @@ float destination[NUM_AXIS] = {
|
||||
0.0, 0.0, 0.0, 0.0};
|
||||
float current_position[NUM_AXIS] = {
|
||||
0.0, 0.0, 0.0, 0.0};
|
||||
float offset[3] = {0.0, 0.0, 0.0};
|
||||
bool home_all_axis = true;
|
||||
float feedrate = 1500.0, next_feedrate, saved_feedrate;
|
||||
long gcode_N, gcode_LastN;
|
||||
@ -441,6 +443,8 @@ inline void get_command()
|
||||
switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
|
||||
case 0:
|
||||
case 1:
|
||||
case 2:
|
||||
case 3:
|
||||
#ifdef SDSUPPORT
|
||||
if(savetosd)
|
||||
break;
|
||||
@ -543,6 +547,16 @@ inline void process_commands()
|
||||
//ClearToSend();
|
||||
return;
|
||||
//break;
|
||||
case 2: // G2 - CW ARC
|
||||
get_arc_coordinates();
|
||||
prepare_arc_move(true);
|
||||
previous_millis_cmd = millis();
|
||||
return;
|
||||
case 3: // G3 - CCW ARC
|
||||
get_arc_coordinates();
|
||||
prepare_arc_move(false);
|
||||
previous_millis_cmd = millis();
|
||||
return;
|
||||
case 4: // G4 dwell
|
||||
codenum = 0;
|
||||
if(code_seen('P')) codenum = code_value(); // milliseconds to wait
|
||||
@ -1139,6 +1153,13 @@ inline void get_coordinates()
|
||||
}
|
||||
}
|
||||
|
||||
inline void get_arc_coordinates()
|
||||
{
|
||||
get_coordinates();
|
||||
if(code_seen("I")) offset[0] = code_value();
|
||||
if(code_seen("J")) offset[1] = code_value();
|
||||
}
|
||||
|
||||
void prepare_move()
|
||||
{
|
||||
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60.0/100.0);
|
||||
@ -1147,7 +1168,122 @@ void prepare_move()
|
||||
}
|
||||
}
|
||||
|
||||
void prepare_arc_move(char isclockwise) {
|
||||
#if 0
|
||||
if (radius_mode) {
|
||||
/*
|
||||
We need to calculate the center of the circle that has the designated radius and passes
|
||||
through both the current position and the target position. This method calculates the following
|
||||
set of equations where [x,y] is the vector from current to target position, d == magnitude of
|
||||
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
|
||||
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
|
||||
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
|
||||
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
|
||||
|
||||
d^2 == x^2 + y^2
|
||||
h^2 == r^2 - (d/2)^2
|
||||
i == x/2 - y/d*h
|
||||
j == y/2 + x/d*h
|
||||
|
||||
O <- [i,j]
|
||||
- |
|
||||
r - |
|
||||
- |
|
||||
- | h
|
||||
- |
|
||||
[0,0] -> C -----------------+--------------- T <- [x,y]
|
||||
| <------ d/2 ---->|
|
||||
|
||||
C - Current position
|
||||
T - Target position
|
||||
O - center of circle that pass through both C and T
|
||||
d - distance from C to T
|
||||
r - designated radius
|
||||
h - distance from center of CT to O
|
||||
|
||||
Expanding the equations:
|
||||
|
||||
d -> sqrt(x^2 + y^2)
|
||||
h -> sqrt(4 * r^2 - x^2 - y^2)/2
|
||||
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
|
||||
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
|
||||
|
||||
Which can be written:
|
||||
|
||||
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
|
||||
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
|
||||
|
||||
Which we for size and speed reasons optimize to:
|
||||
|
||||
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
|
||||
i = (x - (y * h_x2_div_d))/2
|
||||
j = (y + (x * h_x2_div_d))/2
|
||||
|
||||
*/
|
||||
|
||||
// Calculate the change in position along each selected axis
|
||||
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
|
||||
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
|
||||
|
||||
clear_vector(offset);
|
||||
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
|
||||
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
|
||||
// real CNC, and thus - for practical reasons - we will terminate promptly:
|
||||
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
|
||||
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
|
||||
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
|
||||
|
||||
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
|
||||
the left hand circle will be generated - when it is negative the right hand circle is generated.
|
||||
|
||||
|
||||
T <-- Target position
|
||||
|
||||
^
|
||||
Clockwise circles with this center | Clockwise circles with this center will have
|
||||
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
|
||||
\ | /
|
||||
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
||||
|
|
||||
|
|
||||
|
||||
C <-- Current position */
|
||||
|
||||
|
||||
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
|
||||
// even though it is advised against ever generating such circles in a single line of g-code. By
|
||||
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
|
||||
// travel and thus we get the unadvisably long arcs as prescribed.
|
||||
if (r < 0) {
|
||||
h_x2_div_d = -h_x2_div_d;
|
||||
r = -r; // Finished with r. Set to positive for mc_arc
|
||||
}
|
||||
// Complete the operation by calculating the actual center of the arc
|
||||
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
|
||||
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
|
||||
|
||||
} else { // Offset mode specific computations
|
||||
#endif
|
||||
float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
|
||||
|
||||
// }
|
||||
|
||||
// Set clockwise/counter-clockwise sign for mc_arc computations
|
||||
// uint8_t isclockwise = false;
|
||||
// if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
|
||||
|
||||
// Trace the arc
|
||||
mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60.0/100.0, r, isclockwise);
|
||||
|
||||
// }
|
||||
|
||||
// As far as the parser is concerned, the position is now == target. In reality the
|
||||
// motion control system might still be processing the action and the real tool position
|
||||
// in any intermediate location.
|
||||
for(int ii=0; ii < NUM_AXIS; ii++) {
|
||||
current_position[ii] = destination[ii];
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_WATCHDOG
|
||||
|
||||
@ -1219,7 +1355,7 @@ inline void kill()
|
||||
disable_e();
|
||||
|
||||
if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT);
|
||||
Serial.println("!! Printer halted. kill() called!!");
|
||||
Serial.println("!! Printer halted. kill() called !!");
|
||||
while(1); // Wait for reset
|
||||
}
|
||||
|
||||
@ -1233,3 +1369,4 @@ void manage_inactivity(byte debug) {
|
||||
}
|
||||
check_axes_activity();
|
||||
}
|
||||
|
||||
|
||||
133
Marlin/motion_control.cpp
Normal file
133
Marlin/motion_control.cpp
Normal file
@ -0,0 +1,133 @@
|
||||
/*
|
||||
motion_control.c - high level interface for issuing motion commands
|
||||
Part of Grbl
|
||||
|
||||
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
||||
Copyright (c) 2011 Sungeun K. Jeon
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
//#include "motion_control.h"
|
||||
#include "Configuration.h"
|
||||
#include "Marlin.h"
|
||||
//#include <util/delay.h>
|
||||
//#include <math.h>
|
||||
//#include <stdlib.h>
|
||||
#include "stepper.h"
|
||||
#include "planner.h"
|
||||
|
||||
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
|
||||
// segment is configured in settings.mm_per_arc_segment.
|
||||
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
|
||||
uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise)
|
||||
{
|
||||
// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
|
||||
// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
|
||||
Serial.println("mc_arc");
|
||||
float center_axis0 = position[axis_0] + offset[axis_0];
|
||||
float center_axis1 = position[axis_1] + offset[axis_1];
|
||||
float linear_travel = target[axis_linear] - position[axis_linear];
|
||||
float r_axis0 = -offset[axis_0]; // Radius vector from center to current location
|
||||
float r_axis1 = -offset[axis_1];
|
||||
float rt_axis0 = target[axis_0] - center_axis0;
|
||||
float rt_axis1 = target[axis_1] - center_axis1;
|
||||
|
||||
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
|
||||
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
|
||||
if (angular_travel < 0) { angular_travel += 2*M_PI; }
|
||||
if (isclockwise) { angular_travel -= 2*M_PI; }
|
||||
|
||||
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
|
||||
if (millimeters_of_travel == 0.0) { return; }
|
||||
uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
|
||||
/*
|
||||
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
|
||||
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
|
||||
// all segments.
|
||||
if (invert_feed_rate) { feed_rate *= segments; }
|
||||
*/
|
||||
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 double numbers are single precision on the Arduino. (True double 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.5*theta_per_segment*theta_per_segment; // Small angle approximation
|
||||
float sin_T = theta_per_segment;
|
||||
|
||||
float arc_target[3];
|
||||
float sin_Ti;
|
||||
float cos_Ti;
|
||||
float r_axisi;
|
||||
uint16_t i;
|
||||
int8_t count = 0;
|
||||
|
||||
// Initialize the linear axis
|
||||
arc_target[axis_linear] = position[axis_linear];
|
||||
|
||||
for (i = 1; i<segments; i++) { // Increment (segments-1)
|
||||
|
||||
if (count < 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 = cos(i*theta_per_segment);
|
||||
sin_Ti = sin(i*theta_per_segment);
|
||||
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
|
||||
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
|
||||
count = 0;
|
||||
}
|
||||
|
||||
// Update arc_target location
|
||||
arc_target[axis_0] = center_axis0 + r_axis0;
|
||||
arc_target[axis_1] = center_axis1 + r_axis1;
|
||||
arc_target[axis_linear] += linear_per_segment;
|
||||
plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], target[E_AXIS], feed_rate);
|
||||
|
||||
}
|
||||
// Ensure last segment arrives at target location.
|
||||
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate);
|
||||
|
||||
// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
|
||||
}
|
||||
|
||||
32
Marlin/motion_control.h
Normal file
32
Marlin/motion_control.h
Normal file
@ -0,0 +1,32 @@
|
||||
/*
|
||||
motion_control.h - high level interface for issuing motion commands
|
||||
Part of Grbl
|
||||
|
||||
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
||||
Copyright (c) 2011 Sungeun K. Jeon
|
||||
|
||||
Grbl is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
Grbl is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef motion_control_h
|
||||
#define motion_control_h
|
||||
|
||||
// Execute an arc in offset mode format. position == current xyz, target == target xyz,
|
||||
// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
|
||||
// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
|
||||
// for vector transformation direction.
|
||||
void mc_arc(float *position, float *target, float *offset, unsigned char axis_0, unsigned char axis_1,
|
||||
unsigned char axis_linear, float feed_rate, float radius, unsigned char isclockwise);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user