MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Moved corexy implementation from stepper to planner

Browse Source 
(Thanks iquizzle)
This commit is contained in:
Erik van der Zalm 2013-06-09 13:10:06 +02:00
parent c8dcc7c208
commit ff6fa09ecf
3 changed files with 62 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Marlin/Configuration.h
View File

@ -238,6 +238,11 @@ const bool Y_ENDSTOPS_INVERTING = true; // set to true to invert the logic of th
const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of the endstops. const bool Z_ENDSTOPS_INVERTING = true; // set to true to invert the logic of the endstops.
//#define DISABLE_MAX_ENDSTOPS //#define DISABLE_MAX_ENDSTOPS
// Disable max endstops for compatibility with endstop checking routine
#if defined(COREXY) && !defined(DISABLE_MAX_ENDSTOPS)
#define DISABLE_MAX_ENDSTOPS
#endif
// For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1 // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1
#define X_ENABLE_ON 0 #define X_ENABLE_ON 0
#define Y_ENABLE_ON 0 #define Y_ENABLE_ON 0

32
Marlin/planner.cpp
View File

@ -564,8 +564,16 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
block->busy = false; block->busy = false;
// Number of steps for each axis // Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); #ifndef COREXY
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); // default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= extrudemultiply; block->steps_e *= extrudemultiply;
@ -586,6 +594,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
// Compute direction bits for this block // Compute direction bits for this block
block->direction_bits = 0; block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS]) if (target[X_AXIS] < position[X_AXIS])
{ {
block->direction_bits |= (1<<X_AXIS); block->direction_bits |= (1<<X_AXIS);
@ -594,6 +603,16 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
{ {
block->direction_bits |= (1<<Y_AXIS); block->direction_bits |= (1<<Y_AXIS);
} }
#else
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS);
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS);
}
#endif
if (target[Z_AXIS] < position[Z_AXIS]) if (target[Z_AXIS] < position[Z_AXIS])
{ {
block->direction_bits |= (1<<Z_AXIS); block->direction_bits |= (1<<Z_AXIS);
@ -638,8 +657,13 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
} }
float delta_mm[4]; float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; #ifndef COREXY
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0; delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )

119
Marlin/stepper.cpp
View File

@ -345,12 +345,31 @@ ISR(TIMER1_COMPA_vect)
// Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
out_bits = current_block->direction_bits; out_bits = current_block->direction_bits;
// Set direction en check limit switches
if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
#if !defined COREXY //NOT COREXY if((out_bits & (1<<X_AXIS))!=0){
WRITE(X_DIR_PIN, INVERT_X_DIR); WRITE(X_DIR_PIN, INVERT_X_DIR);
#endif
count_direction[X_AXIS]=-1; count_direction[X_AXIS]=-1;
}
else{
WRITE(X_DIR_PIN, !INVERT_X_DIR);
count_direction[X_AXIS]=1;
}
if((out_bits & (1<<Y_AXIS))!=0){
WRITE(Y_DIR_PIN, INVERT_Y_DIR);
count_direction[Y_AXIS]=-1;
}
else{
WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
count_direction[Y_AXIS]=1;
}
// Set direction en check limit switches
#ifndef COREXY
if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis
#else
if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) { //-X occurs for -A and -B
#endif
CHECK_ENDSTOPS CHECK_ENDSTOPS
{ {
#if defined(X_MIN_PIN) && X_MIN_PIN > -1 #if defined(X_MIN_PIN) && X_MIN_PIN > -1
@ -365,11 +384,6 @@ ISR(TIMER1_COMPA_vect)
} }
} }
else { // +direction else { // +direction
#if !defined COREXY //NOT COREXY
WRITE(X_DIR_PIN,!INVERT_X_DIR);
#endif
count_direction[X_AXIS]=1;
CHECK_ENDSTOPS CHECK_ENDSTOPS
{ {
#if defined(X_MAX_PIN) && X_MAX_PIN > -1 #if defined(X_MAX_PIN) && X_MAX_PIN > -1
@ -384,11 +398,11 @@ ISR(TIMER1_COMPA_vect)
} }
} }
#ifndef COREXY
if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
#if !defined COREXY //NOT COREXY #else
WRITE(Y_DIR_PIN,INVERT_Y_DIR); if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) { // -Y occurs for -A and +B
#endif #endif
count_direction[Y_AXIS]=-1;
CHECK_ENDSTOPS CHECK_ENDSTOPS
{ {
#if defined(Y_MIN_PIN) && Y_MIN_PIN > -1 #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
@ -403,10 +417,6 @@ ISR(TIMER1_COMPA_vect)
} }
} }
else { // +direction else { // +direction
#if !defined COREXY //NOT COREXY
WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
#endif
count_direction[Y_AXIS]=1;
CHECK_ENDSTOPS CHECK_ENDSTOPS
{ {
#if defined(Y_MAX_PIN) && Y_MAX_PIN > -1 #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
@ -420,28 +430,7 @@ ISR(TIMER1_COMPA_vect)
#endif #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 if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
WRITE(Z_DIR_PIN,INVERT_Z_DIR); WRITE(Z_DIR_PIN,INVERT_Z_DIR);
@ -516,7 +505,6 @@ ISR(TIMER1_COMPA_vect)
} }
#endif //ADVANCE #endif //ADVANCE
#if !defined COREXY
counter_x += current_block->steps_x; counter_x += current_block->steps_x;
if (counter_x > 0) { if (counter_x > 0) {
WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
@ -532,56 +520,7 @@ ISR(TIMER1_COMPA_vect)
count_position[Y_AXIS]+=count_direction[Y_AXIS]; count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN); WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
} }
#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, !INVERT_X_STEP_PIN);
WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
counter_x -= current_block->step_event_count;
count_position[X_AXIS]+=count_direction[X_AXIS];
WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
}
if (!(counter_x > 0)&&(counter_y>0)){ //Y step only
WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
counter_y -= current_block->step_event_count;
count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
}
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, !INVERT_Y_STEP_PIN);
counter_x -= current_block->step_event_count;
WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
step_wait();
count_position[X_AXIS]+=count_direction[X_AXIS];
count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
counter_y -= current_block->step_event_count;
WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
}
else{ //X and Y in same direction
WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
counter_x -= current_block->step_event_count;
WRITE(X_STEP_PIN, INVERT_X_STEP_PIN) ;
step_wait();
count_position[X_AXIS]+=count_direction[X_AXIS];
count_position[Y_AXIS]+=count_direction[Y_AXIS];
WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
counter_y -= current_block->step_event_count;
WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
}
}
#endif //corexy
counter_z += current_block->steps_z; counter_z += current_block->steps_z;
if (counter_z > 0) { if (counter_z > 0) {
WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN); WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);