ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

added code to estimate steps in arc in order to support helical motion

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-02-09 15:47:51 +01:00
parent 2992683c8d
commit c2981be94a
5 changed files with 178 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
geometry.c
View File

@ -18,7 +18,10 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#include "geometry.h"
#include <avr/io.h>
#include <math.h>
#include <stdlib.h>
// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis,
// positive to the right.
@ -36,3 +39,59 @@ double theta(double x, double y)
}
}
}
/*
Quadrants of the circle
+---- 0 ----+ 0 - y is always positive and |x| < |y|
| | 1 - x is always positive and |x| > |y|
| | 2 - y is always negative and |x| < |y|
3 + 1 3 - x is always negative and |x| > |y|
| |
| |
+---- 2 ----+
*/
// Find the quadrant of the coordinate
int quadrant_of_the_circle(int32_t x, int32_t y) {
if (abs(x)<abs(y)){
if (y>0) {
return(0);
} else {
return(2);
}
} else {
if (x>0) {
return(1);
} else {
return(3);
}
}
}
// Very specialized helper to calculate the amount of steps to travel in the given quadrant of a circle provided the
// axial direction of the quadrant, the angular_direction of travel (-1 or +1) and amount of steps in one half quadrant
// of the circle.
uint32_t steps_in_partial_quadrant(int32_t x, int32_t y, int quadrant, int angular_direction,
int32_t steps_in_half_quadrant) {
if (quadrant_horizontal(quadrant)) { // A horizontal quadrant
if ((angular_direction == 1) ^ (quadrant == 2)) {
return(steps_in_half_quadrant-x);
} else {
return(x+steps_in_half_quadrant);
}
} else { // A vertical quadrant
if ((angular_direction == 1) ^ (quadrant == 3)) {
return(steps_in_half_quadrant-y);
} else {
return(y+steps_in_half_quadrant);
}
}
}
// Counts the amount of full quadrants between quadrant_start and quadrant_target along the angular_direction
int full_quadrants_between(int quadrant_start, int quadrant_target, int angular_direction) {
int diff = angular_direction*(quadrant_target-quadrant_start);
if (diff <= 0) { diff += 4; }
return (diff-1);
}

28
geometry.h
View File

@ -20,8 +20,36 @@
#ifndef geometry_h
#define geometry_h
#include <avr/io.h>
// Find the angle from the positive y axis to the given point with respect to origo.
double theta(double x, double y);
// Find the quadrant of the coordinate
int quadrant_of_the_circle(int32_t x, int32_t y);
/*
Quadrants of the circle
+---- 0 ----+ 0 - y is always positive and |x| < |y|
| | 1 - x is always positive and |x| > |y|
| | 2 - y is always negative and |x| < |y|
3 + 1 3 - x is always negative and |x| > |y|
| |
| |
+---- 2 ----+
*/
// A macro to decide if a quadrant-number represent a horizontal quadrant
#define quadrant_horizontal(q) ((q % 2) == 0)
// Very specialized helper to calculate the amount of steps to travel in the given quadrant of a circle provided the
// axial direction of the quadrant, the angular_direction of travel (-1 or +1) and amount of steps in one half quadrant
// of the circle.
uint32_t steps_in_partial_quadrant(int32_t x, int32_t y, int horizontal_quadrant, int angular_direction,
int32_t steps_in_half_quadrant);
// Counts the amount of full quadrants between quadrant_start and quadrant_target along the angular_direction
int full_quadrants_between(int quadrant_start, int quadrant_target, int angular_direction);
#endif

88
motion_control.c
View File

@ -34,6 +34,9 @@
#include <stdlib.h>
#include "nuts_bolts.h"
#include "stepper.h"
#include "geometry.h"
#include "wiring_serial.h"
#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
@ -74,7 +77,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
maximum_steps; // The larges absolute step-count of any axis
// Setup
// Setup ---------------------------------------------------------------------------------------------------
target[X_AXIS] = x*X_STEPS_PER_MM;
target[Y_AXIS] = y*Y_STEPS_PER_MM;
@ -109,7 +112,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
st_buffer_pace(((millimeters_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / maximum_steps);
}
// Execution
// Execution -----------------------------------------------------------------------------------------------
mode = MC_MODE_LINEAR;
@ -152,8 +155,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// local coordinate system of the arc-generator where [0,0] is the
// center of the arc.
int target_direction_x, target_direction_y; // signof(target_x)*angular_direction precalculated for speed
int32_t error, x2, y2; // error is always == (x**2 + y**2 - radius**2),
// x2 is always 2*x, y2 is always 2*y
int32_t error; // error is always == (x**2 + y**2 - radius**2),
uint8_t axis_x, axis_y; // maps the arc axes to stepper axes
int8_t diagonal_bits; // A bitmask with the stepper bits for both selected axes set
int incomplete; // True if the arc has not reached its target yet
@ -164,6 +166,9 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
if(radius_steps == 0) { return; }
// Setup arc interpolation --------------------------------------------------------------------------------
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
angular_direction = signof(angular_travel);
// Calculate the initial position and target position in the local coordinate system of the arc
@ -178,26 +183,71 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we
// are exactly on top of the arc.
error = x*x + y*y - radius_steps*radius_steps;
// Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
// by keeping track of the doubles of the arc coordinates at all times.
x2 = 2*x;
y2 = 2*y;
// Set up a vector with the steppers we are going to use tracing the plane of this arc
diagonal_bits = st_bit_for_stepper(axis_1);
diagonal_bits |= st_bit_for_stepper(axis_2);
// And map the local coordinate system of the arc onto the tool axes of the selected plane
axis_x = axis_1;
axis_y = axis_2;
// Estimate length of arc in steps -------------------------------------------------------------------------
/*
To support helical motion we need to know in advance how many steppings the arc will need.
The calculations are based on the fact that we trace the circle by offsetting a square. The circle has
four "sides" or quadrants. For each quadrant we step mainly in one axis. The amount steps for one quarter of the
circle (e.g. along the x axis with positive y) is equal to one side of a square inscribed in the circle we
are tracing.
Quadrants of the circle
+---- 0 ----+ 0 - y is always positive and |x| < |y|
| | 1 - x is always positive and |x| > |y|
| | 2 - y is always negative and |x| < |y|
3 + 1 3 - x is always negative and |x| > |y|
| |
| | length of one side: 2*radius/sqrt(2)
+---- 2 ----+
*/
int start_quadrant = quadrant_of_the_circle(start_x, start_y);
int target_quadrant = quadrant_of_the_circle(target_x, target_y);
uint32_t steps_to_travel=0;
// Is the start and target point in the same quadrant?
if (start_quadrant == target_quadrant && (abs(angular_travel) <= (M_PI/2))) {
if(quadrant_horizontal(start_quadrant)) { // a horizontal quadrant where x will be the primary direction
steps_to_travel = abs(target_x-start_x);
} else { // a vertical quadrant where y will be the primary direction
steps_to_travel = abs(target_y-start_y);
}
} else { // the start and target points are in different quadrants
// Lets estimate the amount of steps along one full quadrant
uint32_t steps_in_half_quadrant = ceil(radius_steps/sqrt(2));
// Add the steps in the first partial quadrant
steps_to_travel += steps_in_partial_quadrant(start_x, start_y,
start_quadrant, angular_direction, steps_in_half_quadrant);
// Count the number of full quadrants between the start and end quadrants
uint8_t full_quadrants_traveled = full_quadrants_between(start_quadrant, target_quadrant, angular_direction);
// Add steps for the full quadrants plus some stray steps for "corners"
steps_to_travel += full_quadrants_traveled*(steps_in_half_quadrant*2+1);
// Add the steps in the final partial quadrant. By inverting the angular direction we get the correct number for
// the target quadrant which steps through the opposite part of the quadrant with respect to the start quadrant.
steps_to_travel += steps_in_partial_quadrant(target_x, target_y,
target_quadrant, -angular_direction, steps_in_half_quadrant);
}
// Calculate feed rate -------------------------------------------------------------------------------------
// The amount of steppings performed while tracing a half circle is equal to the sum of sides in a
// square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a half circle:
uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2)));
uint32_t steps_in_half_circle = round((4*radius_steps)/sqrt(2)));
// We then calculate the millimeters of travel along the circumference of that same half circle
double millimeters_half_circumference = radius*M_PI;
// Then we calculate the microseconds between each step as if we will trace the full circle.
// It doesn't matter what fraction of the circle we are actually going to trace. The pace is the same.
st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle);
// Execution
// Execution -----------------------------------------------------------------------------------------------
mode = MC_MODE_ARC;
@ -214,11 +264,11 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
// Check which axis will be "major" for this stepping
if (abs(x)<abs(y)) {
// Step arc horizontally
error += 1+x2*dx;
x+=dx; x2 += 2*dx;
diagonal_error = error + 1 + y2*dy;
error += 1 + 2*x * dx;
x+=dx;
diagonal_error = error + 1 + 2*y*dy;
if(abs(error) >= abs(diagonal_error)) {
y += dy; y2 += 2*dy;
y += dy;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else {
@ -226,11 +276,11 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
}
} else {
// Step arc vertically
error += 1+y2*dy;
y+=dy; y2 += 2*dy;
diagonal_error = error + 1 + x2*dx;
error += 1 + 2*y * dy;
y+=dy;
diagonal_error = error + 1 + 2*x * dx;
if(abs(error) >= abs(diagonal_error)) {
x += dx; x2 += 2*dx;
x += dx;
error = diagonal_error;
step_steppers(diagonal_bits); // step diagonal
} else {
@ -267,7 +317,7 @@ int mc_status()
// Set the direction bits for the stepper motors according to the provided vector.
// direction is an array of three 8 bit integers representing the direction of
// each motor. The values should be -1 (reverse), 0 or 1 (forward).
// each motor. The values should be negative (reverse), 0 or positive (forward).
void set_stepper_directions(int8_t *direction)
{
/* Sorry about this convoluted code! It uses the fact that bit 7 of each direction

26
stepper.c
View File

@ -112,19 +112,20 @@ void st_init()
void st_buffer_step(uint8_t motor_port_bits)
{
if (echo_steps && !(motor_port_bits&0x80)) {
// Buffer nothing unless stepping subsystem is running
if (stepper_mode != STEPPER_MODE_RUNNING) { return; }
// Echo steps. If bit 7 is set, the message is internal to Grbl and should not be echoed
if (echo_steps && !(motor_port_bits&0x80)) {
printByte('!'+motor_port_bits);
}
int i = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
// Calculate the buffer head after we push this byte
int next_buffer_head = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
// If the buffer is full: good! That means we are well ahead of the robot.
// Nap until there is room for more steps.
while(step_buffer_tail == i) { sleep_mode(); }
while(step_buffer_tail == next_buffer_head) { sleep_mode(); }
// Push byte
step_buffer[step_buffer_head] = motor_port_bits;
step_buffer_head = i;
step_buffer_head = next_buffer_head;
}
// Block until all buffered steps are executed
@ -163,18 +164,24 @@ inline void st_stop()
stepper_mode = STEPPER_MODE_STOPPED;
}
// Buffer a pace change. Pace is the rate with which steps are executed. It is measured in microseconds from step to step.
// It is continually adjusted to achieve constant actual feed rate. Unless pace-changes was buffered along with the steps
// they govern they might change at slightly wrong moments in time as the pace would change while the stepper buffer was
// still churning out the previous movement.
void st_buffer_pace(uint32_t microseconds)
{
// Do nothing if the pace in unchanged
if (current_pace == microseconds) { return; }
// Do nothing if the pace in unchanged or the stepping subsytem is not running
if ((current_pace == microseconds) || (stepper_mode != STEPPER_MODE_RUNNING)) { return; }
// If the single-element pace "buffer" is full, sleep until it is popped
while (next_pace != 0) {
sleep_mode();
}
// Buffer the pace change
next_pace = microseconds;
st_buffer_step(PACE_CHANGE_MARKER); // Place a pace-change marker in the step-buffer
}
// Returns a bitmask with the stepper bit for the given axis set
uint8_t st_bit_for_stepper(int axis) {
switch(axis) {
case X_AXIS: return(1<<X_STEP_BIT);
@ -184,6 +191,7 @@ uint8_t st_bit_for_stepper(int axis) {
return(0);
}
// Configures the prescaler and ceiling of timer 1 to produce the given pace as accurately as possible.
void config_pace_timer(uint32_t microseconds)
{
uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;

5
stepper.h
View File

@ -35,7 +35,10 @@ void st_init();
// Returns a bitmask with the stepper bit for the given axis set
uint8_t st_bit_for_stepper(int axis);
// Buffer a change in the rate steps are taken from the buffer and executed
// Buffer a pace change. Pace is the rate with which steps are executed. It is measured in microseconds from step to step.
// It is continually adjusted to achieve constant actual feed rate. Unless pace-changes was buffered along with the steps
// they govern they might change at slightly wrong moments in time as the pace would change while the stepper buffer was
// still churning out the previous movement.
void st_buffer_pace(uint32_t microseconds);
// Buffer a new instruction for the steppers