From 1bde999e0ef60cfac8280f52c26b237d2b67ca08 Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Sun, 31 Jan 2010 17:30:31 +1100 Subject: [PATCH] seems to be working nicely, although the speeds don't seem quite right... --- mendel/arduino.h | 7 +- mendel/dda.c | 194 +++++++++++++++++++++++++++-------------------- mendel/dda.h | 2 - mendel/gcode.c | 5 +- mendel/gcode.h | 2 + mendel/mendel.c | 13 +++- mendel/pinout.h | 12 ++- mendel/serial.c | 16 +++- mendel/timer.c | 11 ++- 9 files changed, 162 insertions(+), 100 deletions(-) diff --git a/mendel/arduino.h b/mendel/arduino.h index f4840d8..b2fc8ee 100644 --- a/mendel/arduino.h +++ b/mendel/arduino.h @@ -12,12 +12,15 @@ #endif #define _READ(IO) (IO ## _RPORT & MASK(IO ## _PIN)) -#define _WRITE(IO, v) if (v) { IO ## _WPORT |= MASK(IO ## _PIN); } else { IO ## _WPORT &= ~MASK(IO ## _PIN); } +#define _WRITE(IO, v) do { if (v) { IO ## _WPORT |= MASK(IO ## _PIN); } else { IO ## _WPORT &= ~MASK(IO ## _PIN); }; } while (0) #define _TOGGLE(IO) (IO ## _RPORT = MASK(IO ## _PIN)) #define _SET_INPUT(IO) (IO ## _DDR &= ~MASK(IO ## _PIN)) #define _SET_OUTPUT(IO) (IO ## _DDR |= MASK(IO ## _PIN)) +#define _GET_INPUT(IO) ((IO ## _DDR & MASK(IO ## _PIN)) == 0) +#define _GET_OUTPUT(IO) ((IO ## _DDR & MASK(IO ## _PIN)) != 0) + // why double up on these macros? see http://gcc.gnu.org/onlinedocs/cpp/Stringification.html #define READ(IO) _READ(IO) @@ -25,6 +28,8 @@ #define TOGGLE(IO) _TOGGLE(IO) #define SET_INPUT(IO) _SET_INPUT(IO) #define SET_OUTPUT(IO) _SET_OUTPUT(IO) +#define GET_INPUT(IO) _GET_INPUT(IO) +#define GET_OUTPUT(IO) _GET_OUTPUT(IO) /* ports and functions diff --git a/mendel/dda.c b/mendel/dda.c index d9c91a0..cb5788a 100644 --- a/mendel/dda.c +++ b/mendel/dda.c @@ -36,8 +36,8 @@ DDA movebuffer[MOVEBUFFER_SIZE]; position tracking */ -TARGET startpoint = { 0, 0, 0, 0, FEEDRATE_SLOW_Z }; -TARGET current_position = { 0, 0, 0, 0, FEEDRATE_SLOW_Z }; +TARGET startpoint = { 0, 0, 0, 0, 0 }; +TARGET current_position = { 0, 0, 0, 0, 0 }; uint8_t queue_full() { if (mb_tail == 0) @@ -47,7 +47,7 @@ uint8_t queue_full() { } uint8_t queue_empty() { - return (mb_tail == mb_head) && !movebuffer[mb_tail].live; + return ((mb_tail == mb_head) && (movebuffer[mb_tail].live == 0))?255:0; } void enqueue(TARGET *t) { @@ -60,14 +60,17 @@ void enqueue(TARGET *t) { h = 0; dda_create(t, &movebuffer[h]); mb_head = h; + + // fire up in case we're not running yet + enableTimerInterrupt(); } void next_move() { if (queue_empty()) { // queue is empty - disable_steppers(); - setTimer(DEFAULT_TICK); -// disableTimerInterrupt(); +// disable_steppers(); +// setTimer(DEFAULT_TICK); + disableTimerInterrupt(); } else { uint8_t t = mb_tail; @@ -166,17 +169,22 @@ void print_queue() { void dda_create(TARGET *target, DDA *dda) { uint32_t distance; + // initialise DDA to a known state + dda->move_duration = 0; + dda->live = 0; + dda->total_steps = 0; + if (DEBUG) serial_writestr_P(PSTR("\n{DDA_CREATE: [")); // we end at the passed target memcpy(&(dda->endpoint), target, sizeof(TARGET)); - dda->x_delta = abs32(dda->endpoint.X - startpoint.X); - dda->y_delta = abs32(dda->endpoint.Y - startpoint.Y); - dda->z_delta = abs32(dda->endpoint.Z - startpoint.Z); - dda->e_delta = abs32(dda->endpoint.E - startpoint.E); - dda->f_delta = abs32(dda->endpoint.F - startpoint.F); + dda->x_delta = abs32(target->X - startpoint.X); + dda->y_delta = abs32(target->Y - startpoint.Y); + dda->z_delta = abs32(target->Z - startpoint.Z); + dda->e_delta = abs32(target->E - startpoint.E); + dda->f_delta = abs32(target->F - startpoint.F); if (DEBUG) { serwrite_uint32(dda->x_delta); serial_writechar(','); @@ -186,86 +194,85 @@ void dda_create(TARGET *target, DDA *dda) { serwrite_uint32(dda->f_delta); serial_writestr_P(PSTR("] [")); } - dda->total_steps = dda->x_delta; + if (dda->x_delta > dda->total_steps) + dda->total_steps = dda->x_delta; if (dda->y_delta > dda->total_steps) dda->total_steps = dda->y_delta; if (dda->z_delta > dda->total_steps) dda->total_steps = dda->z_delta; - if (dda->e_delta > dda->total_steps) dda->total_steps = dda->e_delta; - if (dda->total_steps == 0) + if (dda->total_steps == 0) { dda->nullmove = 1; + } + else { - if (DEBUG) - serwrite_uint32(dda->total_steps); serial_writechar(','); + if (DEBUG) { + serwrite_uint32(dda->total_steps); serial_writechar(','); + } -// if (dda->f_delta > dda->total_steps) { -// dda->f_scale = dda->f_delta / dda->total_steps; -// if (dda->f_scale > 3) { -// dda->f_delta = dda->total_steps; -// } -// else { -// // if we boost the number of steps here, many will only be F-steps which take no time- maybe we should calculate move_distance first? -// dda->f_scale = 1; -// dda->total_steps = dda->f_delta; -// } -// } -// else { -// dda->f_scale = 1; -// } + // if (dda->f_delta > dda->total_steps) { + // dda->f_scale = dda->f_delta / dda->total_steps; + // if (dda->f_scale > 3) { + // dda->f_delta = dda->total_steps; + // } + // else { + // // if we boost the number of steps here, many will only be F-steps which take no time- maybe we should calculate move_distance first? + // dda->f_scale = 1; + // dda->total_steps = dda->f_delta; + // } + // } + // else { + // dda->f_scale = 1; + // } + // + // if (DEBUG) { + // serwrite_uint32(dda->total_steps); serial_writechar(','); + // } - if (DEBUG) - serwrite_uint32(dda->total_steps); serial_writechar(','); + dda->x_direction = (target->X >= startpoint.X)?1:0; + dda->y_direction = (target->Y >= startpoint.Y)?1:0; + dda->z_direction = (target->Z >= startpoint.Z)?1:0; + dda->e_direction = (target->E >= startpoint.E)?1:0; + dda->f_direction = (target->F >= startpoint.F)?1:0; - dda->x_direction = (dda->endpoint.X >= startpoint.X)?1:0; - dda->y_direction = (dda->endpoint.Y >= startpoint.Y)?1:0; - dda->z_direction = (dda->endpoint.Z >= startpoint.Z)?1:0; - dda->e_direction = (dda->endpoint.E >= startpoint.E)?1:0; - dda->f_direction = (dda->endpoint.F >= startpoint.F)?1:0; + dda->x_counter = dda->y_counter = dda->z_counter = dda->e_counter = dda->f_counter = + -(dda->total_steps >> 1); - dda->x_counter = dda->y_counter = dda->z_counter = dda->e_counter = dda->f_counter = - -(dda->total_steps >> 1); + // since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation. + if (dda->z_delta == 0) + distance = approx_distance(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y); + else if (dda->x_delta == 0 && dda->y_delta == 0) + distance = dda->z_delta * UM_PER_STEP_Z; + else + distance = approx_distance_3(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y, dda->z_delta * UM_PER_STEP_Z); - // since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation. - if (dda->z_delta == 0) - distance = approx_distance(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y); - else if (dda->x_delta == 0 && dda->y_delta == 0) - distance = dda->z_delta * UM_PER_STEP_Z; - else - distance = approx_distance_3(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y, dda->z_delta * UM_PER_STEP_Z); + if (distance < 2) + distance = dda->e_delta * UM_PER_STEP_E; + // if (distance < 2) + // distance = dda->f_delta; - if (distance < 2) - distance = dda->e_delta * UM_PER_STEP_E; -// if (distance < 2) -// distance = dda->f_delta; - - // pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context - // mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min - // note: um (distance) * 60000 == mm * 60000000 - // so in the interrupt we must simply calculate - // mm.us per step.min / mm per min (F) = us per step - if (dda->total_steps > 0) + // pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context + // mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min + // note: um (distance) * 60000 == mm * 60000000 + // so in the interrupt we must simply calculate + // mm.us per step.min / mm per min (F) = us per step dda->move_duration = distance * 60000 / dda->total_steps; - else - dda->move_duration = 0; + + if (DEBUG) + serwrite_uint32(dda->move_duration); + } if (DEBUG) - serwrite_uint32(dda->move_duration); serial_writestr_P(PSTR("] }\n")); + serial_writestr_P(PSTR("] }\n")); // next dda starts where we finish memcpy(&startpoint, target, sizeof(TARGET)); - // not running yet, we fire up in dda_start() - dda->live = 0; - // get steppers ready to go steptimeout = 0; enable_steppers(); - - // fire up - enableTimerInterrupt(); } /* @@ -274,21 +281,30 @@ void dda_create(TARGET *target, DDA *dda) { void dda_start(DDA *dda) { // called from interrupt context: keep it simple! - if (dda->nullmove) { + if ( + (current_position.X == dda->endpoint.X) && + (current_position.Y == dda->endpoint.Y) && + (current_position.Z == dda->endpoint.Z) && + (current_position.E == dda->endpoint.E) + ) { +// if (dda->nullmove) { // just change speed? current_position.F = dda->endpoint.F; return; } + // ensure steppers are ready to go + steptimeout = 0; + enable_steppers(); + // set direction outputs x_direction(dda->x_direction); y_direction(dda->y_direction); z_direction(dda->z_direction); e_direction(dda->e_direction); - // ensure steppers are ready to go - steptimeout = 0; - enable_steppers(); + // ensure this dda starts + dda->live = 1; // set timeout for first step setTimer(dda->move_duration / current_position.F); @@ -334,8 +350,12 @@ void dda_step(DDA *dda) { #define REAL_MOVE 32 #define F_REAL_STEP 64 + serial_writechar('!'); + + WRITE(SCK, 0); + do { - WRITE(SCK, 0); +// WRITE(SCK, 0); step_option = 0; // step_option |= can_step(x_min(), x_max(), current_position.X, dda->endpoint.X, dda->x_direction) & X_CAN_STEP; @@ -447,7 +467,7 @@ void dda_step(DDA *dda) { serial_writechar(']'); } - WRITE(SCK, 1); +// WRITE(SCK, 1); } while ( ((step_option & REAL_MOVE ) == 0) && ((step_option & F_CAN_STEP) != 0) ); @@ -455,19 +475,29 @@ void dda_step(DDA *dda) { // turn off step outputs, hopefully they've been on long enough by now to register with the drivers unstep(); - if (step_option & REAL_MOVE) { + if (step_option & REAL_MOVE) // we stepped, reset timeout steptimeout = 0; - // we have stepped in speed and now need to recalculate our delay - // WARNING: this is a divide in interrupt context! (which unfortunately seems unavoidable) - // we simply don't have the memory to precalculate this for each step, - // can't use a simplified process because the denominator changes rather than the numerator so the curve is non-linear - // and don't have a process framework to force it to be done outside interrupt context within a usable period of time - if (step_option & F_REAL_STEP) - setTimer(dda->move_duration / current_position.F); - } + // we have stepped in speed and now need to recalculate our delay + // WARNING: this is a divide in interrupt context! (which unfortunately seems unavoidable) + // we simply don't have the memory to precalculate this for each step, + // can't use a simplified process because the denominator changes rather than the numerator so the curve is non-linear + // and don't have a process framework to force it to be done outside interrupt context within a usable period of time + if (step_option & F_REAL_STEP) + setTimer(dda->move_duration / current_position.F); // if we could step, we're still running - dda->live = (step_option & (X_CAN_STEP | Y_CAN_STEP | Z_CAN_STEP | E_CAN_STEP | F_CAN_STEP))?1:0; +// dda->live = (step_option & (X_CAN_STEP | Y_CAN_STEP | Z_CAN_STEP | E_CAN_STEP | F_CAN_STEP))?1:0; + if ( + (current_position.X == dda->endpoint.X) && + (current_position.Y == dda->endpoint.Y) && + (current_position.Z == dda->endpoint.Z) && + (current_position.E == dda->endpoint.E) && + (current_position.F == dda->endpoint.F) + ) { + dda->live = 0; + } + + WRITE(SCK, 1); } diff --git a/mendel/dda.h b/mendel/dda.h index 0d0033f..b1124c6 100644 --- a/mendel/dda.h +++ b/mendel/dda.h @@ -25,7 +25,6 @@ typedef struct { uint8_t f_direction :1; uint8_t nullmove :1; uint8_t live :1; - uint8_t firstep :1; uint32_t x_delta; uint32_t y_delta; @@ -41,7 +40,6 @@ typedef struct { uint32_t total_steps; -// uint16_t f_scale; uint32_t move_duration; } DDA; diff --git a/mendel/gcode.c b/mendel/gcode.c index 2f1a7d4..b3d1d8a 100644 --- a/mendel/gcode.c +++ b/mendel/gcode.c @@ -14,6 +14,8 @@ decfloat read_digit; const char alphabet[] = "GMXYZEFSP"; +GCODE_COMMAND next_target = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, { 0, 0, 0, 0, 0 } }; + /* utility functions */ @@ -91,7 +93,6 @@ void SpecialMoveE(int32_t e, uint32_t f) { void scan_char(uint8_t c) { static uint8_t last_field = 0; - static GCODE_COMMAND next_target = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, { 0, 0, 0, 0, 0 } }; // uppercase if (c >= 'a' && c <= 'z') @@ -141,7 +142,7 @@ void scan_char(uint8_t c) { serwrite_uint16(next_target.S); break; case 'P': - // if this is dwell, multiply by 1 million to convert seconds to milliseconds + // if this is dwell, multiply by 1000 to convert seconds to milliseconds if (next_target.G == 4) next_target.P = decfloat_to_int(&read_digit, 1000, 1); else diff --git a/mendel/gcode.h b/mendel/gcode.h index 29aab4b..72ec269 100644 --- a/mendel/gcode.h +++ b/mendel/gcode.h @@ -36,6 +36,8 @@ typedef struct { uint16_t P; } GCODE_COMMAND; +extern GCODE_COMMAND next_target; + int8_t indexof(uint8_t c, const char *string); int32_t decfloat_to_int(decfloat *df, int32_t multiplicand, int32_t denominator); diff --git a/mendel/mendel.c b/mendel/mendel.c index 9494c08..cfde75f 100644 --- a/mendel/mendel.c +++ b/mendel/mendel.c @@ -36,15 +36,15 @@ inline void io_init(void) { WRITE(E_DIR_PIN, 0); SET_OUTPUT(E_DIR_PIN); #ifdef HEATER_PIN - disable_heater(); SET_OUTPUT(HEATER_PIN); + disable_heater(); #endif #ifdef FAN_PIN - disable_fan(); SET_OUTPUT(FAN_PIN); + disable_fan(); #endif #ifdef STEPPER_ENABLE_PIN - disable_steppers(); SET_OUTPUT(STEPPER_ENABLE_PIN); + disable_steppers(); #endif WRITE(SCK, 1); SET_OUTPUT(SCK); @@ -65,6 +65,13 @@ inline void init(void) { // set up clock clock_setup(); + // set up variables + + // slow default + current_position.F = FEEDRATE_SLOW_Z; + memcpy(&startpoint, ¤t_position, sizeof(TARGET)); + memcpy(&(next_target.target), ¤t_position, sizeof(TARGET)); + // enable interrupts sei(); diff --git a/mendel/pinout.h b/mendel/pinout.h index 0465ffd..0deee2b 100644 --- a/mendel/pinout.h +++ b/mendel/pinout.h @@ -107,7 +107,7 @@ */ #define enable_heater() WRITE(HEATER_PIN, 1) -#define disable_heater() WRITE(HEATER_PIN, 0) +#define disable_heater() do { WRITE(HEATER_PIN, 0); SET_OUTPUT(HEATER_PIN); } while (0) /* fan @@ -115,7 +115,7 @@ #ifdef FAN_PIN #define enable_fan() WRITE(FAN_PIN, 1) - #define disable_fan() WRITE(FAN_PIN, 0) + #define disable_fan() do { WRITE(FAN_PIN, 0); SET_OUTPUT(FAN_PIN); } while (0) #else #define enable_fan() if (0) {} #define disable_fan() if (0) {} @@ -126,8 +126,12 @@ */ #ifdef STEPPER_ENABLE_PIN - #define enable_steppers() WRITE(STEPPER_ENABLE_PIN, 0) - #define disable_steppers() WRITE(STEPPER_ENABLE_PIN, 1) + // for connection to stepper driver ENABLE pins +// #define enable_steppers() WRITE(STEPPER_ENABLE_PIN, 0) +// #define disable_steppers() WRITE(STEPPER_ENABLE_PIN, 1) + // for connection to ATX PSU PWR_ON signal + #define enable_steppers() do { WRITE(STEPPER_ENABLE_PIN, 0); SET_OUTPUT(STEPPER_ENABLE_PIN); } while (0) + #define disable_steppers() SET_INPUT(STEPPER_ENABLE_PIN) #else #define enable_steppers() if (0) {} #define disable_steppers() if (0) {} diff --git a/mendel/serial.c b/mendel/serial.c index b21a910..35e9f1d 100644 --- a/mendel/serial.c +++ b/mendel/serial.c @@ -1,6 +1,7 @@ #include "serial.h" #include "ringbuffer.h" +#include "arduino.h" #define BUFSIZE 64 + sizeof(ringbuffer) #define BAUD 57600 @@ -60,8 +61,19 @@ uint16_t serial_recvblock(uint8_t *block, int blocksize) void serial_writechar(uint8_t data) { - for (;ringbuffer_canwrite(tx_buffer) == 0;); - ringbuffer_writechar(tx_buffer, data); + // check if interrupts are enabled + if (SREG & MASK(SREG_I)) { + // if they are, we should be ok to block + for (;ringbuffer_canwrite(tx_buffer) == 0;); + ringbuffer_writechar(tx_buffer, data); + } + else { + // interrupts are disabled- maybe we're in one? + // anyway, instead of blocking, only write if we have room + if (ringbuffer_canwrite(tx_buffer)) + ringbuffer_writechar(tx_buffer, data); + } + // enable TX interrupt so we can send this character UCSR0B |= (1 << UDRIE0); } diff --git a/mendel/timer.c b/mendel/timer.c index 5795b55..484e56a 100644 --- a/mendel/timer.c +++ b/mendel/timer.c @@ -6,22 +6,25 @@ #include "dda.h" ISR(TIMER1_COMPA_vect) { - if(movebuffer[mb_tail].live) { +// WRITE(SCK, 0); + if (movebuffer[mb_tail].live) { // this interrupt can be interruptible // TODO: remove when not debugging // disableTimerInterrupt(); // sei(); -// WRITE(SCK, 0); dda_step(&(movebuffer[mb_tail])); -// WRITE(SCK, 1); // cli(); // enableTimerInterrupt(); } - else { +// WRITE(SCK, 1); + + // perhaps we can fall directly into dda_start instead of waiting for another step + if (movebuffer[mb_tail].live == 0) { next_move(); } + } void setupTimerInterrupt()