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

lots and lots of bugfixes after running on reals hardware for the first time

Browse Source
This commit is contained in:
Simen Svale Skogsrud
2009-02-03 09:56:45 +01:00
parent 9799955555
commit 50a9f78088
19 changed files with 827 additions and 368 deletions
Download Patch File Download Diff File Expand all files Collapse all files

369
motion_control.c
View File

@ -35,6 +35,8 @@
#include "nuts_bolts.h"
#include "stepper.h"
#include "wiring_serial.h"
#define ONE_MINUTE_OF_MICROSECONDS 60000000
// Parameters when mode is MC_MODE_ARC
@ -49,10 +51,10 @@ struct LinearMotionParameters {
struct ArcMotionParameters {
int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
uint32_t x, y, target_x, target_y; // current position and target position in the
// local coordinate system of the arc where [0,0] is the
int32_t x, y, target_x, target_y; // current position and target position in the
// local coordinate system of the arc-generator where [0,0] is the
// center of the arc.
int target_direction_x, target_direction_y; // sign(target_x)*angular_direction precalculated for speed
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
uint8_t axis_x, axis_y; // maps the arc axes to stepper axes
@ -69,31 +71,41 @@ struct MotionControlState {
int8_t mode; // The current operation mode
int32_t position[3]; // The current position of the tool in absolute steps
int32_t pace; // Microseconds between each update in the current mode
uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
union {
struct LinearMotionParameters linear; // variables used in MC_MODE_LINEAR
struct ArcMotionParameters arc; // variables used in MC_MODE_ARC
uint32_t dwell_milliseconds; // variable used in MC_MODE_DWELL
};
};
struct MotionControlState state;
struct MotionControlState mc;
uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
void set_stepper_directions(int8_t *direction);
inline void step_steppers(uint8_t *enabled);
inline void step_axis(uint8_t axis);
void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
// void printCurrentPosition() {
// int axis;
// printString("[ ");
// for(axis=X_AXIS; axis<=Z_AXIS; axis++) {
// printInteger(trunc(mc.position[axis]*100));
// printByte(' ');
// }
// printString("]\n\r");
// }
//
void mc_init()
{
// Initialize state variables
memset(&state, 0, sizeof(state));
memset(&mc, 0, sizeof(mc));
}
void mc_dwell(uint32_t milliseconds)
{
state.mode = MC_MODE_DWELL;
state.dwell_milliseconds = milliseconds;
mc.mode = MC_MODE_DWELL;
mc.dwell_milliseconds = milliseconds;
}
// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
@ -105,39 +117,45 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
// Same as mc_linear_motion but accepts target in absolute step coordinates
void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
{
state.mode = MC_MODE_LINEAR;
{
mc.linear.target[X_AXIS] = x;
mc.linear.target[Y_AXIS] = y;
mc.linear.target[Z_AXIS] = z;
mc.mode = MC_MODE_LINEAR;
uint8_t axis; // loop variable
// Determine direction and travel magnitude for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
state.linear.step_count[axis] = abs(state.linear.target[axis] - state.position[axis]);
state.linear.direction[axis] = sign(state.linear.step_count[axis]);
mc.linear.step_count[axis] = abs(mc.linear.target[axis] - mc.position[axis]);
mc.linear.direction[axis] = signof(mc.linear.target[axis] - mc.position[axis]);
}
// Find the magnitude of the axis with the longest travel
state.linear.maximum_steps = max(state.linear.step_count[Z_AXIS],
max(state.linear.step_count[X_AXIS], state.linear.step_count[Y_AXIS]));
mc.linear.maximum_steps = max(mc.linear.step_count[Z_AXIS],
max(mc.linear.step_count[X_AXIS], mc.linear.step_count[Y_AXIS]));
// Nothing to do?
if ((mc.linear.maximum_steps) == 0)
{
mc.mode = MC_MODE_AT_REST;
return;
}
// Set up a neat counter for each axis
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
state.linear.counter[axis] = -state.linear.maximum_steps/2;
mc.linear.counter[axis] = -mc.linear.maximum_steps/2;
}
// Set our direction pins
set_stepper_directions(state.linear.direction);
set_stepper_directions(mc.linear.direction);
// Calculate the microseconds we need to wait between each step to achieve the desired feed rate
if (invert_feed_rate) {
state.pace =
(feed_rate*1000000)/state.linear.maximum_steps;
mc.pace =
(feed_rate*1000000)/mc.linear.maximum_steps;
} else {
// Ask old Phytagoras to estimate how many steps our next move is going to take us:
uint32_t steps_to_travel =
ceil(sqrt(pow((X_STEPS_PER_MM*state.linear.step_count[X_AXIS]),2) +
pow((Y_STEPS_PER_MM*state.linear.step_count[Y_AXIS]),2) +
pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2)));
state.pace =
((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;
ceil(sqrt(pow((X_STEPS_PER_MM*mc.linear.step_count[X_AXIS]),2) +
pow((Y_STEPS_PER_MM*mc.linear.step_count[Y_AXIS]),2) +
pow((Z_STEPS_PER_MM*mc.linear.step_count[Z_AXIS]),2)));
mc.pace =
((steps_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / mc.linear.maximum_steps;
}
}
@ -147,27 +165,27 @@ void execute_linear_motion()
uint8_t step[3];
uint8_t axis; // loop variable
// Trace the line
clear_vector(step);
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
if (state.linear.target[axis] != state.position[axis])
{
state.linear.counter[axis] += state.linear.step_count[axis];
if (state.linear.counter[axis] > 0)
{
step[axis] = true;
state.linear.counter[axis] -= state.linear.maximum_steps;
state.position[axis] += state.linear.direction[axis];
}
}
}
if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
step_steppers(step);
} else {
state.mode = MC_MODE_AT_REST;
}
while(mc.mode) {
// Trace the line
clear_vector(step);
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
if (mc.linear.target[axis] != mc.position[axis])
{
mc.linear.counter[axis] += mc.linear.step_count[axis];
if (mc.linear.counter[axis] > 0)
{
step[axis] = true;
mc.linear.counter[axis] -= mc.linear.maximum_steps;
mc.position[axis] += mc.linear.direction[axis];
}
}
}
if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
step_steppers(step);
} else {
mc.mode = MC_MODE_AT_REST;
}
}
}
// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc,
@ -176,61 +194,62 @@ void execute_linear_motion()
// ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis.
void mc_arc(double theta, double angular_travel, double radius, int axis_1, int axis_2, double feed_rate)
{
state.mode = MC_MODE_ARC;
uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
mc.mode = MC_MODE_ARC;
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
state.arc.angular_direction = sign(angular_travel);
mc.arc.angular_direction = signof(angular_travel);
// Calculate the initial position and target position in the local coordinate system of the arc
state.arc.x = round(sin(theta)*radius*X_STEPS_PER_MM);
state.arc.y = round(cos(theta)*radius*X_STEPS_PER_MM);
state.arc.target_x = trunc(sin(theta+angular_travel)*(radius*X_STEPS_PER_MM-0.5));
state.arc.target_y = trunc(cos(theta+angular_travel)*(radius*X_STEPS_PER_MM-0.5));
mc.arc.x = round(sin(theta)*radius_steps);
mc.arc.y = round(cos(theta)*radius_steps);
mc.arc.target_x = trunc(sin(theta+angular_travel)*radius_steps);
mc.arc.target_y = trunc(cos(theta+angular_travel)*radius_steps);
// Precalculate these values to optimize target detection
state.arc.target_direction_x = sign(state.arc.target_x)*state.arc.angular_direction;
state.arc.target_direction_y = sign(state.arc.target_y)*state.arc.angular_direction;
mc.arc.target_direction_x = signof(mc.arc.target_x)*mc.arc.angular_direction;
mc.arc.target_direction_y = signof(mc.arc.target_y)*mc.arc.angular_direction;
// The "error" factor is kept up to date so that it is always == (x**2+y**2-radius**2). When error
// <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.
state.arc.error = round(pow(state.arc.x,2) + pow(state.arc.y,2) - pow(radius,2));
mc.arc.error = mc.arc.x*mc.arc.x + mc.arc.y*mc.arc.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.
state.arc.x2 = 2*state.arc.x;
state.arc.y2 = 2*state.arc.y;
mc.arc.x2 = 2*mc.arc.x;
mc.arc.y2 = 2*mc.arc.y;
// Set up a vector with the steppers we are going to use tracing the plane of this arc
clear_vector(state.arc.plane_steppers);
state.arc.plane_steppers[axis_1] = 1;
state.arc.plane_steppers[axis_2] = 1;
clear_vector(mc.arc.plane_steppers);
mc.arc.plane_steppers[axis_1] = 1;
mc.arc.plane_steppers[axis_2] = 1;
// And map the local coordinate system of the arc onto the tool axes of the selected plane
state.arc.axis_x = axis_1;
state.arc.axis_y = axis_2;
mc.arc.axis_x = axis_1;
mc.arc.axis_y = axis_2;
// mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
state.pace =
mc.pace =
ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
state.arc.incomplete = true;
mc.arc.incomplete = true;
}
#define check_arc_target \
if ((state.arc.x * state.arc.target_direction_y >= \
state.arc.target_x * state.arc.target_direction_y) && \
(state.arc.y * state.arc.target_direction_x <= \
state.arc.target_y * state.arc.target_direction_x)) \
{ state.arc.incomplete = false; }
if ((mc.arc.x * mc.arc.target_direction_y >= \
mc.arc.target_x * mc.arc.target_direction_y) && \
(mc.arc.y * mc.arc.target_direction_x <= \
mc.arc.target_y * mc.arc.target_direction_x)) \
{ mc.arc.incomplete = false; }
// Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
void step_arc_along_x(int8_t dx, int8_t dy)
{
uint32_t diagonal_error;
state.arc.x+=dx;
state.arc.error += 1+state.arc.x2*dx;
state.arc.x2 += 2*dx;
diagonal_error = state.arc.error + 1 + state.arc.y2*dy;
if(abs(state.arc.error) < abs(diagonal_error)) {
state.arc.y += dy;
state.arc.y2 += 2*dy;
state.arc.error = diagonal_error;
step_steppers(state.arc.plane_steppers); // step diagonal
mc.arc.x+=dx;
mc.arc.error += 1+mc.arc.x2*dx;
mc.arc.x2 += 2*dx;
diagonal_error = mc.arc.error + 1 + mc.arc.y2*dy;
if(abs(mc.arc.error) >= abs(diagonal_error)) {
mc.arc.y += dy;
mc.arc.y2 += 2*dy;
mc.arc.error = diagonal_error;
step_steppers(mc.arc.plane_steppers); // step diagonal
} else {
step_axis(state.arc.axis_x); // step straight
step_axis(mc.arc.axis_x); // step straight
}
check_arc_target;
}
@ -239,172 +258,77 @@ void step_arc_along_x(int8_t dx, int8_t dy)
void step_arc_along_y(int8_t dx, int8_t dy)
{
uint32_t diagonal_error;
state.arc.y+=dy;
state.arc.error += 1+state.arc.y2*dy;
state.arc.y2 += 2*dy;
diagonal_error = state.arc.error + 1 + state.arc.x2*dx;
if(abs(state.arc.error) < abs(diagonal_error)) {
state.arc.x += dx;
state.arc.x2 += 2*dx;
state.arc.error = diagonal_error;
step_steppers(state.arc.plane_steppers); // step diagonal
mc.arc.y+=dy;
mc.arc.error += 1+mc.arc.y2*dy;
mc.arc.y2 += 2*dy;
diagonal_error = mc.arc.error + 1 + mc.arc.x2*dx;
if(abs(mc.arc.error) >= abs(diagonal_error)) {
mc.arc.x += dx;
mc.arc.x2 += 2*dx;
mc.arc.error = diagonal_error;
step_steppers(mc.arc.plane_steppers); // step diagonal
} else {
step_axis(state.arc.axis_y); // step straight
step_axis(mc.arc.axis_y); // step straight
}
check_arc_target;
}
// Take dx and dy which are local to the arc being generated and map them on to the
// selected tool-space-axes for the current arc.
void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
{
int8_t direction[3];
direction[state.arc.axis_x] = dx;
direction[state.arc.axis_y] = dy;
set_stepper_directions(direction);
}
/*
Quandrants of the arc
\ 7|0 /
\ | /
6 \|/ 1 y+
---------|-----------
5 /|\ 2 y-
/ | \
x- / 4|3 \ x+ */
#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
// Determine within which quadrant of the circle the provided coordinate falls
int quadrant(uint32_t x,uint32_t y)
{
// determine if the coordinate is in the quadrants 0,3,4 or 7
register int quad0347 = abs(x)<abs(y);
if (x<0) { // quad 4567
if (y<0) { // quad 45
return(quad0347 ? 4 : 5);
} else { // quad 67
return(quad0347 ? 7 : 6);
}
} else {
if (y<0) { // quad 23
return(quad0347 ? 3 : 2);
} else { // quad 01
return(quad0347 ? 0 : 1);
}
}
}
#endif
// Will trace the configured arc until the target is reached.
void execute_arc()
{
uint32_t start_x = state.arc.x;
uint32_t start_y = state.arc.y;
uint32_t start_x = mc.arc.x;
uint32_t start_y = mc.arc.y;
int dx, dy; // Trace directions
int8_t direction[3];
// state.mode is set to 0 (MC_MODE_AT_REST) when target is reached
while(state.arc.incomplete)
// mc.mode is set to 0 (MC_MODE_AT_REST) when target is reached
while(mc.arc.incomplete)
{
#ifdef UNROLLED_ARC_LOOP
// Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
int q = quadrant(state.arc.x, state.arc.y);
if (state.arc.angular_direction) {
switch (q) {
case 0:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
case 1:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(1,-1); }
case 2:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
case 5:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(-1,1); }
case 6:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
case 7:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,1); }
}
} else {
switch (q) {
case 7:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
case 6:
map_local_arc_directions_to_stepper_directions(-1,-1);
while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(-1,-1); }
case 5:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.y>state.arc.x)) { step_arc_along_y(1,-1); }
case 4:
map_local_arc_directions_to_stepper_directions(1,-1);
while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,-1); }
case 3:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }
case 2:
map_local_arc_directions_to_stepper_directions(1,1);
while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(1,1); }
case 1:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_y(-1,1); }
case 0:
map_local_arc_directions_to_stepper_directions(-1,1);
while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,1); }
}
}
#else
dx = (state.arc.y!=0) ? sign(state.arc.y) * state.arc.angular_direction : -sign(state.arc.x);
dy = (state.arc.x!=0) ? -sign(state.arc.x) * state.arc.angular_direction : -sign(state.arc.y);
if (fabs(state.arc.x)<fabs(state.arc.y)) {
dx = (mc.arc.y!=0) ? signof(mc.arc.y) * mc.arc.angular_direction : -signof(mc.arc.x);
dy = (mc.arc.x!=0) ? -signof(mc.arc.x) * mc.arc.angular_direction : -signof(mc.arc.y);
// Take dx and dy which are local to the arc being generated and map them on to the
// selected tool-space-axes for the current arc.
direction[mc.arc.axis_x] = dx;
direction[mc.arc.axis_y] = dy;
set_stepper_directions(direction);
if (abs(mc.arc.x)<abs(mc.arc.y)) {
step_arc_along_x(dx,dy);
} else {
} else {
step_arc_along_y(dx,dy);
}
#endif
}
// Update the tool position to the new actual position
state.position[state.arc.axis_x] += state.arc.x-start_x;
state.position[state.arc.axis_y] += state.arc.y-start_y;
mc.position[mc.arc.axis_x] += mc.arc.x-start_x;
mc.position[mc.arc.axis_y] += mc.arc.y-start_y;
// Because of rounding errors we might be off by a step or two. Adjust for this
// To be implemented
//void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
mc.mode = MC_MODE_AT_REST;
}
void mc_go_home()
{
state.mode = MC_MODE_HOME;
mc.mode = MC_MODE_HOME;
}
void execute_go_home()
{
st_go_home();
st_synchronize();
clear_vector(state.position); // By definition this is location [0, 0, 0]
state.mode = MC_MODE_AT_REST;
clear_vector(mc.position); // By definition this is location [0, 0, 0]
mc.mode = MC_MODE_AT_REST;
}
void mc_execute() {
st_set_pace(state.pace);
while(state.mode) {
switch(state.mode) {
st_set_pace(mc.pace);
printByte('~');
while(mc.mode) { // Loop because one task might start another task
switch(mc.mode) {
case MC_MODE_AT_REST: break;
case MC_MODE_DWELL: st_synchronize(); _delay_ms(state.dwell_milliseconds); state.mode = MC_MODE_AT_REST; break;
case MC_MODE_DWELL: st_synchronize(); _delay_ms(mc.dwell_milliseconds); mc.mode = MC_MODE_AT_REST; break;
case MC_MODE_LINEAR: execute_linear_motion(); break;
case MC_MODE_ARC: execute_arc(); break;
case MC_MODE_HOME: execute_go_home(); break;
@ -414,7 +338,7 @@ void mc_execute() {
int mc_status()
{
return(state.mode);
return(mc.mode);
}
// Set the direction pins for the stepper motors according to the provided vector.
@ -427,11 +351,10 @@ void set_stepper_directions(int8_t *direction)
way we can generate the whole direction bit-mask without doing any comparisions
or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry.
*/
direction_bits = ~(
mc.direction_bits = (
((direction[X_AXIS]&0x80)>>(7-X_DIRECTION_BIT)) |
((direction[Y_AXIS]&0x80)>>(7-Y_DIRECTION_BIT)) |
((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT))
);
((direction[Z_AXIS]&0x80)>>(7-Z_DIRECTION_BIT)));
}
// Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one
@ -439,16 +362,17 @@ void set_stepper_directions(int8_t *direction)
// 1, and the rest to 0.
inline void step_steppers(uint8_t *enabled)
{
st_buffer_step(direction_bits | enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT);
st_buffer_step(mc.direction_bits | (enabled[X_AXIS]<<X_STEP_BIT) |
(enabled[Y_AXIS]<<Y_STEP_BIT) | (enabled[Z_AXIS]<<Z_STEP_BIT));
}
// Step only one motor
inline void step_axis(uint8_t axis)
{
switch (axis) {
case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
case X_AXIS: st_buffer_step(mc.direction_bits | (1<<X_STEP_BIT)); break;
case Y_AXIS: st_buffer_step(mc.direction_bits | (1<<Y_STEP_BIT)); break;
case Z_AXIS: st_buffer_step(mc.direction_bits | (1<<Z_STEP_BIT)); break;
}
}
@ -457,3 +381,4 @@ void mc_wait()
{
st_synchronize();
}