From ac2e26fda91d116e21600dfaf769bd670a60b632 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Wed, 28 Jan 2009 23:48:21 +0100 Subject: [PATCH] added buffered stepping support and the rudiments of the arc-interpolator --- Makefile | 2 +- arc_algorithm/arc.rb | 261 ++++++++++++++++++++++++++++++++++++++++-- config.h | 10 +- main.c | 7 +- motion_control.c | 262 ++++++++++++++++++++++--------------------- motion_control.h | 7 +- stepper.c | 220 ++++++++++++++++++++++++++++++++++++ stepper.h | 56 +++++++++ todo.txt | 1 + 9 files changed, 676 insertions(+), 150 deletions(-) create mode 100644 stepper.c create mode 100644 stepper.h diff --git a/Makefile b/Makefile index 1befab1..cc53371 100644 --- a/Makefile +++ b/Makefile @@ -30,7 +30,7 @@ DEVICE = atmega168 CLOCK = 20000000 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 # Tune the lines below only if you know what you are doing: diff --git a/arc_algorithm/arc.rb b/arc_algorithm/arc.rb index 062707d..31eb975 100644 --- a/arc_algorithm/arc.rb +++ b/arc_algorithm/arc.rb @@ -15,11 +15,11 @@ class CircleTest def init @pixels = [] @tool_position = [14,14] - 30.times { @pixels << '.'*30 } + 40.times { @pixels << '.'*40 } end 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] end @@ -34,11 +34,11 @@ class CircleTest # 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) - # dP[x-1, y-1]: 2 (1 - x - 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) 1+2x+1-2y + # dP[x-1, y-1]: 2 (1 - x - y) 2-2x-2y + # 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 # 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 def arc_clean(theta, angular_travel, radius) + radius = radius x = (sin(theta)*radius).round y = (cos(theta)*radius).round angular_direction = angular_travel.sign @@ -130,8 +131,10 @@ class CircleTest plot_pixel(x+14, -y+14, "X") end - dx = (y==0) ? angular_direction : y.sign*angular_direction - dy = (x==0) ? angular_direction : -x.sign*angular_direction + dx = (y==0) ? -x.sign : y.sign*angular_direction + dy = (x==0) ? -y.sign : -x.sign*angular_direction + + pp [[x,y],[dx,dy]] if x.abs 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= 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.abs0 # 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 test = CircleTest.new 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 diff --git a/config.h b/config.h index a103e98..05ae101 100644 --- a/config.h +++ b/config.h @@ -39,19 +39,17 @@ #define STEPPERS_ENABLE_PORT PORTB #define STEPPERS_ENABLE_BIT 6 -#define STEP_DDR DDRB -#define STEP_PORT PORTB +#define MOTORS_DDR DDRB +#define MOTORS_PORT PORTB #define X_STEP_BIT 0 #define Y_STEP_BIT 2 #define Z_STEP_BIT 4 -#define STEP_MASK (1< #include +#include "stepper.h" +#include "spindle_control.h" #include "motion_control.h" #include "gcode.h" -#include "spindle_control.h" #include "serial_protocol.h" int main(void) { + st_init(); mc_init(); // initialize motion control subsystem - gc_init(); // initialize gcode-parser spindle_init(); // initialize spindle controller + gc_init(); // initialize gcode-parser sp_init(); // initialize the serial protocol - gc_execute_line("123.1"); for(;;){ sleep_mode(); sp_process(); // process the serial protocol diff --git a/motion_control.c b/motion_control.c index ad41216..447ee57 100644 --- a/motion_control.c +++ b/motion_control.c @@ -27,6 +27,7 @@ #include #include #include "nuts_bolts.h" +#include "stepper.h" // position represents the current position of the head measured in steps // target is the target for the current linear motion @@ -38,7 +39,6 @@ #define MODE_ARC 2 #define MODE_DWELL 3 #define MODE_HOME 4 -#define MODE_LIMIT_OVERRUN -1 #define PHASE_HOME_RETURN 0 #define PHASE_HOME_NUDGE 1 @@ -55,17 +55,15 @@ struct LinearMotionParameters { maximum_steps; // The larges absolute step-count of any axis }; -// Parameters when mode is MODE_LINEAR struct ArcMotionParameters { - uint32_t radius; - int16_t degrees; - int ccw; -}; - -struct HomeCycleParameters { - int8_t direction[3]; // The direction of travel along each axis (-1, 0 or 1) - int8_t phase; // current phase of the home cycle. - int8_t away[3]; // a vector of booleans. True for each axis that is still away. + int8_t angular_direction; // 1 = clockwise, -1 = anticlockwise + uint32_t circle_x, circle_y, target_x, target_y; // current position and target position in the + // 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), + // x2 is always 2*x, y2 is always 2*y + uint8_t axis_x, axis_y; // maps the circle axes to stepper axes + int32_t target[3]; // The target position in absolute steps }; /* 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 { int8_t mode; // The current operation mode 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 { struct LinearMotionParameters linear; // variables used in MODE_LINEAR 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 - int8_t limit_overrun_direction[3]; // variable used in MODE_LIMIT_OVERRUN }; }; 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 disable_steppers(); -void set_direction_pins(int8_t *direction); +void set_direction_bits(int8_t *direction); 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); void mc_init() { // Initialize state variables 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<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)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 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_y0) { step_arc_along_x(-1,1); } + } + } +} + void mc_go_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() { - 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_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; - } + st_go_home(); + clear_vector(state.position); // By definition this is location [0, 0, 0] + state.mode = MODE_AT_REST; } void mc_execute() { - enable_steppers(); + st_set_pace(state.pace); while(state.mode) { switch(state.mode) { case MODE_AT_REST: break; @@ -254,13 +303,7 @@ void mc_execute() { case MODE_LINEAR: perform_linear_motion(); 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() @@ -268,49 +311,22 @@ int mc_status() 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<>(7-X_DIRECTION_BIT)) | ((direction[Y_AXIS]&128)>>(7-Y_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 @@ -318,21 +334,15 @@ void set_direction_pins(int8_t *direction) // 1, and the rest to 0. inline void step_steppers(uint8_t *enabled) { - STEP_PORT |= enabled[X_AXIS]< 0: the system is busy, // result < 0: the system is in an error state. int mc_status(); diff --git a/stepper.c b/stepper.c new file mode 100644 index 0000000..21fe403 --- /dev/null +++ b/stepper.c @@ -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 . +*/ + +#include "stepper.h" +#include "config.h" +#include "nuts_bolts.h" +#include + +#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<> 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<. +*/ + +#ifndef stepper_h +#define stepper_h + +#include +#include + +#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 diff --git a/todo.txt b/todo.txt index d6ee5dd..e2aace7 100644 --- a/todo.txt +++ b/todo.txt @@ -1,3 +1,4 @@ +* Eliminate need for circle_x and circle_y in step_arc_along… * Use timer interrupts to drive steppers * Tool table * Tool length offsets