From 2992683c8dd68546ac85bd7d475bff9df1578622 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sun, 8 Feb 2009 22:08:27 +0100 Subject: [PATCH] optimized for size and did some housekeeping --- gcode.c | 17 +++------- motion_control.c | 87 ++++++++++++++++++------------------------------ motion_control.h | 5 --- stepper.c | 30 +++++++++-------- todo.txt | 11 +++--- 5 files changed, 59 insertions(+), 91 deletions(-) diff --git a/gcode.c b/gcode.c index 40a3d06..309c240 100644 --- a/gcode.c +++ b/gcode.c @@ -54,8 +54,6 @@ #include "errno.h" #include "serial_protocol.h" -#include "wiring_serial.h" - #define NEXT_ACTION_DEFAULT 0 #define NEXT_ACTION_DWELL 1 #define NEXT_ACTION_GO_HOME 2 @@ -195,11 +193,11 @@ uint8_t gc_execute_line(char *line) { // If there were any errors parsing this line, we will return right away with the bad news if (gc.status_code) { return(gc.status_code); } - // Pass 2: Parameters counter = 0; clear_vector(offset); memcpy(target, gc.position, sizeof(target)); // target = gc.position + // Pass 2: Parameters while(next_statement(&letter, &value, line, &counter)) { int_value = trunc(value); unit_converted_value = to_millimeters(value); @@ -313,7 +311,6 @@ uint8_t gc_execute_line(char *line) { // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any // real CNC, and thus - for practical reasons - we will terminate promptly: if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); } - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } @@ -336,10 +333,9 @@ uint8_t gc_execute_line(char *line) { // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), // even though it is advised against ever generating such circles in a single line of g-code. By - // inverting the sign of h_x2_div_d the center of the circles is placed on the opposide side of the line of + // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of // travel and thus we get the unadvisably long circles as prescribed. - if (r < 0) { h_x2_div_d = -h_x2_div_d; } - + if (r < 0) { h_x2_div_d = -h_x2_div_d; } // Complete the operation by calculating the actual center of the arc offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2; offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2; @@ -375,12 +371,7 @@ uint8_t gc_execute_line(char *line) { } // Find the radius double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); - // Prepare the arc - // printString("mc_arc("); - // printInteger(trunc(theta_start/M_PI*180)); printByte(','); - // printInteger(trunc(angular_travel/M_PI*180)); printByte(','); - // printInteger(trunc(radius)); - // printByte(')'); + // Trace the arc mc_arc(theta_start, angular_travel, radius, gc.plane_axis_0, gc.plane_axis_1, gc.feed_rate); break; } diff --git a/motion_control.c b/motion_control.c index 15e3425..0aaa4e1 100644 --- a/motion_control.c +++ b/motion_control.c @@ -34,13 +34,10 @@ #include #include "nuts_bolts.h" #include "stepper.h" -#include "serial_protocol.h" - -#include "wiring_serial.h" #define ONE_MINUTE_OF_MICROSECONDS 60000000.0 -int8_t mode; // The current operation mode +volatile int8_t mode; // The current operation mode int32_t position[3]; // The current position of the tool in absolute steps uint8_t direction_bits; // The direction bits to be used with any upcoming step-instruction @@ -51,7 +48,7 @@ void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, void mc_init() { - mode = 0; + mode = MC_MODE_AT_REST; clear_vector(position); } @@ -63,7 +60,7 @@ void mc_dwell(uint32_t milliseconds) mode = MC_MODE_AT_REST; } -// Prepare for linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second +// Execute 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. void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate) { @@ -76,12 +73,12 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate counter[3], // A counter used in the bresenham algorithm for line plotting maximum_steps; // The larges absolute step-count of any axis + + // Setup + target[X_AXIS] = x*X_STEPS_PER_MM; target[Y_AXIS] = y*Y_STEPS_PER_MM; target[Z_AXIS] = z*Z_STEPS_PER_MM; - - mode = MC_MODE_LINEAR; - // Determine direction and travel magnitude for each axis for(axis = X_AXIS; axis <= Z_AXIS; axis++) { step_count[axis] = abs(target[axis] - position[axis]); @@ -91,11 +88,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate maximum_steps = max(step_count[Z_AXIS], max(step_count[X_AXIS], step_count[Y_AXIS])); // Nothing to do? - if (maximum_steps == 0) - { - mode = MC_MODE_AT_REST; - return; - } + if (maximum_steps == 0) { return; } // Set up a neat counter for each axis for(axis = X_AXIS; axis <= Z_AXIS; axis++) { counter[axis] = -maximum_steps/2; @@ -118,6 +111,8 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate // Execution + mode = MC_MODE_LINEAR; + while(mode) { // Trace the line step_bits = 0; @@ -142,7 +137,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate } -// Prepare an arc. theta == start angle, angular_travel == number of radians to go along the arc, +// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the // circle in millimeters. axis_1 and axis_2 selects the plane in tool space. // ISSUE: The arc interpolator assumes all axes have the same steps/mm as the X axis. @@ -160,14 +155,15 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int 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 - int8_t diagonal_bits; // A bitmask with the stepper bits for both selected axes set + int8_t diagonal_bits; // A bitmask with the stepper bits for both selected axes set int incomplete; // True if the arc has not reached its target yet int dx, dy; // Trace directions + // Setup + uint32_t radius_steps = round(radius*X_STEPS_PER_MM); if(radius_steps == 0) { return; } - mode = MC_MODE_ARC; // Determine angular direction (+1 = clockwise, -1 = counterclockwise) angular_direction = signof(angular_travel); // Calculate the initial position and target position in the local coordinate system of the arc @@ -192,69 +188,63 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int // And map the local coordinate system of the arc onto the tool axes of the selected plane axis_x = axis_1; axis_y = axis_2; - // The amount of steppings performed while tracing a full circle is equal to the sum of sides in a - // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a full circle: + // The amount of steppings performed while tracing a half circle is equal to the sum of sides in a + // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a half circle: uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2))); - // We then calculate the millimeters of travel along the circumference of that same full circle + // We then calculate the millimeters of travel along the circumference of that same half circle double millimeters_half_circumference = radius*M_PI; // Then we calculate the microseconds between each step as if we will trace the full circle. - // It doesn't matter what fraction of the circle we are actuallyt going to trace. The pace is the same. - st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle); - - incomplete = true; + // It doesn't matter what fraction of the circle we are actually going to trace. The pace is the same. + st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle); // Execution + + mode = MC_MODE_ARC; + incomplete = true; while(incomplete) { dx = (y!=0) ? signof(y) * angular_direction : -signof(x); dy = (x!=0) ? -signof(x) * angular_direction : -signof(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[axis_x] = dx; direction[axis_y] = dy; set_stepper_directions(direction); - + // Check which axis will be "major" for this stepping if (abs(x)= abs(diagonal_error)) { - y += dy; - y2 += 2*dy; + y += dy; y2 += 2*dy; error = diagonal_error; step_steppers(diagonal_bits); // step diagonal } else { step_axis(axis_x); // step straight } } else { - // Step arc vertically - y+=dy; + // Step arc vertically error += 1+y2*dy; - y2 += 2*dy; + y+=dy; y2 += 2*dy; diagonal_error = error + 1 + x2*dx; if(abs(error) >= abs(diagonal_error)) { - x += dx; - x2 += 2*dx; + x += dx; x2 += 2*dx; error = diagonal_error; step_steppers(diagonal_bits); // step diagonal } else { step_axis(axis_y); // step straight } - } - - // Check if target has been reached + } + // Check if target has been reached. Todo: Simplify/optimize/clarify if ((x * target_direction_y >= target_x * target_direction_y) && (y * target_direction_x <= target_y * target_direction_x)) { if ((signof(x) == signof(target_x)) && (signof(y) == signof(target_y))) { incomplete = false; } } - } - + } // Update the tool position to the new actual position position[axis_x] += x-start_x; position[axis_y] += y-start_y; @@ -275,7 +265,7 @@ int mc_status() return(mode); } -// Set the direction pins for the stepper motors according to the provided vector. +// Set the direction bits for the stepper motors according to the provided vector. // direction is an array of three 8 bit integers representing the direction of // each motor. The values should be -1 (reverse), 0 or 1 (forward). void set_stepper_directions(int8_t *direction) @@ -302,16 +292,5 @@ inline void step_steppers(uint8_t bits) // Step only one motor inline void step_axis(uint8_t axis) { - switch (axis) { - case X_AXIS: st_buffer_step(direction_bits | (1< 0: the system is busy, // result < 0: the system is in an error state. int mc_status(); -void printCurrentPosition(); - #endif diff --git a/stepper.c b/stepper.c index 57113d3..0abb49b 100644 --- a/stepper.c +++ b/stepper.c @@ -32,6 +32,9 @@ #define TICKS_PER_MICROSECOND (F_CPU/1000000) #define STEP_BUFFER_SIZE 100 +// A marker used to notify the stepper handler of a pace change +#define PACE_CHANGE_MARKER 0xff + 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; @@ -43,25 +46,26 @@ uint8_t echo_steps = true; void config_pace_timer(uint32_t microseconds); -// This timer interrupt is executed at the pace set with set_pace. It pops one instruction from +// This timer interrupt is executed at the pace set with st_buffer_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) { - if(step_buffer[step_buffer_tail] == 0xff) { - // If this is not a step-instruction, but a pace-marker: change pace + uint8_t popped = step_buffer[step_buffer_tail]; + if(popped == PACE_CHANGE_MARKER) { + // This is not a step-instruction, but a pace-change-marker: change pace config_pace_timer(next_pace); next_pace = 0; } else { // Set the direction pins a nanosecond or two before you step the steppers - STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (step_buffer[step_buffer_tail] & DIRECTION_MASK); + STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (popped & DIRECTION_MASK); // Then pulse the stepping pins - STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | step_buffer[step_buffer_tail]; + STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | popped; // 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 |= (1<