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

added buffered stepping support and the rudiments of the arc-interpolator

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2009-01-28 23:48:21 +01:00
parent 2207acdf2b
commit ac2e26fda9
9 changed files with 676 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -30,7 +30,7 @@
DEVICE = atmega168 DEVICE = atmega168
CLOCK = 20000000 CLOCK = 20000000
PROGRAMMER = -c avrisp2 -P usb PROGRAMMER = -c avrisp2 -P usb
OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o
FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
# Tune the lines below only if you know what you are doing: # Tune the lines below only if you know what you are doing:

261
arc_algorithm/arc.rb
View File

@ -15,11 +15,11 @@ class CircleTest
def init def init
@pixels = [] @pixels = []
@tool_position = [14,14] @tool_position = [14,14]
30.times { @pixels << '.'*30 } 40.times { @pixels << '.'*40 }
end end
def plot_pixel(x,y, c) def plot_pixel(x,y, c)
return if x<0 || y<0 || x>29 || y > 29 return if x<0 || y<0 || x>39 || y > 39
@pixels[y] = @pixels[y][0..x][0..-2]+c+@pixels[y][(x+1)..-1] @pixels[y] = @pixels[y][0..x][0..-2]+c+@pixels[y][(x+1)..-1]
end end
@ -34,11 +34,11 @@ class CircleTest
# dP[x, y+1]: 1 + 2 y # dP[x, y+1]: 1 + 2 y
# dP[x, y-1]: 1 - 2 y # dP[x, y-1]: 1 - 2 y
# dP[x+1, y+1]: 2 (1 + x + y) # dP[x+1, y+1]: 2 (1 + x + y) 1+2x+1+2y
# dP[x+1, y-1]: 2 (1 + x - y) # dP[x+1, y-1]: 2 (1 + x - y) 1+2x+1-2y
# dP[x-1, y-1]: 2 (1 - x - y) # dP[x-1, y-1]: 2 (1 - x - y) 2-2x-2y
# dP[x-1, y+1]: 2 (1 - x + y) # dP[x-1, y+1]: 2 (1 - x + y) 2-2x+2x
# dP[x+a, y+b]: |dx| - 2*dx*x + |dy| + 2*dy*y # dP[x+a, y+b]: |dx| - 2*dx*x + |dy| + 2*dy*y
# Algorithm from the wikipedia aricle on the Midpoint circle algorithm. # Algorithm from the wikipedia aricle on the Midpoint circle algorithm.
@ -111,6 +111,7 @@ class CircleTest
# A DDA-direct search circle interpolator. Optimal and impure # A DDA-direct search circle interpolator. Optimal and impure
def arc_clean(theta, angular_travel, radius) def arc_clean(theta, angular_travel, radius)
radius = radius
x = (sin(theta)*radius).round x = (sin(theta)*radius).round
y = (cos(theta)*radius).round y = (cos(theta)*radius).round
angular_direction = angular_travel.sign angular_direction = angular_travel.sign
@ -130,8 +131,10 @@ class CircleTest
plot_pixel(x+14, -y+14, "X") plot_pixel(x+14, -y+14, "X")
end end
dx = (y==0) ? angular_direction : y.sign*angular_direction dx = (y==0) ? -x.sign : y.sign*angular_direction
dy = (x==0) ? angular_direction : -x.sign*angular_direction dy = (x==0) ? -y.sign : -x.sign*angular_direction
pp [[x,y],[dx,dy]]
if x.abs<y.abs if x.abs<y.abs
f_straight = f + 1+2*x*dx f_straight = f + 1+2*x*dx
@ -173,12 +176,250 @@ class CircleTest
puts "Diameter: #{max_x-min_x}" puts "Diameter: #{max_x-min_x}"
end end
# A DDA-direct search circle interpolator. Optimal and impure
def arc_supaoptimal(theta, angular_travel, radius)
radius = radius
x = (sin(theta)*radius).round
y = (cos(theta)*radius).round
angular_direction = angular_travel.sign
tx = (sin(theta+angular_travel)*(radius-0.5)).floor
ty = (cos(theta+angular_travel)*(radius-0.5)).floor
f = (x**2 + y**2 - radius**2).round
x2 = 2*x
y2 = 2*y
dx = (y==0) ? -x.sign : y.sign*angular_direction
dy = (x==0) ? -y.sign : -x.sign*angular_direction
max_steps = (angular_travel.abs*radius*2).floor
# dP[x+1,y]: 1 + 2 x
# dP[x, y+1]: 1 + 2 y
max_steps.times do |i|
if i > 0
plot_pixel(x+20, -y+20, "012"[i%3].chr)
else
plot_pixel(x+20, -y+20, "X")
end
raise "x2 out of range" unless x2 == 2*x
raise "y2 out of range" unless y2 == 2*y
f_should_be = (x**2+y**2-radius**2)
if f.round != f_should_be.round
show
raise "f out of range. Is #{f}, should be #{f_should_be}"
end
if x.abs<y.abs
x += dx
f += 1+x2*dx
x2 += 2*dx
f_diagonal = f + 1 + y2*dy
if (f.abs >= f_diagonal.abs)
y += dy
dx = y.sign*angular_direction unless y == 0
y2 += 2*dy
f = f_diagonal
end
dy = -x.sign*angular_direction unless x == 0
else
y += dy
f += 1+y2*dy
y2 += 2*dy
f_diagonal = f + 1 + x2*dx
if (f.abs >= f_diagonal.abs)
x += dx
dy = -x.sign*angular_direction unless x == 0
x2 += 2*dx
f = f_diagonal
end
dx = y.sign*angular_direction unless y == 0
end
break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
end
plot_pixel(tx+20, -ty+20, "o")
return {:tx => tx, :ty => ty, :x => x, :y => y}
end
# A DDA-direct search circle interpolator unrolled for each octant. Optimal and impure
def arc_unrolled(theta, angular_travel, radius)
radius = radius
x = (sin(theta)*radius).round
y = (cos(theta)*radius).round
angular_direction = angular_travel.sign
tx = (sin(theta+angular_travel)*(radius-0.5)).floor
ty = (cos(theta+angular_travel)*(radius-0.5)).floor
f = (x**2 + y**2 - radius**2).round
x2 = 2*x
y2 = 2*y
dx = (y==0) ? -x.sign : y.sign*angular_direction
dy = (x==0) ? -y.sign : -x.sign*angular_direction
max_steps = (angular_travel.abs*radius*2).floor
# Quandrants of the circls
# \ 1|2 /
# 8\ | / 3
# \|/
# ---------|-----------
# 7 /|\ 4
# / | \
# / 6 | 5 \
#
#
#
if angular_direction>0 # clockwise
if x.abs<y.abs # quad 1,2,6,5
if y>0 # quad 1,2
while x<0 # quad 1 x+,y+
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 2, x+, y-
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
if y<=0 # quad 6, 5
while x<0 # quad 6 x-, y+
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 5 x-, y-
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
# Quandrants of the circls
# \ 1|2 /
# 8\ | / 3
# \|/
# ---------|-----------
# 7 /|\ 4
# / | \
# / 6 | 5 \
else 3 # quad 3,4,7,8
if x>0 # quad 3,4
while y>0 # quad 3 x+,y+
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 2, x+, y-
x += 1
f += 1+x2
x2 += 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
if y<=0 # quad 6, 5
while x<0 # quad 6 x-, y+
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 + y2
if (f.abs >= f_diagonal.abs)
y += 1
y2 += 2
f = f_diagonal
end
end
while x>=0 # quad 5 x-, y-
x -= 1
f += 1-x2
x2 -= 2
f_diagonal = f + 1 - y2
if (f.abs >= f_diagonal.abs)
y -= 1
y2 -= 2
f = f_diagonal
end
end
end
else
y += dy
f += 1+y2*dy
y2 += 2*dy
f_diagonal = f + 1 + x2*dx
if (f.abs >= f_diagonal.abs)
x += dx
dy = -x.sign*angular_direction unless x == 0
x2 += 2*dx
f = f_diagonal
end
dx = y.sign*angular_direction unless y == 0
end
break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
end
plot_pixel(tx+20, -ty+20, "o")
return {:tx => tx, :ty => ty, :x => x, :y => y}
end
end end
test = CircleTest.new test = CircleTest.new
test.init test.init
test.arc_clean(0, -Math::PI, 5) #test.arc_clean(0, Math::PI*2, 5)
(1..10000).each do |r|
test.init
data = test.arc_supaoptimal(2.9, Math::PI*1, r)
if (data[:tx]-data[:x]).abs > 1 || (data[:ty]-data[:y]).abs > 1
puts "r=#{r} fails target control"
pp data
puts
end
end
# test.init
# data = test.arc_supaoptimal(1.1, -Math::PI, 19)
# pp data
#test.pure_arc(0,1,1,4)
test.show test.show

10
config.h
View File

@ -39,19 +39,17 @@
#define STEPPERS_ENABLE_PORT PORTB #define STEPPERS_ENABLE_PORT PORTB
#define STEPPERS_ENABLE_BIT 6 #define STEPPERS_ENABLE_BIT 6
#define STEP_DDR DDRB #define MOTORS_DDR DDRB
#define STEP_PORT PORTB #define MOTORS_PORT PORTB
#define X_STEP_BIT 0 #define X_STEP_BIT 0
#define Y_STEP_BIT 2 #define Y_STEP_BIT 2
#define Z_STEP_BIT 4 #define Z_STEP_BIT 4
#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
#define DIRECTION_DDR DDRB
#define DIRECTION_PORT PORTB
#define X_DIRECTION_BIT 1 #define X_DIRECTION_BIT 1
#define Y_DIRECTION_BIT 3 #define Y_DIRECTION_BIT 3
#define Z_DIRECTION_BIT 5 #define Z_DIRECTION_BIT 5
#define STEP_MASK (1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)
#define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT) #define DIRECTION_MASK (1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)
#define MOTORS_MASK STEP_MASK | DIRECTION_MASK
#define LIMIT_DDR DDRC #define LIMIT_DDR DDRC
#define LIMIT_PORT PORTC #define LIMIT_PORT PORTC

7
main.c
View File

@ -20,19 +20,20 @@
#include <avr/io.h> #include <avr/io.h>
#include <avr/sleep.h> #include <avr/sleep.h>
#include "stepper.h"
#include "spindle_control.h"
#include "motion_control.h" #include "motion_control.h"
#include "gcode.h" #include "gcode.h"
#include "spindle_control.h"
#include "serial_protocol.h" #include "serial_protocol.h"
int main(void) int main(void)
{ {
st_init();
mc_init(); // initialize motion control subsystem mc_init(); // initialize motion control subsystem
gc_init(); // initialize gcode-parser
spindle_init(); // initialize spindle controller spindle_init(); // initialize spindle controller
gc_init(); // initialize gcode-parser
sp_init(); // initialize the serial protocol sp_init(); // initialize the serial protocol
gc_execute_line("123.1");
for(;;){ for(;;){
sleep_mode(); sleep_mode();
sp_process(); // process the serial protocol sp_process(); // process the serial protocol

262
motion_control.c
View File

@ -27,6 +27,7 @@
#include <math.h> #include <math.h>
#include <stdlib.h> #include <stdlib.h>
#include "nuts_bolts.h" #include "nuts_bolts.h"
#include "stepper.h"
// position represents the current position of the head measured in steps // position represents the current position of the head measured in steps
// target is the target for the current linear motion // target is the target for the current linear motion
@ -38,7 +39,6 @@
#define MODE_ARC 2 #define MODE_ARC 2
#define MODE_DWELL 3 #define MODE_DWELL 3
#define MODE_HOME 4 #define MODE_HOME 4
#define MODE_LIMIT_OVERRUN -1
#define PHASE_HOME_RETURN 0 #define PHASE_HOME_RETURN 0
#define PHASE_HOME_NUDGE 1 #define PHASE_HOME_NUDGE 1
@ -55,17 +55,15 @@ struct LinearMotionParameters {
maximum_steps; // The larges absolute step-count of any axis maximum_steps; // The larges absolute step-count of any axis
}; };
// Parameters when mode is MODE_LINEAR
struct ArcMotionParameters { struct ArcMotionParameters {
uint32_t radius; int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise
int16_t degrees; uint32_t circle_x, circle_y, target_x, target_y; // current position and target position in the
int ccw; // local coordinate system of the circle where [0,0] is the
}; // center of the circle.
int32_t error, x2, y2; // error is always == (circle_x**2 + circle_y**2 - radius**2),
struct HomeCycleParameters { // x2 is always 2*x, y2 is always 2*y
int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1) uint8_t axis_x, axis_y; // maps the circle axes to stepper axes
int8_t phase; // current phase of the home cycle. int32_t target[3]; // The target position in absolute steps
int8_t away[3]; // a vector of booleans. True for each axis that is still away.
}; };
/* The whole state of the motion-control-system in one struct. Makes the code a little bit hard to /* The whole state of the motion-control-system in one struct. Makes the code a little bit hard to
@ -75,57 +73,39 @@ struct HomeCycleParameters {
struct MotionControlState { struct MotionControlState {
int8_t mode; // The current operation mode int8_t mode; // The current operation mode
int32_t position[3]; // The current position of the tool in absolute steps int32_t position[3]; // The current position of the tool in absolute steps
int32_t update_delay_us; // Microseconds between each update in the current mode int32_t pace; // Microseconds between each update in the current mode
union { union {
struct LinearMotionParameters linear; // variables used in MODE_LINEAR struct LinearMotionParameters linear; // variables used in MODE_LINEAR
struct ArcMotionParameters arc; // variables used in MODE_ARC struct ArcMotionParameters arc; // variables used in MODE_ARC
struct HomeCycleParameters home; // variables used in MODE_HOME
uint32_t dwell_milliseconds; // variable used in MODE_DWELL uint32_t dwell_milliseconds; // variable used in MODE_DWELL
int8_t limit_overrun_direction[3]; // variable used in MODE_LIMIT_OVERRUN
}; };
}; };
struct MotionControlState state; struct MotionControlState state;
int check_limit_switches(); uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction
void enable_steppers(); void enable_steppers();
void disable_steppers(); void disable_steppers();
void set_direction_pins(int8_t *direction); void set_direction_bits(int8_t *direction);
inline void step_steppers(uint8_t *enabled); inline void step_steppers(uint8_t *enabled);
void limit_overrun(uint8_t *direction);
int check_limit_switch(int axis);
inline void step_axis(uint8_t axis); inline void step_axis(uint8_t axis);
void mc_init() void mc_init()
{ {
// Initialize state variables // Initialize state variables
memset(&state, 0, sizeof(state)); memset(&state, 0, sizeof(state));
// Configure directions of interface pins
STEP_DDR |= STEP_MASK;
DIRECTION_DDR |= DIRECTION_MASK;
LIMIT_DDR &= ~(LIMIT_MASK);
STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
disable_steppers();
}
void limit_overrun(uint8_t *direction)
{
state.mode = MODE_LIMIT_OVERRUN;
memcpy(state.limit_overrun_direction, direction, sizeof(state.limit_overrun_direction));
} }
void mc_dwell(uint32_t milliseconds) void mc_dwell(uint32_t milliseconds)
{ {
mc_wait(); st_synchronize();
state.mode = MODE_DWELL; state.mode = MODE_DWELL;
state.dwell_milliseconds = milliseconds; state.dwell_milliseconds = milliseconds;
state.update_delay_us = 1000; state.pace = 1000;
} }
void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate) void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate)
{ {
mc_wait();
state.mode = MODE_LINEAR; state.mode = MODE_LINEAR;
state.linear.target[X_AXIS] = trunc(x*X_STEPS_PER_MM); state.linear.target[X_AXIS] = trunc(x*X_STEPS_PER_MM);
@ -149,19 +129,19 @@ void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_
} }
// Set our direction pins // Set our direction pins
set_direction_pins(state.linear.direction); set_direction_bits(state.linear.direction);
// Calculate the microseconds we need to wait between each step to achieve the desired feed rate // Calculate the microseconds we need to wait between each step to achieve the desired feed rate
if (invert_feed_rate) { if (invert_feed_rate) {
state.update_delay_us = state.pace =
(feed_rate*1000000.0)/state.linear.maximum_steps; (feed_rate*1000000)/state.linear.maximum_steps;
} else { } else {
// Ask old Phytagoras how many millimeters our next move is going to take us: // Ask old Phytagoras how many millimeters our next move is going to take us:
float millimeters_of_travel = float millimeters_of_travel =
sqrt(pow((X_STEPS_PER_MM*state.linear.step_count[X_AXIS]),2) + 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((Y_STEPS_PER_MM*state.linear.step_count[Y_AXIS]),2) +
pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2)); pow((Z_STEPS_PER_MM*state.linear.step_count[Z_AXIS]),2));
state.update_delay_us = state.pace =
((millimeters_of_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps; ((millimeters_of_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / state.linear.maximum_steps;
} }
} }
@ -190,63 +170,132 @@ void perform_linear_motion()
if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) { if (step[X_AXIS] | step[Y_AXIS] | step[Z_AXIS]) {
step_steppers(step); step_steppers(step);
// If we trip any limit switch while moving: Abort, abort!
if (check_limit_switches()) {
limit_overrun(state.linear.direction);
}
_delay_us(state.update_delay_us);
} else { } else {
state.mode = MODE_AT_REST; state.mode = MODE_AT_REST;
} }
} }
void mc_arc(double theta, double angular_travel, double radius, uint32_t *target)
{
state.mode = MODE_ARC;
// Calculate the initial position and target position in the local coordinate system of the circle
state.arc.circle_x = round(sin(theta)*radius);
state.arc.circle_y = round(cos(theta)*radius);
state.arc.target_x = trunc(sin(theta+angular_travel)*(radius-0.5));
state.arc.target_y = trunc(cos(theta+angular_travel)*(radius-0.5));
// Determine angular direction (+1 = clockwise, -1 = counterclockwise)
state.arc.angular_direction = sign(angular_travel);
// 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 circle, when it is >0 we are outside of the circle, and when it is 0 we
// are exactly on top of the circle.
state.arc.error = round(pow(state.arc.circle_x,2) + pow(state.arc.circle_y,2) - pow(radius,2));
// 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 circle coordinates at all times.
state.arc.x2 = 2*state.arc.circle_x;
state.arc.y2 = 2*state.arc.circle_y;
}
void step_arc_along_x(dx,dy)
{
uint32_t diagonal_error;
state.arc.circle_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.circle_y += dy;
state.arc.y2 += 2*dy;
state.arc.error = diagonal_error;
};
}
void step_arc_along_y(dx,dy)
{
uint32_t diagonal_error;
state.arc.circle_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.circle_x += dx;
state.arc.x2 += 2*dx;
state.arc.error = diagonal_error;
}
}
/*
Quandrants of the circle
\ 7|0 /
\ | /
6 \|/ 1 y+
---------|-----------
5 /|\ 2 y-
/ | \
x- / 4|3 \ x+ */
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);
}
}
}
void perform_arc()
{
int q = quadrant(state.arc.circle_x, state.arc.circle_y);
if (state.arc.angular_direction) {
switch (q) {
case 0: while(state.arc.circle_x>state.arc.circle_y) { step_arc_along_x(1,-1); }
case 1: while(state.arc.circle_y>0) { step_arc_along_y(1,-1); }
case 2: while(state.arc.circle_y>-state.arc.circle_x) { step_arc_along_y(-1,-1); }
case 3: while(state.arc.circle_x>0) { step_arc_along_x(-1,-1); }
case 4: while(state.arc.circle_y<state.arc.circle_x) { step_arc_along_x(-1,1); }
case 5: while(state.arc.circle_y<0) { step_arc_along_y(-1,1); }
case 6: while(state.arc.circle_y<-state.arc.circle_x) { step_arc_along_y(1,1); }
case 7: while(state.arc.circle_x<0) { step_arc_along_x(1,1); }
}
} else {
switch (q) {
case 7: while(state.arc.circle_y>-state.arc.circle_x) { step_arc_along_x(-1,-1); }
case 6: while(state.arc.circle_y>0) { step_arc_along_y(-1,-1); }
case 5: while(state.arc.circle_y>state.arc.circle_x) { step_arc_along_y(1,-1); }
case 4: while(state.arc.circle_x<0) { step_arc_along_x(1,-1); }
case 3: while(state.arc.circle_y<-state.arc.circle_x) { step_arc_along_x(1,1); }
case 2: while(state.arc.circle_y<0) { step_arc_along_y(1,1); }
case 1: while(state.arc.circle_y<state.arc.circle_x) { step_arc_along_y(-1,1); }
case 0: while(state.arc.circle_x>0) { step_arc_along_x(-1,1); }
}
}
}
void mc_go_home() void mc_go_home()
{ {
state.mode = MODE_HOME; state.mode = MODE_HOME;
memset(state.home.direction, -1, sizeof(state.home.direction)); // direction = [-1,-1,-1]
set_direction_pins(state.home.direction);
clear_vector(state.home.away);
} }
void perform_go_home() void perform_go_home()
{ {
int axis; st_go_home();
if(state.home.phase == PHASE_HOME_RETURN) { clear_vector(state.position); // By definition this is location [0, 0, 0]
// We are running all axes in reverse until all limit switches are tripped state.mode = MODE_AT_REST;
// Check all limit switches:
for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
state.home.away[axis] |= check_limit_switch(axis);
}
// Step steppers. First retract along Z-axis. Then X and Y.
if(state.home.away[Z_AXIS]) {
step_axis(Z_AXIS);
} else {
// Check if all axes are home
if(!(state.home.away[X_AXIS] || state.home.away[Y_AXIS])) {
// All axes are home, prepare next phase: to nudge the tool carefully out of the limit switches
memset(state.home.direction, 1, sizeof(state.home.direction)); // direction = [1,1,1]
set_direction_pins(state.home.direction);
state.home.phase == PHASE_HOME_NUDGE;
return;
}
step_steppers(state.home.away);
}
} else {
for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
if(check_limit_switch(axis)) {
step_axis(axis);
return;
}
}
// When this code is reached it means all axes are free of their limit-switches. Complete the cycle and rest:
clear_vector(state.position); // By definition this is location [0, 0, 0]
state.mode = MODE_AT_REST;
}
} }
void mc_execute() { void mc_execute() {
enable_steppers(); st_set_pace(state.pace);
while(state.mode) { while(state.mode) {
switch(state.mode) { switch(state.mode) {
case MODE_AT_REST: break; case MODE_AT_REST: break;
@ -254,13 +303,7 @@ void mc_execute() {
case MODE_LINEAR: perform_linear_motion(); case MODE_LINEAR: perform_linear_motion();
case MODE_HOME: perform_go_home(); case MODE_HOME: perform_go_home();
} }
_delay_us(state.update_delay_us);
} }
disable_steppers();
}
void mc_wait() {
return; // No concurrency support yet. So waiting for all to pass is moot.
} }
int mc_status() int mc_status()
@ -268,49 +311,22 @@ int mc_status()
return(state.mode); return(state.mode);
} }
int check_limit_switches()
{
// Dual read as crude debounce
return((LIMIT_PORT & LIMIT_MASK) | (LIMIT_PORT & LIMIT_MASK));
}
int check_limit_switch(int axis)
{
uint8_t mask = 0;
switch (axis) {
case X_AXIS: mask = 1<<X_LIMIT_BIT; break;
case Y_AXIS: mask = 1<<Y_LIMIT_BIT; break;
case Z_AXIS: mask = 1<<Z_LIMIT_BIT; break;
}
return((LIMIT_PORT&mask) || (LIMIT_PORT&mask));
}
void enable_steppers()
{
STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
}
void disable_steppers()
{
STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
}
// Set the direction pins for the stepper motors according to the provided vector. // Set the direction pins for the stepper motors according to the provided vector.
// direction is an array of three 8 bit integers representing the direction of // 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 -1 (reverse), 0 or 1 (forward).
void set_direction_pins(int8_t *direction) void set_direction_bits(int8_t *direction)
{ {
/* Sorry about this convoluted code! It uses the fact that bit 7 of each direction /* Sorry about this convoluted code! It uses the fact that bit 7 of each direction
int is set when the direction == -1, but is 0 when direction is forward. This int is set when the direction == -1, but is 0 when direction is forward. This
way we can generate the whole direction bit-mask without doing any comparisions way we can generate the whole direction bit-mask without doing any comparisions
or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry. or branching. Fast and compact, yet practically unreadable. Sorry sorry sorry.
*/ */
uint8_t forward_bits = ~( direction_bits = ~(
((direction[X_AXIS]&128)>>(7-X_DIRECTION_BIT)) | ((direction[X_AXIS]&128)>>(7-X_DIRECTION_BIT)) |
((direction[Y_AXIS]&128)>>(7-Y_DIRECTION_BIT)) | ((direction[Y_AXIS]&128)>>(7-Y_DIRECTION_BIT)) |
((direction[Z_AXIS]&128)>>(7-Z_DIRECTION_BIT)) ((direction[Z_AXIS]&128)>>(7-Z_DIRECTION_BIT))
); );
DIRECTION_PORT = DIRECTION_PORT & ~(DIRECTION_MASK) | forward_bits;
} }
// Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one // Step enabled steppers. Enabled should be an array of three bytes. Each byte represent one
@ -318,21 +334,15 @@ void set_direction_pins(int8_t *direction)
// 1, and the rest to 0. // 1, and the rest to 0.
inline void step_steppers(uint8_t *enabled) inline void step_steppers(uint8_t *enabled)
{ {
STEP_PORT |= enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT; st_buffer_step(direction_bits | enabled[X_AXIS]<<X_STEP_BIT | enabled[Y_AXIS]<<Y_STEP_BIT | enabled[Z_AXIS]<<Z_STEP_BIT);
_delay_us(5);
STEP_PORT &= ~STEP_MASK;
} }
// Step only one motor // Step only one motor
inline void step_axis(uint8_t axis) inline void step_axis(uint8_t axis)
{ {
uint8_t mask = 0;
switch (axis) { switch (axis) {
case X_AXIS: mask = 1<<X_STEP_BIT; break; case X_AXIS: st_buffer_step(direction_bits | (1<<X_STEP_BIT)); break;
case Y_AXIS: mask = 1<<Y_STEP_BIT; break; case Y_AXIS: st_buffer_step(direction_bits | (1<<Y_STEP_BIT)); break;
case Z_AXIS: mask = 1<<Z_STEP_BIT; break; case Z_AXIS: st_buffer_step(direction_bits | (1<<Z_STEP_BIT)); break;
} }
STEP_PORT &= mask;
_delay_us(5);
STEP_PORT &= ~STEP_MASK;
} }

7
motion_control.h
View File

@ -31,17 +31,16 @@ void mc_init();
// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second // Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement. // unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement.
void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate); void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate);
// Prepare linear motion relative to the current position. // Prepare linear motion relative to the current position.
void mc_dwell(uint32_t milliseconds); void mc_dwell(uint32_t milliseconds);
// Prepare to send the tool position home // Prepare to send the tool position home
void mc_go_home(); void mc_go_home();
// Start the prepared operation. // Start the prepared operation.
void mc_execute(); void mc_execute();
// Block until the motion control system is idle
void mc_wait();
// Check motion control status. result == 0: the system is idle. result > 0: the system is busy, // Check motion control status. result == 0: the system is idle. result > 0: the system is busy,
// result < 0: the system is in an error state. // result < 0: the system is in an error state.
int mc_status(); int mc_status();

220
stepper.c Normal file
View File

@ -0,0 +1,220 @@
/*
stepper.c - stepper motor interface
Part of Grbl
Copyright (c) 2009 Simen Svale Skogsrud
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 "stepper.h"
#include "config.h"
#include "nuts_bolts.h"
#include <avr/interrupt.h>
#define TICKS_PER_MICROSECOND F_CPU/1000000
#define STEP_BUFFER_SIZE 100
volatile uint8_t step_buffer[STEP_BUFFER_SIZE]; // A buffer for step instructions
volatile int step_buffer_head = 0;
volatile int step_buffer_tail = 0;
uint8_t state = STEPPER_STATE_STOPPED;
// This timer interrupt is executed at the pace set with set_pace. It pops one instruction from
// the step_buffer, executes it. Then it starts timer2 in order to reset the motor port after
// five microseconds.
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
if (step_buffer_head != step_buffer_tail) {
// Set the stepper port according to the instructions
MOTORS_PORT = (MOTORS_PORT & ~MOTORS_MASK) | step_buffer[step_buffer_tail];
// Reset and start timer 2 which will reset the motor port after 5 microsecond
TCNT2 = 0; // reset counter
OCR2A = 5*TICKS_PER_MICROSECOND; // set the time
TIMSK2 |= OCIE2A; // enable interrupt
// move the step buffer tail to the next instruction
step_buffer_tail = (step_buffer_tail + 1) % STEP_BUFFER_SIZE;
}
}
// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
// the motor port after a short period (5us) completing one step cycle.
SIGNAL(SIG_OUTPUT_COMPARE2A)
{
MOTORS_PORT = MOTORS_PORT & ~MOTORS_MASK; // reset stepper pins
TIMSK2 &= ~OCIE2A; // disable this timer interrupt until next time
}
// Initialize and start the stepper motor subsystem
void st_init()
{
// Configure directions of interface pins
MOTORS_DDR |= MOTORS_MASK;
LIMIT_DDR &= ~(LIMIT_MASK);
STEPPERS_ENABLE_DDR |= 1<<STEPPERS_ENABLE_BIT;
// waveform generation = 0100 = CTC
TCCR1B &= ~(1<<WGM13);
TCCR1B |= (1<<WGM12);
TCCR1A &= ~(1<<WGM11);
TCCR1A &= ~(1<<WGM10);
// output mode = 00 (disconnected)
TCCR1A &= ~(3<<COM1A0);
TCCR1A &= ~(3<<COM1B0);
// Configure Timer 2
TCCR2A = 0; // Normal operation
TCCT2B = 1<<CS20; // Full speed, no prescaler
TIMSK2 = 0; // All interrupts disabled
// start off with a slow pace
st_set_pace(1000000);
st_start();
}
void st_buffer_step(uint8_t motor_port_bits)
{
int i = (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(); }
step_buffer[step_buffer_head] = motor_port_bits;
step_buffer_head = i;
}
// Block until all buffered steps are executed
void st_synchronize()
{
if (state == STEPPER_MODE_RUNNING) {
while(step_buffer_tail != step_buffer_head) { sleep_mode(); }
} else {
st_flush();
}
}
// Cancel all pending steps
void st_flush()
{
step_buffer_tail = step_buffer_head;
}
// Start the stepper subsystem
void st_start()
{
// Enable timer interrupt
TIMSK1 |= (1<<OCIE1A);
STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
state = STEPPER_STATE_RUNNING;
}
// Execute all buffered steps, then stop the stepper subsystem
inline void st_stop()
{
st_synchronize();
TIMSK1 &= ~(1<<OCIE1A);
STEPPERS_ENABLE_PORT &= ~(1<<STEPPERS_ENABLE_BIT);
state = STEPPER_STATE_STOPPED;
}
void st_set_pace(uint32_t microseconds)
{
uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;
uint16_t ceiling;
uint16_t prescaler;
if (ticks <= 65535L) {
ceiling = ticks;
prescaler = 0; // prescaler: 0
} else if (ticks <= 0x7ffffL) {
ceiling = ticks >> 3;
prescaler = 1; // prescaler: 8
} else if (ticks <= 0x3fffffL) {
ceiling = ticks >> 6;
prescaler = 2; // prescaler: 64
} else if (ticks <= 0xffffffL) {
ceiling = (ticks >> 8);
prescaler = 3; // prescaler: 256
} else if (ticks <= 0x3ffffffL) {
ceiling = (ticks >> 10);
prescaler = 4; // prescaler: 1024
} else {
ceiling = 0xffff;
prescaler = 4;
}
// Set prescaler
TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
// Set ceiling
OCR1A = ceiling;
}
int check_limit_switches()
{
// Dual read as crude debounce
return((LIMIT_PORT & LIMIT_MASK) | (LIMIT_PORT & LIMIT_MASK));
}
int check_limit_switch(int axis)
{
uint8_t mask = 0;
switch (axis) {
case X_AXIS: mask = 1<<X_LIMIT_BIT; break;
case Y_AXIS: mask = 1<<Y_LIMIT_BIT; break;
case Z_AXIS: mask = 1<<Z_LIMIT_BIT; break;
}
return((LIMIT_PORT&mask) || (LIMIT_PORT&mask));
}
// void perform_go_home()
// {
// int axis;
// if(state.home.phase == PHASE_HOME_RETURN) {
// // We are running all axes in reverse until all limit switches are tripped
// // Check all limit switches:
// for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
// state.home.away[axis] |= check_limit_switch(axis);
// }
// // Step steppers. First retract along Z-axis. Then X and Y.
// if(state.home.away[Z_AXIS]) {
// step_axis(Z_AXIS);
// } else {
// // Check if all axes are home
// if(!(state.home.away[X_AXIS] || state.home.away[Y_AXIS])) {
// // All axes are home, prepare next phase: to nudge the tool carefully out of the limit switches
// memset(state.home.direction, 1, sizeof(state.home.direction)); // direction = [1,1,1]
// set_direction_bits(state.home.direction);
// state.home.phase == PHASE_HOME_NUDGE;
// return;
// }
// step_steppers(state.home.away);
// }
// } else {
// for(axis=X_AXIS; axis <= Z_AXIS; axis++) {
// if(check_limit_switch(axis)) {
// step_axis(axis);
// return;
// }
// }
// // When this code is reached it means all axes are free of their limit-switches. Complete the cycle and rest:
// clear_vector(state.position); // By definition this is location [0, 0, 0]
// state.mode = MODE_AT_REST;
// }
// }
void st_go_home()
{
// Todo: Perform the homing cycle
}

56
stepper.h Normal file
View File

@ -0,0 +1,56 @@
/*
stepper.h - stepper motor interface
Part of Grbl
Copyright (c) 2009 Simen Svale Skogsrud
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 stepper_h
#define stepper_h
#include <avr/io.h>
#include <avr/sleep.h>
#define STEPPER_STATE_STOPPED 0
#define STEPPER_STATE_RUNNING 1
#define STEPPER_STATE_LIMIT_OVERRUN 2
#define STEPPER_STATE_HOMING 3
// Initialize and start the stepper motor subsystem
void st_init();
// Set the rate steps are taken from the buffer and executed
void st_set_pace(uint32_t microseconds);
// Buffer a new instruction for the steppers
void st_buffer_step(uint8_t motor_port_bits);
// Block until all buffered steps are executed
void st_synchronize();
// Cancel all pending steps
void st_flush();
// Start the stepper subsystem
void st_start();
// Execute all buffered steps, then stop the stepper subsystem
inline void st_stop();
// Execute the homing cycle
void st_go_home();
#endif

1
todo.txt
View File

@ -1,3 +1,4 @@
* Eliminate need for circle_x and circle_y in step_arc_along…
* Use timer interrupts to drive steppers * Use timer interrupts to drive steppers
* Tool table * Tool table
* Tool length offsets * Tool length offsets