|
|
@ -214,6 +214,12 @@ void st_wake_up() { |
|
|
|
ENABLE_STEPPER_DRIVER_INTERRUPT(); |
|
|
|
} |
|
|
|
|
|
|
|
void step_wait(){ |
|
|
|
for(int8_t i=0; i < 6; i++){ |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { |
|
|
|
unsigned short timer; |
|
|
|
if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; |
|
|
@ -317,8 +323,10 @@ ISR(TIMER1_COMPA_vect) |
|
|
|
out_bits = current_block->direction_bits; |
|
|
|
|
|
|
|
// Set direction en check limit switches
|
|
|
|
if ((out_bits & (1<<X_AXIS)) != 0) { // -direction
|
|
|
|
WRITE(X_DIR_PIN, INVERT_X_DIR); |
|
|
|
if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis
|
|
|
|
#if !defined COREXY //NOT COREXY
|
|
|
|
WRITE(X_DIR_PIN, INVERT_X_DIR); |
|
|
|
#endif |
|
|
|
count_direction[X_AXIS]=-1; |
|
|
|
CHECK_ENDSTOPS |
|
|
|
{ |
|
|
@ -333,8 +341,11 @@ ISR(TIMER1_COMPA_vect) |
|
|
|
#endif |
|
|
|
} |
|
|
|
} |
|
|
|
else { // +direction
|
|
|
|
WRITE(X_DIR_PIN,!INVERT_X_DIR); |
|
|
|
else { // +direction
|
|
|
|
#if !defined COREXY //NOT COREXY
|
|
|
|
WRITE(X_DIR_PIN,!INVERT_X_DIR); |
|
|
|
#endif |
|
|
|
|
|
|
|
count_direction[X_AXIS]=1; |
|
|
|
CHECK_ENDSTOPS |
|
|
|
{ |
|
|
@ -351,7 +362,9 @@ ISR(TIMER1_COMPA_vect) |
|
|
|
} |
|
|
|
|
|
|
|
if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
|
|
|
|
WRITE(Y_DIR_PIN,INVERT_Y_DIR); |
|
|
|
#if !defined COREXY //NOT COREXY
|
|
|
|
WRITE(Y_DIR_PIN,INVERT_Y_DIR); |
|
|
|
#endif |
|
|
|
count_direction[Y_AXIS]=-1; |
|
|
|
CHECK_ENDSTOPS |
|
|
|
{ |
|
|
@ -367,7 +380,9 @@ ISR(TIMER1_COMPA_vect) |
|
|
|
} |
|
|
|
} |
|
|
|
else { // +direction
|
|
|
|
WRITE(Y_DIR_PIN,!INVERT_Y_DIR); |
|
|
|
#if !defined COREXY //NOT COREXY
|
|
|
|
WRITE(Y_DIR_PIN,!INVERT_Y_DIR); |
|
|
|
#endif |
|
|
|
count_direction[Y_AXIS]=1; |
|
|
|
CHECK_ENDSTOPS |
|
|
|
{ |
|
|
@ -382,7 +397,28 @@ ISR(TIMER1_COMPA_vect) |
|
|
|
#endif |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#ifdef COREXY //coreXY kinematics defined
|
|
|
|
if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) == 0)){ //+X is major axis
|
|
|
|
WRITE(X_DIR_PIN, !INVERT_X_DIR); |
|
|
|
WRITE(Y_DIR_PIN, !INVERT_Y_DIR); |
|
|
|
} |
|
|
|
if((current_block->steps_x >= current_block->steps_y)&&((out_bits & (1<<X_AXIS)) != 0)){ //-X is major axis
|
|
|
|
WRITE(X_DIR_PIN, INVERT_X_DIR); |
|
|
|
WRITE(Y_DIR_PIN, INVERT_Y_DIR); |
|
|
|
} |
|
|
|
if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) == 0)){ //+Y is major axis
|
|
|
|
WRITE(X_DIR_PIN, !INVERT_X_DIR); |
|
|
|
WRITE(Y_DIR_PIN, INVERT_Y_DIR); |
|
|
|
} |
|
|
|
if((current_block->steps_y > current_block->steps_x)&&((out_bits & (1<<Y_AXIS)) != 0)){ //-Y is major axis
|
|
|
|
WRITE(X_DIR_PIN, INVERT_X_DIR); |
|
|
|
WRITE(Y_DIR_PIN, !INVERT_Y_DIR); |
|
|
|
} |
|
|
|
#endif //coreXY
|
|
|
|
|
|
|
|
|
|
|
|
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
|
|
|
|
WRITE(Z_DIR_PIN,INVERT_Z_DIR); |
|
|
|
count_direction[Z_AXIS]=-1; |
|
|
@ -446,23 +482,73 @@ ISR(TIMER1_COMPA_vect) |
|
|
|
} |
|
|
|
} |
|
|
|
#endif //ADVANCE
|
|
|
|
|
|
|
|
counter_x += current_block->steps_x; |
|
|
|
if (counter_x > 0) { |
|
|
|
WRITE(X_STEP_PIN, HIGH); |
|
|
|
counter_x -= current_block->step_event_count; |
|
|
|
WRITE(X_STEP_PIN, LOW); |
|
|
|
count_position[X_AXIS]+=count_direction[X_AXIS]; |
|
|
|
} |
|
|
|
|
|
|
|
counter_y += current_block->steps_y; |
|
|
|
if (counter_y > 0) { |
|
|
|
WRITE(Y_STEP_PIN, HIGH); |
|
|
|
counter_y -= current_block->step_event_count; |
|
|
|
WRITE(Y_STEP_PIN, LOW); |
|
|
|
count_position[Y_AXIS]+=count_direction[Y_AXIS]; |
|
|
|
} |
|
|
|
#if !defined COREXY |
|
|
|
counter_x += current_block->steps_x; |
|
|
|
if (counter_x > 0) { |
|
|
|
WRITE(X_STEP_PIN, HIGH); |
|
|
|
counter_x -= current_block->step_event_count; |
|
|
|
WRITE(X_STEP_PIN, LOW); |
|
|
|
count_position[X_AXIS]+=count_direction[X_AXIS]; |
|
|
|
} |
|
|
|
|
|
|
|
counter_y += current_block->steps_y; |
|
|
|
if (counter_y > 0) { |
|
|
|
WRITE(Y_STEP_PIN, HIGH); |
|
|
|
|
|
|
|
WRITE(Y_STEP_PIN, LOW); |
|
|
|
|
|
|
|
} |
|
|
|
#endif |
|
|
|
|
|
|
|
#ifdef COREXY |
|
|
|
counter_x += current_block->steps_x; |
|
|
|
counter_y += current_block->steps_y; |
|
|
|
|
|
|
|
if ((counter_x > 0)&&!(counter_y>0)){ //X step only
|
|
|
|
WRITE(X_STEP_PIN, HIGH); |
|
|
|
WRITE(Y_STEP_PIN, HIGH); |
|
|
|
counter_x -= current_block->step_event_count; |
|
|
|
WRITE(X_STEP_PIN, LOW); |
|
|
|
WRITE(Y_STEP_PIN, LOW); |
|
|
|
count_position[X_AXIS]+=count_direction[X_AXIS]; |
|
|
|
} |
|
|
|
|
|
|
|
if (!(counter_x > 0)&&(counter_y>0)){ //Y step only
|
|
|
|
WRITE(X_STEP_PIN, HIGH); |
|
|
|
WRITE(Y_STEP_PIN, HIGH); |
|
|
|
counter_y -= current_block->step_event_count; |
|
|
|
WRITE(X_STEP_PIN, LOW); |
|
|
|
WRITE(Y_STEP_PIN, LOW); |
|
|
|
count_position[Y_AXIS]+=count_direction[Y_AXIS]; |
|
|
|
} |
|
|
|
|
|
|
|
if ((counter_x > 0)&&(counter_y>0)){ //step in both axes
|
|
|
|
if (((out_bits & (1<<X_AXIS)) == 0)^((out_bits & (1<<Y_AXIS)) == 0)){ //X and Y in different directions
|
|
|
|
WRITE(Y_STEP_PIN, HIGH); |
|
|
|
counter_x -= current_block->step_event_count; |
|
|
|
WRITE(Y_STEP_PIN, LOW); |
|
|
|
step_wait(); |
|
|
|
count_position[X_AXIS]+=count_direction[X_AXIS]; |
|
|
|
count_position[Y_AXIS]+=count_direction[Y_AXIS]; |
|
|
|
WRITE(Y_STEP_PIN, HIGH); |
|
|
|
counter_y -= current_block->step_event_count; |
|
|
|
WRITE(Y_STEP_PIN, LOW); |
|
|
|
} |
|
|
|
else{ //X and Y in same direction
|
|
|
|
WRITE(X_STEP_PIN, HIGH); |
|
|
|
counter_x -= current_block->step_event_count; |
|
|
|
WRITE(X_STEP_PIN, LOW) ; |
|
|
|
step_wait(); |
|
|
|
count_position[X_AXIS]+=count_direction[X_AXIS]; |
|
|
|
count_position[Y_AXIS]+=count_direction[Y_AXIS]; |
|
|
|
WRITE(X_STEP_PIN, HIGH); |
|
|
|
counter_y -= current_block->step_event_count; |
|
|
|
WRITE(X_STEP_PIN, LOW); |
|
|
|
} |
|
|
|
} |
|
|
|
#endif //corexy
|
|
|
|
|
|
|
|
counter_z += current_block->steps_z; |
|
|
|
if (counter_z > 0) { |
|
|
|
WRITE(Z_STEP_PIN, HIGH); |
|
|
|