diff --git a/arduino_32U4.h b/arduino_32U4.h index b160aea..aea073c 100644 --- a/arduino_32U4.h +++ b/arduino_32U4.h @@ -299,6 +299,13 @@ pins // The PWM // Port A isn't defined +#undef PA4 +#define PA4_PIN PINA4 +#define PA4_RPORT PINA +#define PA4_WPORT PORTA +#define PA4_PWM NULL +#define PA4_DDR DDRA + #undef PA5 #define PA5_PIN PINA5 #define PA5_RPORT PINA diff --git a/config.h b/config.h index 77baf70..cf8735c 100644 --- a/config.h +++ b/config.h @@ -75,8 +75,8 @@ /** \def MAXIMUM_FEEDRATE_X MAXIMUM_FEEDRATE_Y MAXIMUM_FEEDRATE_Z MAXIMUM_FEEDRATE_E Used for G0 rapid moves and as a cap for all other feedrates. (mm / min) */ -#define MAXIMUM_FEEDRATE_X 36000 -#define MAXIMUM_FEEDRATE_Y 36000 +#define MAXIMUM_FEEDRATE_X 33000 +#define MAXIMUM_FEEDRATE_Y 33000 /*#define MAXIMUM_FEEDRATE_Z 6000 #define MAXIMUM_FEEDRATE_E 20000 */ @@ -110,7 +110,8 @@ Sane values: 0 to 20000 (0 to 20 mm) Valid range: 0 to 1000000 */ -#define ENDSTOP_CLEARANCE 2000 +#define ENDSTOP_CLEARANCE_X 100 +#define ENDSTOP_CLEARANCE_Y 100 /** \def ENDSTOP_STEPS Number of steps to run into the endstops intentionally. As endstops trigger @@ -129,6 +130,11 @@ */ #define MILD_HOMING +/** \def TRIGGERED_MOVEMENT + Define this to start new G1 movement only by external interrupt from additional sensor switch. +*/ +#define TRIGGERED_MOVEMENT + /** \def LOOKAHEAD Define this to enable look-ahead during *ramping* acceleration to smoothly @@ -242,14 +248,16 @@ #define E_INVERT_ENABLE */ -#define ENCODER_PIN PD3 +#define ENCODER_PIN_A PD3 +#define ENCODER_PIN_B PA4 +#define INVERT_DIRECTION_ENCODER #define PWR_OUT1_PIN 18 #define PWR_OUT2_PIN 19 #define ANALOG_IN_PIN 33 #define SWITCH1_PIN 34 #define SWITCH2_PIN 35 -#define SWITCH3_PIN 16 +#define SWITCH3_PIN PD2 #endif @@ -322,9 +330,5 @@ extern volatile uint8_t debug_flags; #define BSS __attribute__ ((__section__ (".bss"))) #endif -#ifdef ENDSTOP_CLEARANCE - #define SEARCH_FAST (uint32_t)((double)60. * \ - sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE / 1000.)) -#endif -#endif /* _CONFIG_H */ \ No newline at end of file +#endif /* _CONFIG_H */ diff --git a/gcode_parser.c b/gcode_parser.c index 1aaf813..e6e6035 100644 --- a/gcode_parser.c +++ b/gcode_parser.c @@ -1,6 +1,7 @@ #include "queue.h" #include "motor.h" #include "pinio.h" +#include "homing.h" #include "serial.h" #include "gcode_parser.h" @@ -105,21 +106,21 @@ uint8_t process_command() switch(gcode_params[0].value) { case 0: - case 1: - //? Example: G1 + //? Example: G0 //? //? Linear move enqueue(&next_target); break; + case 1: + //? Example: G1 + //? + //? Linear move with tool down + enqueue_home(&next_target, 0, 0xf0); break; case 28: //? Example: G28 //? - //? home axis, y only - next_target.axis[X] = next_target.axis[Y] = 0; - home_pos_x(); - home_pos_y(); - // just set X axis pos as zero - current_position.axis[X] = startpoint.axis[X] = 0; - dda_new_startpoint(); + //? home all axis + queue_wait(); // wait for queue to empty + home(); break; case 90: //? Example: G90 @@ -315,60 +316,3 @@ uint8_t gcode_parse_char(uint8_t c) { } return 0; } - -/// find Y MIN endstop position -void home_pos_y() { - #if defined Y_MIN_PIN - TARGET t = startpoint; - - t.axis[Y] = -1000000; - if (SEARCH_FAST > SEARCH_FEEDRATE_Y) - t.F = SEARCH_FAST; - else - t.F = SEARCH_FEEDRATE_Y; - enqueue_home(&t, 0x04, 1); // 04 is magic for y min endstp check - - if (SEARCH_FAST > SEARCH_FEEDRATE_Y) { - t.axis[Y] = +1000000; - t.F = SEARCH_FEEDRATE_Y; - enqueue_home(&t, 0x04, 0); - } - - // set Y home - //queue_wait(); - #ifdef Y_MIN - startpoint.axis[Y] = next_target.axis[Y] = (int32_t)(Y_MIN * 1000.); - #else - startpoint.axis[Y] = next_target.axis[Y] = 0; - #endif - dda_new_startpoint(); - #endif -} -/// find X MIN endstop position -void home_pos_x() { - #if defined X_MIN_PIN - TARGET t = startpoint; - - t.axis[Y] = -1000000; - if (SEARCH_FAST > SEARCH_FEEDRATE_X) - t.F = SEARCH_FAST; - else - t.F = SEARCH_FEEDRATE_X; - enqueue_home(&t, 0x01, 1); // 01 is magic for x min endstp check - - if (SEARCH_FAST > SEARCH_FEEDRATE_X) { - t.axis[Y] = +1000000; - t.F = SEARCH_FEEDRATE_X; - enqueue_home(&t, 0x01, 0); - } - - // set Y home - //queue_wait(); - #ifdef X_MIN - startpoint.axis[X] = next_target.axis[X] = (int32_t)(X_MIN * 1000.); - #else - startpoint.axis[X] = next_target.axis[X] = 0; - #endif - dda_new_startpoint(); - #endif -} \ No newline at end of file diff --git a/gcode_parser.h b/gcode_parser.h index 252469b..dd647ed 100644 --- a/gcode_parser.h +++ b/gcode_parser.h @@ -18,7 +18,6 @@ typedef struct { uint8_t is_negative; } GCODE_PARAM; -extern GCODE_PARAM gcode_params[8]; extern TARGET next_target; //a few state functions prototypes @@ -42,8 +41,5 @@ void parser_init(); } #endif -// help function, home axis position by hitting endstop -void home_pos_x(); -void home_pos_y(); -#endif /* _GCODE_PARSE_H */ \ No newline at end of file +#endif /* _GCODE_PARSE_H */ diff --git a/homing.c b/homing.c new file mode 100644 index 0000000..1031224 --- /dev/null +++ b/homing.c @@ -0,0 +1,180 @@ +#include "homing.h" +#include "config.h" +#include "maths.h" +#include "queue.h" +#include "motor.h" +#include "gcode_parser.h" + +// Check configuration. +#if defined X_MIN_PIN || defined X_MAX_PIN + #ifndef SEARCH_FEEDRATE_X + #error SEARCH_FEEDRATE_X undefined. It should be defined in config.h. + #endif + #ifndef ENDSTOP_CLEARANCE_X + #error ENDSTOP_CLEARANCE_X undefined. It should be defined in config.h. + #endif +#endif +#if defined Y_MIN_PIN || defined Y_MAX_PIN + #ifndef SEARCH_FEEDRATE_Y + #error SEARCH_FEEDRATE_Y undefined. It should be defined in config.h. + #endif + #ifndef ENDSTOP_CLEARANCE_Y + #error ENDSTOP_CLEARANCE_Y undefined. It should be defined in config.h. + #endif +#endif +#if defined Z_MIN_PIN || defined Z_MAX_PIN + #ifndef SEARCH_FEEDRATE_Z + #error SEARCH_FEEDRATE_Z undefined. It should be defined in config.h. + #endif + #ifndef ENDSTOP_CLEARANCE_Z + #error ENDSTOP_CLEARANCE_Z undefined. It should be defined in config.h. + #endif +#endif + +// Calculate feedrates according to clearance and deceleration. +// For a description, see #define ENDSTOP_CLEARANCE_{XYZ} in config.h. +// s = 1/2 * a * t^2; t = v / a <==> v = sqrt(2 * a * s)) +// units: / 1000 for um -> mm; * 60 for mm/s -> mm/min +#ifdef ENDSTOP_CLEARANCE_X + #define SEARCH_FAST_X (uint32_t)((double)60. * \ + sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_X / 1000.)) +#endif +#ifdef ENDSTOP_CLEARANCE_Y + #define SEARCH_FAST_Y (uint32_t)((double)60. * \ + sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_Y / 1000.)) +#endif +#ifdef ENDSTOP_CLEARANCE_Z + #define SEARCH_FAST_Z (uint32_t)((double)60. * \ + sqrt((double)2 * ACCELERATION * ENDSTOP_CLEARANCE_Z / 1000.)) +#endif + +static const uint32_t PROGMEM fast_feedrate_P[3] = { + (SEARCH_FAST_X > SEARCH_FEEDRATE_X) ? SEARCH_FAST_X : SEARCH_FEEDRATE_X, + (SEARCH_FAST_Y > SEARCH_FEEDRATE_Y) ? SEARCH_FAST_Y : SEARCH_FEEDRATE_Y, + #ifdef SEARCH_FAST_Z + (SEARCH_FAST_Z > SEARCH_FEEDRATE_Z) ? SEARCH_FAST_Z : SEARCH_FEEDRATE_Z, + #endif +}; + +static const uint32_t PROGMEM search_feedrate_P[3] = { + (SEARCH_FAST_X > SEARCH_FEEDRATE_X) ? SEARCH_FEEDRATE_X : 0, + (SEARCH_FAST_Y > SEARCH_FEEDRATE_Y) ? SEARCH_FEEDRATE_Y : 0, + #ifdef SEARCH_FAST_Z + (SEARCH_FAST_Z > SEARCH_FEEDRATE_Z) ? SEARCH_FEEDRATE_Z : 0, + #endif +}; + + +uint8_t get_endstop_check(enum axis_e n, int8_t dir); +void home_axis(enum axis_e n, int8_t dir, enum axis_endstop_e endstop_check); +void set_axis_home_position(enum axis_e n, int8_t dir); + + + +void home() { + /// find X MIN endstop + #if defined X_MIN_PIN + home_axis(X, -1, X_MIN_ENDSTOP); + #endif + + /// find X_MAX endstop + #if defined X_MAX_PIN && ! defined X_MAX + #warning X_MAX_PIN defined, but not X_MAX. home_x_positive() disabled. + #endif + #if defined X_MAX_PIN && defined X_MAX + home_axis(X, 1, X_MAX_ENDSTOP); + #endif + + /// fund Y MIN endstop + #if defined Y_MIN_PIN + home_axis(Y, -1, Y_MIN_ENDSTOP); + #endif + + /// find Y MAX endstop + #if defined Y_MAX_PIN && ! defined Y_MAX + #warning Y_MAX_PIN defined, but not Y_MAX. home_y_positive() disabled. + #endif + #if defined Y_MAX_PIN && defined Y_MAX + home_axis(Y, 1, Y_MAX_ENDSTOP); + #endif + + /// find Z MIN endstop + #if defined Z_MIN_PIN + home_axis(Z, -1, Z_MIN_ENDSTOP); + #endif + + /// find Z MAX endstop + #if defined Z_MAX_PIN && ! defined Z_MAX + #warning Z_MAX_PIN defined, but not Z_MAX. home_z_positive() disabled. + #endif + #if defined Z_MAX_PIN && defined Z_MAX + home_axis(Z, 1, Z_MAX_ENDSTOP); + #endif +} + + +void home_axis(enum axis_e n, int8_t dir, enum axis_endstop_e endstop_check) { + TARGET t = startpoint; + startpoint.axis[n] = 0; + + t.f_multiplier = 64; + t.axis[n] = dir * MAX_DELTA_UM; + t.F = pgm_read_dword(&fast_feedrate_P[n]); + enqueue_home(&t, endstop_check, 1); + + uint32_t search_feedrate; + search_feedrate = pgm_read_dword(&search_feedrate_P[n]); + if (search_feedrate) { + // back off slowly + t.axis[n] = 0; + t.F = search_feedrate; + enqueue_home(&t, endstop_check, 0); + } + + queue_wait(); + set_axis_home_position(n, dir); + dda_new_startpoint(); +} + +void set_axis_home_position(enum axis_e n, int8_t dir) { + int32_t home_position = 0; + if (dir < 0) { + if (n == X) { + #ifdef X_MIN + home_position = (int32_t)(X_MIN * 1000); + #endif + } + else if (n == Y) { + #ifdef Y_MIN + home_position = (int32_t)(Y_MIN * 1000); + #endif + } + #ifdef SEARCH_FAST_Z + else if (n == Z) { + #ifdef Z_MIN + home_position = (int32_t)(Z_MIN * 1000); + #endif + } + #endif + } + else { + if (n == X) { + #ifdef X_MAX + home_position = (int32_t)(X_MAX * 1000); + #endif + } + else if (n == Y) { + #ifdef Y_MAX + home_position = (int32_t)(Y_MAX * 1000); + #endif + } + #ifdef SEARCH_FAST_Z + else if (n == Z) { + #ifdef Z_MAX + home_position = (int32_t)(Z_MAX * 1000); + #endif + } + #endif + } + startpoint.axis[n] = next_target.axis[n] = home_position; +} diff --git a/homing.h b/homing.h new file mode 100644 index 0000000..0e64916 --- /dev/null +++ b/homing.h @@ -0,0 +1,23 @@ +#ifndef _HOMING_H +#define _HOMING_H + +void home(void); + +enum axis_endstop_e { + X_MIN_ENDSTOP = 0x01, + X_MAX_ENDSTOP = 0x02, + Y_MIN_ENDSTOP = 0x04, + Y_MAX_ENDSTOP = 0x08, + Z_MIN_ENDSTOP = 0x10, + Z_MAX_ENDSTOP = 0x20, +}; + +void home_none(void); +void home_x_negative(void); +void home_x_positive(void); +void home_y_negative(void); +void home_y_positive(void); +void home_z_negative(void); +void home_z_positive(void); + +#endif /* _HOME_H */ diff --git a/motor.c b/motor.c index 05e48c7..41353d2 100644 --- a/motor.c +++ b/motor.c @@ -149,9 +149,6 @@ void dda_create(DDA *dda, const TARGET *target) { prev_dda = NULL; #endif - if (dda->waitfor) - return; - // We end at the passed target. memcpy(&(dda->endpoint), target, sizeof(TARGET)); #ifdef STEPS_PER_M_Z @@ -407,7 +404,15 @@ void dda_create(DDA *dda, const TARGET *target) { void dda_start(DDA *dda) { // called from interrupt context: keep it simple! - + + if (dda->endstop_check) + endstops_on(); + #ifdef TRIGGERED_MOVEMENT + // if movement dda in not allowed yet + else if(dda->waitfor) + return; + #endif + if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) #ifdef STEPS_PER_M_Z sersendf_P(PSTR("Start: X %lq Y %lq Z %lq F %lu\n"), @@ -417,13 +422,10 @@ void dda_start(DDA *dda) { sersendf_P(PSTR("Start: X %lq Y %lq F %lu\n"), dda->endpoint.axis[X], dda->endpoint.axis[Y],dda->endpoint.F); #endif - + // get ready to go //psu_timeout = 0; - if (dda->endstop_check) - endstops_on(); - // set direction outputs x_direction(dda->x_direction); y_direction(dda->y_direction); @@ -610,13 +612,15 @@ void dda_clock() { // but start deceleration here. ATOMIC_START move_state.endstop_stop = 1; - if (move_state.step_no < dda->rampup_steps) // still accelerating - dda->total_steps = move_state.step_no * 2; - else - // A "-=" would overflow earlier. - dda->total_steps = dda->total_steps - dda->rampdown_steps + - move_state.step_no; - dda->rampdown_steps = move_state.step_no; + move_step_no = dda->total_steps - move_state.steps[dda->fast_axis]; + + if (move_step_no > dda->rampup_steps) { // cruising? + move_step_no = dda->total_steps - dda->rampdown_steps; + } + + dda->rampdown_steps = move_step_no; + dda->total_steps = move_step_no * 2; + move_state.steps[dda->fast_axis] = move_step_no; ATOMIC_END // Not atomic, because not used in dda_step(). @@ -752,4 +756,4 @@ void update_current_position() { #endif // current_position.F is updated in dda_start() } -} \ No newline at end of file +} diff --git a/motor.h b/motor.h index 9e35596..e802867 100644 --- a/motor.h +++ b/motor.h @@ -5,6 +5,9 @@ #include "config.h" +// The distance of any move and a single axis should never go above this limit. +// Check move_duration and c_limit calculations in dda.c +#define MAX_DELTA_UM ((int32_t)(UINT32_MAX / 2400L)) // Enum to denote an axis enum axis_e { X = 0, Y, diff --git a/msg.c b/msg.c index f8ba3f8..6ce1f47 100644 --- a/msg.c +++ b/msg.c @@ -42,7 +42,7 @@ void write_hex32(void (*writechar)(uint8_t), uint32_t v) { } /// list of powers of ten, used for dividing down decimal numbers for sending, and also for our crude floating point algorithm -extern const uint32_t powers[] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; +const uint32_t powers[] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; /** write decimal digits from a long unsigned int \param v number to send diff --git a/queue.c b/queue.c index a48a8ca..773d04e 100644 --- a/queue.c +++ b/queue.c @@ -105,6 +105,13 @@ void enqueue_home(TARGET const *t, uint8_t endstop_check, uint8_t endstop_stop_c new_movebuffer->endstop_check = endstop_check; new_movebuffer->endstop_stop_cond = endstop_stop_cond; + #ifdef TRIGGERED_MOVEMENT + // this dda is started by interrupt + if(endstop_stop_cond & 0xf0) + { + new_movebuffer->waitfor = 1; + } + #endif dda_create(new_movebuffer, t); /** @@ -157,12 +164,15 @@ void queue_flush() { mb_tail = mb_head; mb_tail_dda = NULL; } -/* + /// wait for queue to empty void queue_wait() { - while (mb_tail_dda) - clock(); + while (mb_tail_dda){ + delay_us(100); + // do recalc of another stuff + //clock(); + } } -*/ + diff --git a/queue.h b/queue.h index 60df761..5fd9947 100644 --- a/queue.h +++ b/queue.h @@ -6,11 +6,12 @@ extern uint_fast8_t mb_tail; extern DDA movebuffer[MOVEBUFFER_SIZE]; -extern DDA *mb_tail_dda; #ifdef __cplusplus extern "C" { #endif +extern DDA *mb_tail_dda; + // queue status methods uint8_t queue_full(void); uint8_t queue_empty(void); @@ -39,5 +40,6 @@ inline void enqueue(const TARGET *t) { enqueue_home(t, 0, 0); } +void queue_wait(); -#endif /* _QUEUE_H */ \ No newline at end of file +#endif /* _QUEUE_H */ diff --git a/sensors_control.c b/sensors_control.c new file mode 100644 index 0000000..884d926 --- /dev/null +++ b/sensors_control.c @@ -0,0 +1,54 @@ +#include "sensors_control.h" + +uint8_t last_direction = 0; + +// Init INT0 and INT1 interrupts for optical sensors +void sensing_init() +{ + // externally trigged interrupts setup + MCUCR |= (1<live) + { + mb_tail_dda->waitfor = 0; + dda_start(mb_tail_dda); + sei(); + } + } + else // needle goes downwards + { + if(!last_direction) + return; + //serial_writestr_P(PSTR("int1 down\n")); + last_direction = 0; + //TODO: stop any movement currently in progress + } +} + +// motor rotary encoder interrupt +ISR(INT0_vect) +{ + +} + + diff --git a/sensors_control.h b/sensors_control.h new file mode 100644 index 0000000..dffd058 --- /dev/null +++ b/sensors_control.h @@ -0,0 +1,19 @@ +#include +#include "queue.h" +#include "pinio.h" +#include "serial.h" +#include "config.h" + +#ifdef INVERT_DIRECTION_ENCODER + #define direction_encoder() (READ(ENCODER_PIN_B)?1:0) +#else + #define direction_encoder() (READ(ENCODER_PIN_B)?0:1) +#endif + +#ifdef __cplusplus +extern "C" { +#endif +void sensing_init(); + #ifdef __cplusplus +} +#endif diff --git a/teathimble.ino b/teathimble.ino index e1f6593..063dbf0 100644 --- a/teathimble.ino +++ b/teathimble.ino @@ -3,6 +3,7 @@ #include "timer.h" #include "serial.h" #include "pinio.h" +#include "sensors_control.h" #include "gcode_parser.h" uint8_t c, line_done, ack_waiting = 0; @@ -13,6 +14,7 @@ void setup(){ pinio_init(); timer_init(); dda_init(); + sensing_init(); parser_init(); sei(); serial_writestr_P(PSTR("start\nok\n")); @@ -43,4 +45,4 @@ void loop() } } } -#endif \ No newline at end of file +#endif diff --git a/teathimble.ino.c b/teathimble.ino.c index e7af497..3d60b3b 100644 --- a/teathimble.ino.c +++ b/teathimble.ino.c @@ -27,6 +27,7 @@ void setup(){ pinio_init(); timer_init(); dda_init(); + sensing_init(); parser_init(); sei(); serial_writestr_P(PSTR("start\nok\n")); @@ -57,4 +58,4 @@ void loop() } } } -#endif \ No newline at end of file +#endif diff --git a/timer.h b/timer.h index 012f3e4..b064dc9 100644 --- a/timer.h +++ b/timer.h @@ -94,7 +94,7 @@ void timer_stop(void); // Should be called every TICK_TIME (currently 2 ms). //void clock_tick(void); - +// Called in busy loops //void clock(void); // TIMER