Permalink
Browse files

Add 5th and 6th axes. This will work with Sanguino (atmega644p) only,…

… because it has additional PORTA.

Now DIR and STEP pins split between different ports (see config.h for detals).


Signed-off-by: Alexander Danilov <alexander.a.danilov@gmail.com>
  • Loading branch information...
1 parent eb649d3 commit 9fc73ef95fdaf73724cd772d84ccbbfeb5584f41 @daapp committed Jan 9, 2013
Showing with 199 additions and 52 deletions.
  1. +13 −5 config.h
  2. +9 −5 gcode.c
  3. +1 −1 gcode.h
  4. +31 −9 limits.c
  5. +12 −10 motion_control.c
  6. +1 −1 motion_control.h
  7. +4 −2 nuts_bolts.c
  8. +3 −1 nuts_bolts.h
  9. +22 −8 planner.c
  10. +3 −3 planner.h
  11. +3 −1 report.c
  12. +3 −1 settings.c
  13. +94 −5 stepper.c
View
18 config.h
@@ -34,14 +34,18 @@
#define Y_STEP_BIT 1 // Uno Digital Pin 3
#define Z_STEP_BIT 2 // Uno Digital Pin 4
#define A_STEP_BIT 3
+#define B_STEP_BIT 4
+#define C_STEP_BIT 5
#define DIRECTION_DDR DDRD
#define DIRECTION_PORT PORTD
#define X_DIRECTION_BIT 2 // Uno Digital Pin 5
#define Y_DIRECTION_BIT 3 // Uno Digital Pin 6
#define Z_DIRECTION_BIT 4 // Uno Digital Pin 7
#define A_DIRECTION_BIT 5
-#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)|(1<<A_STEP_BIT)) // All step bits
-#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)|(1<<A_DIRECTION_BIT)) // All direction bits
+#define B_DIRECTION_BIT 6
+#define C_DIRECTION_BIT 7
+#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)|(1<<A_STEP_BIT)|(1<<B_STEP_BIT)|(1<<C_STEP_BIT)) // All step bits
+#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)|(1<<A_DIRECTION_BIT)|(1<<B_DIRECTION_BIT)|(1<<C_DIRECTION_BIT)) // All direction bits
#ifdef DIRECTION_DDR
#define STEPPING_MASK STEP_MASK
#else
@@ -61,10 +65,12 @@
#define Y_LIMIT_BIT 1 // Uno Digital Pin 10
#define Z_LIMIT_BIT 2 // Uno Digital Pin 11
#define A_LIMIT_BIT 3
+#define B_LIMIT_BIT 4
+#define C_LIMIT_BIT 5
#define LIMIT_INT PCIE0 // Pin change interrupt enable pin
#define LIMIT_INT_vect PCINT0_vect
#define LIMIT_PCMSK PCMSK0 // Pin change interrupt register
-#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)|(1<<A_LIMIT_BIT)) // All limit bits
+#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)|(1<<A_LIMIT_BIT)|(1<<B_LIMIT_BIT)|(1<<C_LIMIT_BIT)) // All limit bits
#define SPINDLE_ENABLE_DDR DDRC
#define SPINDLE_ENABLE_PORT PORTC
@@ -106,14 +112,16 @@
#define DEFAULT_X_STEPS_PER_MM (94.488188976378*MICROSTEPS)
#define DEFAULT_Y_STEPS_PER_MM (94.488188976378*MICROSTEPS)
#define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS)
-#define DEFAULT_A_STEPS_PER_MM (94.488188976378*MICROSTEPS)
+#define DEFAULT_A_STEPS_PER_DEG (94.488188976378*MICROSTEPS)
+#define DEFAULT_B_STEPS_PER_DEG (94.488188976378*MICROSTEPS)
+#define DEFAULT_C_STEPS_PER_DEG (94.488188976378*MICROSTEPS)
#define DEFAULT_STEP_PULSE_MICROSECONDS 10
#define DEFAULT_MM_PER_ARC_SEGMENT 0.1
#define DEFAULT_RAPID_FEEDRATE 500.0 // mm/min
#define DEFAULT_FEEDRATE 250.0
#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE*60*60/10.0) // mm/min^2
#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm
-#define DEFAULT_STEPPING_INVERT_MASK ((1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)|(1<<A_DIRECTION_BIT))
+#define DEFAULT_STEPPING_INVERT_MASK ((1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)|(1<<A_DIRECTION_BIT)|(1<<B_DIRECTION_BIT)|(1<<C_DIRECTION_BIT))
#define DEFAULT_REPORT_INCHES 0 // false
#define DEFAULT_AUTO_START 1 // true
#define DEFAULT_INVERT_ST_ENABLE 0 // false
View
14 gcode.c
@@ -66,12 +66,14 @@ void gc_init()
// Sets g-code parser position in mm. Input in steps. Called by the system abort and hard
// limit pull-off routines.
-void gc_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a)
+void gc_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b, int32_t c)
{
gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS];
gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS];
gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS];
gc.position[A_AXIS] = a/settings.steps_per_mm[A_AXIS];
+ gc.position[B_AXIS] = b/settings.steps_per_mm[B_AXIS];
+ gc.position[C_AXIS] = c/settings.steps_per_mm[C_AXIS];
}
static float to_millimeters(float value)
@@ -238,6 +240,8 @@ uint8_t gc_execute_line(char *line)
case 'Y': target[Y_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Y_AXIS)); break;
case 'Z': target[Z_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Z_AXIS)); break;
case 'A': target[A_AXIS] = to_millimeters(value); bit_true(axis_words,bit(A_AXIS)); break;
+ case 'B': target[B_AXIS] = to_millimeters(value); bit_true(axis_words,bit(B_AXIS)); break;
+ case 'C': target[C_AXIS] = to_millimeters(value); bit_true(axis_words,bit(C_AXIS)); break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
}
@@ -325,14 +329,14 @@ uint8_t gc_execute_line(char *line)
target[i] = gc.position[i];
}
}
- mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], settings.default_seek_rate, false);
+ mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], target[B_AXIS], target[C_AXIS], settings.default_seek_rate, false);
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
uint8_t home_select = SETTING_INDEX_G28;
if (non_modal_action == NON_MODAL_GO_HOME_1) { home_select = SETTING_INDEX_G30; }
if (!settings_read_coord_data(home_select,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
- mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[A_AXIS], settings.default_seek_rate, false);
+ mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[A_AXIS], coord_data[B_AXIS], coord_data[C_AXIS], settings.default_seek_rate, false);
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
@@ -402,15 +406,15 @@ uint8_t gc_execute_line(char *line)
break;
case MOTION_MODE_SEEK:
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
- else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], settings.default_seek_rate, false); }
+ else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], target[B_AXIS], target[C_AXIS], settings.default_seek_rate, false); }
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
// to check for initial F-word upon startup. Maybe just set to zero upon initialization
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);}
- else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS],
+ else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], target[B_AXIS], target[C_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); }
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
View
2 gcode.h
@@ -105,6 +105,6 @@ void gc_init();
uint8_t gc_execute_line(char *line);
// Set g-code parser position. Input in steps.
-void gc_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a);
+void gc_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b, int32_t c);
#endif
View
40 limits.c
@@ -77,8 +77,8 @@ ISR(LIMIT_INT_vect)
// algorithm is written here. This also lets users hack and tune this code freely for
// their own particular needs without affecting the rest of Grbl.
// NOTE: Only the abort runtime command can interrupt this process.
-static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool a_axis, int8_t pos_dir,
- bool invert_pin, float homing_rate)
+static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool a_axis, bool b_axis, bool c_axis,
+ int8_t pos_dir, bool invert_pin, float homing_rate)
{
// Determine governing axes with finest step resolution per distance for the Bresenham
// algorithm. This solves the issue when homing multiple axes that have different
@@ -93,14 +93,16 @@ static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool a_axis, int
if (y_axis) { steps[Y_AXIS] = lround(settings.steps_per_mm[Y_AXIS]); }
if (z_axis) { steps[Z_AXIS] = lround(settings.steps_per_mm[Z_AXIS]); }
if (a_axis) { steps[A_AXIS] = lround(settings.steps_per_mm[A_AXIS]); }
- uint32_t step_event_count = max(steps[X_AXIS], max(steps[Y_AXIS], max(steps[Z_AXIS], steps[A_AXIS])));
+ if (b_axis) { steps[B_AXIS] = lround(settings.steps_per_mm[B_AXIS]); }
+ if (c_axis) { steps[C_AXIS] = lround(settings.steps_per_mm[C_AXIS]); }
+ uint32_t step_event_count = max(steps[X_AXIS], max(steps[Y_AXIS], max(steps[Z_AXIS], max(steps[A_AXIS], max(steps[B_AXIS], steps[C_AXIS])))));
// To ensure global acceleration is not exceeded, reduce the governing axes nominal rate
// by adjusting the actual axes distance traveled per step. This is the same procedure
// used in the main planner to account for distance traveled when moving multiple axes.
// NOTE: When axis acceleration independence is installed, this will be updated to move
// all axes at their maximum acceleration and rate.
- float ds = step_event_count/sqrt(x_axis+y_axis+z_axis+a_axis);
+ float ds = step_event_count/sqrt(x_axis+y_axis+z_axis+a_axis+b_axis+c_axis);
// Compute the adjusted step rate change with each acceleration tick. (in step/min/acceleration_tick)
uint32_t delta_rate = ceil( ds*settings.acceleration/(60*ACCELERATION_TICKS_PER_SECOND));
@@ -123,6 +125,8 @@ static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool a_axis, int
int32_t counter_y = counter_x;
int32_t counter_z = counter_x;
int32_t counter_a = counter_x;
+ int32_t counter_b = counter_x;
+ int32_t counter_c = counter_x;
uint32_t step_delay = dt-settings.pulse_microseconds; // Step delay after pulse
uint32_t step_rate = 0; // Tracks step rate. Initialized from 0 rate. (in step/min)
uint32_t trap_counter = MICROSECONDS_PER_ACCELERATION_TICK/2; // Acceleration trapezoid counter
@@ -172,9 +176,27 @@ static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool a_axis, int
}
}
+ if (b_axis) {
+ counter_b += steps[B_AXIS];
+ if (counter_b > 0) {
+ if (limit_state & (1<<B_LIMIT_BIT)) { out_bits ^= (1<<B_STEP_BIT); }
+ else { b_axis = false; }
+ counter_b -= step_event_count;
+ }
+ }
+
+ if (c_axis) {
+ counter_c += steps[C_AXIS];
+ if (counter_c > 0) {
+ if (limit_state & (1<<C_LIMIT_BIT)) { out_bits ^= (1<<C_STEP_BIT); }
+ else { c_axis = false; }
+ counter_c -= step_event_count;
+ }
+ }
+
// Check if we are done or for system abort
protocol_execute_runtime();
- if (!(x_axis || y_axis || z_axis) || a_axis || sys.abort) { return; }
+ if (!(x_axis || y_axis || z_axis) || a_axis || b_axis || c_axis || sys.abort) { return; }
// Perform step.
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (out_bits & STEP_MASK);
@@ -205,21 +227,21 @@ void limits_go_home()
st_wake_up();
// Jog all axes toward home to engage their limit switches at faster homing seek rate.
- homing_cycle(false, false, true, true, true, false, settings.homing_seek_rate); // First jog the z and a axes
- homing_cycle(true, true, false, false, true, false, settings.homing_seek_rate); // Then jog the x and y axis
+ homing_cycle(false, false, true, true, true, true, true, false, settings.homing_seek_rate); // First jog the z and a axes
+ homing_cycle(true, true, false, false, false, false, true, false, settings.homing_seek_rate); // Then jog the x and y axis
delay_ms(settings.homing_debounce_delay); // Delay to debounce signal
// Now in proximity of all limits. Carefully leave and approach switches in multiple cycles
// to precisely hone in on the machine zero location. Moves at slower homing feed rate.
int8_t n_cycle = N_HOMING_CYCLE;
while (n_cycle--) {
// Leave all switches to release them. After cycles complete, this is machine zero.
- homing_cycle(true, true, true, true, false, true, settings.homing_feed_rate);
+ homing_cycle(true, true, true, true, true, true, false, true, settings.homing_feed_rate);
delay_ms(settings.homing_debounce_delay);
if (n_cycle > 0) {
// Re-approach all switches to re-engage them.
- homing_cycle(true, true, true, true, true, false, settings.homing_feed_rate);
+ homing_cycle(true, true, true, true, true, true, true, false, settings.homing_feed_rate);
delay_ms(settings.homing_debounce_delay);
}
}
View
22 motion_control.c
@@ -47,7 +47,7 @@
// However, this keeps the memory requirements lower since it doesn't have to call and hold two
// plan_buffer_lines in memory. Grbl only has to retain the original line input variables during a
// backlash segment(s).
-void mc_line(float x, float y, float z, float a, float feed_rate, uint8_t invert_feed_rate)
+void mc_line(float x, float y, float z, float a, float b, float c, float feed_rate, uint8_t invert_feed_rate)
{
// TODO: Backlash compensation may be installed here. Only need direction info to track when
// to insert a backlash line motion(s) before the intended line motion. Requires its own
@@ -65,7 +65,7 @@ void mc_line(float x, float y, float z, float a, float feed_rate, uint8_t invert
// If in check gcode mode, prevent motion by blocking planner.
if (bit_isfalse(gc.switches,BITFLAG_CHECK_GCODE)) {
- plan_buffer_line(x, y, z, a, feed_rate, invert_feed_rate);
+ plan_buffer_line(x, y, z, a, b, c, feed_rate, invert_feed_rate);
// Indicate to the system there is now a planned block in the buffer ready to cycle start.
// NOTE: If homing cycles are enabled, a position lost state will lock out all motions,
@@ -124,8 +124,8 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Solution approach by Jens Geisler.
- r_T = [cos(phi) -sin(phi);
- sin(phi) cos(phi] * r ;
+ 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
@@ -183,13 +183,13 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
arc_target[axis_0] = center_axis0 + r_axis0;
arc_target[axis_1] = center_axis1 + r_axis1;
arc_target[axis_linear] += linear_per_segment;
- mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[A_AXIS], feed_rate, invert_feed_rate);
+ mc_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[A_AXIS], arc_target[B_AXIS], arc_target[C_AXIS], feed_rate, invert_feed_rate);
// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
if (sys.abort) { return; }
}
// Ensure last segment arrives at target location.
- mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], feed_rate, invert_feed_rate);
+ mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[A_AXIS], target[B_AXIS], target[C_AXIS], feed_rate, invert_feed_rate);
}
@@ -231,14 +231,16 @@ void mc_go_home()
// Pull-off all axes from limit switches before continuing motion. This provides some initial
// clearance off the switches and should also help prevent them from falsely tripping when
// hard limits are enabled.
- int8_t x_dir, y_dir, z_dir, a_dir;
- x_dir = y_dir = z_dir = a_dir = -1;
+ int8_t x_dir, y_dir, z_dir, a_dir, b_dir, c_dir;
+ x_dir = y_dir = z_dir = a_dir = b_dir = c_dir = -1;
if (bit_istrue(settings.homing_dir_mask,bit(X_DIRECTION_BIT))) { x_dir = 1; }
if (bit_istrue(settings.homing_dir_mask,bit(Y_DIRECTION_BIT))) { y_dir = 1; }
if (bit_istrue(settings.homing_dir_mask,bit(Z_DIRECTION_BIT))) { z_dir = 1; }
if (bit_istrue(settings.homing_dir_mask,bit(A_DIRECTION_BIT))) { a_dir = 1; }
- mc_line(x_dir*settings.homing_pulloff, y_dir*settings.homing_pulloff,
- z_dir*settings.homing_pulloff, a_dir*settings.homing_pulloff, settings.homing_feed_rate, false);
+ if (bit_istrue(settings.homing_dir_mask,bit(B_DIRECTION_BIT))) { b_dir = 1; }
+ if (bit_istrue(settings.homing_dir_mask,bit(C_DIRECTION_BIT))) { c_dir = 1; }
+ mc_line(x_dir*settings.homing_pulloff, y_dir*settings.homing_pulloff, z_dir*settings.homing_pulloff,
+ a_dir*settings.homing_pulloff, b_dir*settings.homing_pulloff, c_dir*settings.homing_pulloff, settings.homing_feed_rate, false);
st_cycle_start(); // Move it. Nothing should be in the buffer except this motion.
plan_synchronize(); // Make sure the motion completes.
View
2 motion_control.h
@@ -28,7 +28,7 @@
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
-void mc_line(float x, float y, float z, float a, float feed_rate, uint8_t invert_feed_rate);
+void mc_line(float x, float y, float z, float a, float b, float c, float feed_rate, uint8_t invert_feed_rate);
// 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
View
6 nuts_bolts.c
@@ -143,6 +143,8 @@ void delay_us(uint32_t us)
void sys_sync_current_position()
{
- plan_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS], sys.position[A_AXIS]);
- gc_set_current_position(sys.position[X_AXIS],sys.position[Y_AXIS],sys.position[Z_AXIS], sys.position[Z_AXIS]);
+ plan_set_current_position(sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS],
+ sys.position[A_AXIS], sys.position[B_AXIS], sys.position[C_AXIS]);
+ gc_set_current_position(sys.position[X_AXIS], sys.position[Y_AXIS], sys.position[Z_AXIS],
+ sys.position[A_AXIS], sys.position[B_AXIS], sys.position[C_AXIS]);
}
View
4 nuts_bolts.h
@@ -30,11 +30,13 @@
#define false 0
#define true 1
-#define N_AXIS 4 // Number of axes
+#define N_AXIS 6 // Number of axes
#define X_AXIS 0 // Axis indexing value
#define Y_AXIS 1
#define Z_AXIS 2
#define A_AXIS 3
+#define B_AXIS 4
+#define C_AXIS 5
#define MM_PER_INCH (25.40)
#define INCH_PER_MM (0.0393701)
View
30 planner.c
@@ -34,11 +34,11 @@
static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
static volatile uint8_t block_buffer_head; // Index of the next block to be pushed
static volatile uint8_t block_buffer_tail; // Index of the block to process now
-static uint8_t next_buffer_head; // Index of the next buffer head
+static uint8_t next_buffer_head; // Index of the next buffer head
// Define planner variables
typedef struct {
- int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate
+ int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate
// from g-code position for movements requiring multiple line motions,
// i.e. arcs, canned cycles, and backlash compensation.
float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment
@@ -342,7 +342,7 @@ void plan_synchronize()
// All position data passed to the planner must be in terms of machine position to keep the planner
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
-void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8_t invert_feed_rate)
+void plan_buffer_line(float x, float y, float z, float a, float b, float c, float feed_rate, uint8_t invert_feed_rate)
{
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
@@ -353,20 +353,26 @@ void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8
target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);
target[A_AXIS] = lround(a*settings.steps_per_mm[A_AXIS]);
+ target[B_AXIS] = lround(b*settings.steps_per_mm[B_AXIS]);
+ target[C_AXIS] = lround(c*settings.steps_per_mm[C_AXIS]);
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
if (target[A_AXIS] < pl.position[A_AXIS]) { block->direction_bits |= (1<<A_DIRECTION_BIT); }
+ if (target[B_AXIS] < pl.position[B_AXIS]) { block->direction_bits |= (1<<B_DIRECTION_BIT); }
+ if (target[C_AXIS] < pl.position[C_AXIS]) { block->direction_bits |= (1<<C_DIRECTION_BIT); }
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
block->steps_a = labs(target[A_AXIS]-pl.position[A_AXIS]);
- block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_a)));
+ block->steps_b = labs(target[B_AXIS]-pl.position[B_AXIS]);
+ block->steps_c = labs(target[C_AXIS]-pl.position[C_AXIS]);
+ block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, max(block->steps_a, max(block->steps_b, block->steps_c)))));
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; };
@@ -377,8 +383,10 @@ void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8
delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
delta_mm[A_AXIS] = (target[A_AXIS]-pl.position[A_AXIS])/settings.steps_per_mm[A_AXIS];
- block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] +
- delta_mm[Z_AXIS]*delta_mm[Z_AXIS] + delta_mm[A_AXIS]*delta_mm[A_AXIS]);
+ delta_mm[B_AXIS] = (target[B_AXIS]-pl.position[B_AXIS])/settings.steps_per_mm[B_AXIS];
+ delta_mm[C_AXIS] = (target[C_AXIS]-pl.position[C_AXIS])/settings.steps_per_mm[C_AXIS];
+ block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + delta_mm[Z_AXIS]*delta_mm[Z_AXIS] +
+ delta_mm[A_AXIS]*delta_mm[A_AXIS] + delta_mm[B_AXIS]*delta_mm[B_AXIS] + delta_mm[C_AXIS]*delta_mm[C_AXIS]);
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
@@ -409,6 +417,8 @@ void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
unit_vec[A_AXIS] = delta_mm[A_AXIS]*inverse_millimeters;
+ unit_vec[B_AXIS] = delta_mm[B_AXIS]*inverse_millimeters;
+ unit_vec[C_AXIS] = delta_mm[C_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
@@ -434,7 +444,9 @@ void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8
float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS]
- - pl.previous_unit_vec[A_AXIS] * unit_vec[A_AXIS] ;
+ - pl.previous_unit_vec[A_AXIS] * unit_vec[A_AXIS]
+ - pl.previous_unit_vec[B_AXIS] * unit_vec[B_AXIS]
+ - pl.previous_unit_vec[C_AXIS] * unit_vec[C_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
@@ -481,12 +493,14 @@ void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8
}
// Reset the planner position vector (in steps). Called by the system abort routine.
-void plan_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a)
+void plan_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b, int32_t c)
{
pl.position[X_AXIS] = x;
pl.position[Y_AXIS] = y;
pl.position[Z_AXIS] = z;
pl.position[A_AXIS] = a;
+ pl.position[B_AXIS] = b;
+ pl.position[C_AXIS] = c;
}
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
View
6 planner.h
@@ -33,7 +33,7 @@ typedef struct {
// Fields used by the bresenham algorithm for tracing the line
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
- uint32_t steps_x, steps_y, steps_z, steps_a; // Step count along each axis
+ uint32_t steps_x, steps_y, steps_z, steps_a, steps_b, steps_c; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block
// Fields used by the motion planner to manage acceleration
@@ -60,7 +60,7 @@ void plan_init();
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
-void plan_buffer_line(float x, float y, float z, float a, float feed_rate, uint8_t invert_feed_rate);
+void plan_buffer_line(float x, float y, float z, float a, float b, float c, float feed_rate, uint8_t invert_feed_rate);
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks.
@@ -70,7 +70,7 @@ void plan_discard_current_block();
block_t *plan_get_current_block();
// Reset the planner position vector (in steps)
-void plan_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a);
+void plan_set_current_position(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b, int32_t c);
// Reinitialize plan with a partially completed block
void plan_cycle_reinitialize(int32_t step_events_remaining);
View
4 report.c
@@ -164,7 +164,9 @@ void report_grbl_settings() {
printPgmString(PSTR(" (homing seek, mm/min)\r\n$21=")); printInteger(settings.homing_debounce_delay);
printPgmString(PSTR(" (homing debounce, msec)\r\n$22=")); printFloat(settings.homing_pulloff);
printPgmString(PSTR(" (homing pull-off, mm)\r\n$30=")); printFloat(settings.steps_per_mm[A_AXIS]);
- printPgmString(PSTR(" (a, step/mm)\r\n"));
+ printPgmString(PSTR(" (a, deg/mm)\r\n$31=")); printFloat(settings.steps_per_mm[B_AXIS]);
+ printPgmString(PSTR(" (b, deg/mm)\r\n$32=")); printFloat(settings.steps_per_mm[C_AXIS]);
+ printPgmString(PSTR(" (c, deg/mm)\r\n"));
}
View
4 settings.c
@@ -71,7 +71,9 @@ void settings_reset(bool reset_all) {
settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM;
settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM;
settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM;
- settings.steps_per_mm[A_AXIS] = DEFAULT_A_STEPS_PER_MM;
+ settings.steps_per_mm[A_AXIS] = DEFAULT_A_STEPS_PER_DEG;
+ settings.steps_per_mm[B_AXIS] = DEFAULT_B_STEPS_PER_DEG;
+ settings.steps_per_mm[C_AXIS] = DEFAULT_C_STEPS_PER_DEG;
settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS;
settings.default_feed_rate = DEFAULT_FEEDRATE;
settings.default_seek_rate = DEFAULT_RAPID_FEEDRATE;
View
99 stepper.c
@@ -22,6 +22,7 @@
/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
and Philipp Tiefenbacher. */
+#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include "stepper.h"
#include "config.h"
@@ -38,7 +39,9 @@ typedef struct {
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z,
- counter_a;
+ counter_a,
+ counter_b,
+ counter_c;
uint32_t event_count;
uint32_t step_events_completed; // The number of step events left in current motion
@@ -55,7 +58,11 @@ static block_t *current_block; // A pointer to the block currently being traced
// Used by the stepper driver interrupt
static uint8_t step_pulse_time; // Step pulse reset time after step rise
+#ifdef DIRECTION_DDR
+static uint8_t out_step_bits, out_dir_bits;
+#else
static uint8_t out_bits; // The next stepping-bits to be output
+#endif
static volatile uint8_t busy; // True when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
#if STEP_PULSE_DELAY > 0
@@ -93,7 +100,11 @@ void st_wake_up()
}
if (sys.state == STATE_CYCLE) {
// Initialize stepper output bits
- out_bits = (0) ^ (settings.invert_mask);
+ #ifdef DIRECTION_DDR
+ out_dir_bits = (0) ^ (settings.invert_mask);
+ #else
+ out_bits = (0) ^ (settings.invert_mask);
+ #endif
// Initialize step pulse timing from settings. Here to ensure updating after re-writing.
#ifdef STEP_PULSE_DELAY
// Set total step pulse time after direction pin set. Ad hoc computation from oscilloscope.
@@ -151,15 +162,23 @@ ISR(TIMER1_COMPA_vect)
// Set the direction pins a couple of nanoseconds before we step the steppers
#ifdef DIRECTION_DDR
- DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
+ DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (out_dir_bits & DIRECTION_MASK);
#else
- STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
+ STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
#endif
// Then pulse the stepping pins
#ifdef STEP_PULSE_DELAY
+ #ifdef DIRECTION_DDR
+ step_bits = (STEPPING_PORT & ~STEP_MASK) | out_step_bits;
+ #else
step_bits = (STEPPING_PORT & ~STEP_MASK) | out_bits; // Store out_bits to prevent overwriting.
+ #endif
#else // Normal operation
+ #ifdef DIRECTION_DDR
+ STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_step_bits;
+ #else
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
+ #endif
#endif
// Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
// exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
@@ -188,6 +207,8 @@ ISR(TIMER1_COMPA_vect)
st.counter_y = st.counter_x;
st.counter_z = st.counter_x;
st.counter_a = st.counter_x;
+ st.counter_b = st.counter_x;
+ st.counter_c = st.counter_x;
st.event_count = current_block->step_event_count;
st.step_events_completed = 0;
} else {
@@ -198,7 +219,55 @@ ISR(TIMER1_COMPA_vect)
if (current_block != NULL) {
// Execute step displacement profile by bresenham line algorithm
- out_bits = current_block->direction_bits;
+ #ifdef DIRECTION_DDR
+ out_dir_bits = current_block->direction_bits;
+ out_step_bits = 0;
+
+ st.counter_x += current_block->steps_x;
+ if (st.counter_x > 0) {
+ out_step_bits |= (1<<X_STEP_BIT);
+ st.counter_x -= st.event_count;
+ if (out_dir_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+ else { sys.position[X_AXIS]++; }
+ }
+ st.counter_y += current_block->steps_y;
+ if (st.counter_y > 0) {
+ out_step_bits |= (1<<Y_STEP_BIT);
+ st.counter_y -= st.event_count;
+ if (out_dir_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+ else { sys.position[Y_AXIS]++; }
+ }
+ st.counter_z += current_block->steps_z;
+ if (st.counter_z > 0) {
+ out_step_bits |= (1<<Z_STEP_BIT);
+ st.counter_z -= st.event_count;
+ if (out_dir_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
+ else { sys.position[Z_AXIS]++; }
+ }
+
+ st.counter_a += current_block->steps_a;
+ if (st.counter_a > 0) {
+ out_step_bits |= (1<<A_STEP_BIT);
+ st.counter_a -= st.event_count;
+ if (out_dir_bits & (1<<A_DIRECTION_BIT)) { sys.position[A_AXIS]--; }
+ else { sys.position[A_AXIS]++; }
+ }
+ st.counter_b += current_block->steps_b;
+ if (st.counter_b > 0) {
+ out_step_bits |= (1<<B_STEP_BIT);
+ st.counter_b -= st.event_count;
+ if (out_dir_bits & (1<<B_DIRECTION_BIT)) { sys.position[B_AXIS]--; }
+ else { sys.position[B_AXIS]++; }
+ }
+ st.counter_c += current_block->steps_c;
+ if (st.counter_c > 0) {
+ out_step_bits |= (1<<C_STEP_BIT);
+ st.counter_c -= st.event_count;
+ if (out_dir_bits & (1<<C_DIRECTION_BIT)) { sys.position[C_AXIS]--; }
+ else { sys.position[C_AXIS]++; }
+ }
+ #else
+ out_bits = current_block->direction_bits;
st.counter_x += current_block->steps_x;
if (st.counter_x > 0) {
out_bits |= (1<<X_STEP_BIT);
@@ -228,6 +297,21 @@ ISR(TIMER1_COMPA_vect)
if (out_bits & (1<<A_DIRECTION_BIT)) { sys.position[A_AXIS]--; }
else { sys.position[A_AXIS]++; }
}
+ st.counter_b += current_block->steps_b;
+ if (st.counter_b > 0) {
+ out_bits |= (1<<B_STEP_BIT);
+ st.counter_b -= st.event_count;
+ if (out_bits & (1<<B_DIRECTION_BIT)) { sys.position[B_AXIS]--; }
+ else { sys.position[B_AXIS]++; }
+ }
+ st.counter_c += current_block->steps_c;
+ if (st.counter_c > 0) {
+ out_bits |= (1<<C_STEP_BIT);
+ st.counter_c -= st.event_count;
+ if (out_bits & (1<<C_DIRECTION_BIT)) { sys.position[C_AXIS]--; }
+ else { sys.position[C_AXIS]++; }
+ }
+ #endif
st.step_events_completed++; // Iterate step events
@@ -317,7 +401,12 @@ ISR(TIMER1_COMPA_vect)
plan_discard_current_block();
}
}
+ #ifdef DIRECTION_DDR
+ // todo: check what happens with steps
+ out_dir_bits ^= settings.invert_mask;
+ #else
out_bits ^= settings.invert_mask; // Apply step and direction invert mask
+ #endif
busy = false;
}

0 comments on commit 9fc73ef

Please sign in to comment.