From 0773ea752d26a47ce6c1d21c3f2cffc9bfb48126 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Tue, 17 May 2011 20:30:27 +0200 Subject: Refactored do_xxx_step() functions in 1 do_step() function. This is the first of a series of commits to refactor Sprinter to use bresenham on all axis. Much of the inspiration comes from ScribbleJ fork. --- Tonokip_Firmware/Tonokip_Firmware.pde | 120 ++++++++++++++-------------------- 1 file changed, 48 insertions(+), 72 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index e193265..63babc5 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -58,7 +58,9 @@ //Stepper Movement Variables bool direction_x, direction_y, direction_z, direction_e; -unsigned long previous_micros = 0, previous_micros_x = 0, previous_micros_y = 0, previous_micros_z = 0, previous_micros_e = 0, previous_millis_heater, previous_millis_bed_heater; +const int STEP_PIN[NUM_AXIS] = {X_STEP_PIN, Y_STEP_PIN, Z_STEP_PIN, E_STEP_PIN}; +long axis_previous_micros[NUM_AXIS]; +unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take; #ifdef RAMP_ACCELERATION unsigned long max_x_interval = 100000000.0 / (min_units_per_second * x_steps_per_unit); @@ -83,7 +85,7 @@ boolean acceleration_enabled = false, accelerating = false; unsigned long interval; float destination_x = 0.0, destination_y = 0.0, destination_z = 0.0, destination_e = 0.0; float current_x = 0.0, current_y = 0.0, current_z = 0.0, current_e = 0.0; -long x_interval, y_interval, z_interval, e_interval; // for speed delay +long axis_interval[NUM_AXIS]; // for speed delay float feedrate = 1500, next_feedrate, z_feedrate, saved_feedrate; float time_for_move; long gcode_N, gcode_LastN; @@ -207,11 +209,9 @@ void setup() for(int i = 0; i < BUFSIZE; i++){ fromsd[i] = false; } + //Initialize Step Pins - if(X_STEP_PIN > -1) pinMode(X_STEP_PIN,OUTPUT); - if(Y_STEP_PIN > -1) pinMode(Y_STEP_PIN,OUTPUT); - if(Z_STEP_PIN > -1) pinMode(Z_STEP_PIN,OUTPUT); - if(E_STEP_PIN > -1) pinMode(E_STEP_PIN,OUTPUT); + for(int i=0; i < NUM_AXIS; i++) if(STEP_PIN[i] > -1) pinMode(STEP_PIN[i],OUTPUT); //Initialize Dir Pins if(X_DIR_PIN > -1) pinMode(X_DIR_PIN,OUTPUT); @@ -900,10 +900,10 @@ inline void prepare_move() time_for_move = max(time_for_move, Z_TIME_FOR_MOVE); if(time_for_move <= 0) time_for_move = max(time_for_move, E_TIME_FOR_MOVE); - if(x_steps_to_take) x_interval = time_for_move / x_steps_to_take * 100; - if(y_steps_to_take) y_interval = time_for_move / y_steps_to_take * 100; - if(z_steps_to_take) z_interval = time_for_move / z_steps_to_take * 100; - if(e_steps_to_take && (x_steps_to_take + y_steps_to_take <= 0) ) e_interval = time_for_move / e_steps_to_take * 100; + if(x_steps_to_take) axis_interval[0] = time_for_move / x_steps_to_take * 100; + if(y_steps_to_take) axis_interval[1] = time_for_move / y_steps_to_take * 100; + if(z_steps_to_take) axis_interval[2] = time_for_move / z_steps_to_take * 100; + if(e_steps_to_take && (x_steps_to_take + y_steps_to_take <= 0) ) axis_interval[3] = time_for_move / e_steps_to_take * 100; //#define DEBUGGING false #if 0 @@ -912,25 +912,25 @@ inline void prepare_move() Serial.print("current_x: "); Serial.println(current_x); Serial.print("x_steps_to_take: "); Serial.println(x_steps_to_take); Serial.print("X_TIME_FOR_MVE: "); Serial.println(X_TIME_FOR_MOVE); - Serial.print("x_interval: "); Serial.println(x_interval); + Serial.print("axis_interval[0]: "); Serial.println(axis_interval[0]); Serial.println(""); Serial.print("destination_y: "); Serial.println(destination_y); Serial.print("current_y: "); Serial.println(current_y); Serial.print("y_steps_to_take: "); Serial.println(y_steps_to_take); Serial.print("Y_TIME_FOR_MVE: "); Serial.println(Y_TIME_FOR_MOVE); - Serial.print("y_interval: "); Serial.println(y_interval); + Serial.print("axis_interval[1]: "); Serial.println(axis_interval[1]); Serial.println(""); Serial.print("destination_z: "); Serial.println(destination_z); Serial.print("current_z: "); Serial.println(current_z); Serial.print("z_steps_to_take: "); Serial.println(z_steps_to_take); Serial.print("Z_TIME_FOR_MVE: "); Serial.println(Z_TIME_FOR_MOVE); - Serial.print("z_interval: "); Serial.println(z_interval); + Serial.print("axis_interval[2]: "); Serial.println(axis_interval[2]); Serial.println(""); Serial.print("destination_e: "); Serial.println(destination_e); Serial.print("current_e: "); Serial.println(current_e); Serial.print("e_steps_to_take: "); Serial.println(e_steps_to_take); Serial.print("E_TIME_FOR_MVE: "); Serial.println(E_TIME_FOR_MOVE); - Serial.print("e_interval: "); Serial.println(e_interval); + Serial.print("axis_interval[3]: "); Serial.println(axis_interval[3]); Serial.println(""); } #endif @@ -961,8 +961,8 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. if(x_steps_remaining) enable_x(); if(y_steps_remaining) enable_y(); - if(z_steps_remaining) { enable_z(); do_z_step(); z_steps_remaining--; } - if(e_steps_remaining) { enable_e(); do_e_step(); e_steps_remaining--; } + if(z_steps_remaining) { enable_z(); do_step(2); z_steps_remaining--; } + if(e_steps_remaining) { enable_e(); do_step(3); e_steps_remaining--; } //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. unsigned int delta_x = x_steps_remaining; @@ -991,7 +991,7 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin //Do some Bresenham calculations depending on which axis will lead it. if(steep_y) { error_x = delta_y / 2; - interval = y_interval; + interval = axis_interval[1]; #ifdef RAMP_ACCELERATION max_interval = max_y_interval; if(e_steps_to_take > 0) steps_per_sqr_second = y_steps_per_sqr_second; @@ -1012,7 +1012,7 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin steps_to_take = delta_y; } else if (steep_x) { error_y = delta_x / 2; - interval = x_interval; + interval = axis_interval[0]; #ifdef RAMP_ACCELERATION max_interval = max_x_interval; if(e_steps_to_take > 0) steps_per_sqr_second = x_steps_per_sqr_second; @@ -1056,10 +1056,9 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin #endif unsigned long start_move_micros = micros(); - previous_micros_x = start_move_micros*100; - previous_micros_y = previous_micros_x; - previous_micros_z = previous_micros_x; - previous_micros_e = previous_micros_x; + for(int i = 0; i < NUM_AXIS; i++) { + axis_previous_micros[i] = start_move_micros * 100; + } //move until no more steps remain while(x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) { @@ -1130,15 +1129,15 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; if(steep_y) { - timediff = micros() * 100 - previous_micros_y; + timediff = micros() * 100 - axis_previous_micros[1]; while(timediff >= interval && y_steps_remaining > 0) { steps_done++; steps_remaining--; y_steps_remaining--; timediff -= interval; error_x = error_x - delta_x; - do_y_step(); + do_step(1); if(error_x < 0) { - do_x_step(); x_steps_remaining--; + do_step(0); x_steps_remaining--; error_x = error_x + delta_y; } #ifdef RAMP_ACCELERATION @@ -1152,15 +1151,15 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin #endif } } else if (steep_x) { - timediff=micros() * 100 - previous_micros_x; + timediff=micros() * 100 - axis_previous_micros[0]; while(timediff >= interval && x_steps_remaining>0) { steps_done++; steps_remaining--; x_steps_remaining--; timediff -= interval; error_y = error_y - delta_y; - do_x_step(); + do_step(0); if(error_y < 0) { - do_y_step(); y_steps_remaining--; + do_step(1); y_steps_remaining--; error_y = error_y + delta_x; } #ifdef RAMP_ACCELERATION @@ -1185,39 +1184,39 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin if(z_steps_remaining) { if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; - timediff = micros() * 100-previous_micros_z; - while(timediff >= z_interval && z_steps_remaining) { - do_z_step(); + timediff = micros() * 100-axis_previous_micros[2]; + while(timediff >= axis_interval[2] && z_steps_remaining) { + do_step(2); z_steps_remaining--; - timediff -= z_interval; + timediff -= axis_interval[2]; #ifdef STEP_DELAY_RATIO - if(timediff >= z_interval) delayMicroseconds(long_step_delay_ratio * z_interval / 10000); + if(timediff >= axis_interval[2]) delayMicroseconds(long_step_delay_ratio * axis_interval[2] / 10000); #endif #ifdef STEP_DELAY_MICROS - if(timediff >= z_interval) delayMicroseconds(STEP_DELAY_MICROS); + if(timediff >= axis_interval[2]) delayMicroseconds(STEP_DELAY_MICROS); #endif } } //If there are e steps remaining, check if e steps must be taken if(e_steps_remaining){ - if (x_steps_to_take + y_steps_to_take <= 0) timediff = micros()*100 - previous_micros_e; + if (x_steps_to_take + y_steps_to_take <= 0) timediff = micros()*100 - axis_previous_micros[3]; unsigned int final_e_steps_remaining = 0; if (steep_x && x_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * x_steps_remaining / x_steps_to_take; else if (steep_y && y_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * y_steps_remaining / y_steps_to_take; //If this move has X or Y steps, let E follow the Bresenham pace - if (final_e_steps_remaining > 0) while(e_steps_remaining > final_e_steps_remaining) { do_e_step(); e_steps_remaining--;} - else if (x_steps_to_take + y_steps_to_take > 0) while(e_steps_remaining) { do_e_step(); e_steps_remaining--;} + if (final_e_steps_remaining > 0) while(e_steps_remaining > final_e_steps_remaining) { do_step(3); e_steps_remaining--;} + else if (x_steps_to_take + y_steps_to_take > 0) while(e_steps_remaining) { do_step(3); e_steps_remaining--;} //Else, normally check if e steps must be taken - else while (timediff >= e_interval && e_steps_remaining) { - do_e_step(); + else while (timediff >= axis_interval[3] && e_steps_remaining) { + do_step(3); e_steps_remaining--; - timediff -= e_interval; + timediff -= axis_interval[3]; #ifdef STEP_DELAY_RATIO - if(timediff >= e_interval) delayMicroseconds(long_step_delay_ratio * e_interval / 10000); + if(timediff >= axis_interval[3]) delayMicroseconds(long_step_delay_ratio * axis_interval[3] / 10000); #endif #ifdef STEP_DELAY_MICROS - if(timediff >= e_interval) delayMicroseconds(STEP_DELAY_MICROS); + if(timediff >= axis_interval[3]) delayMicroseconds(STEP_DELAY_MICROS); #endif } } @@ -1240,36 +1239,13 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin } -inline void do_x_step() -{ - digitalWrite(X_STEP_PIN, HIGH); - previous_micros_x += interval; - //delayMicroseconds(3); - digitalWrite(X_STEP_PIN, LOW); -} - -inline void do_y_step() -{ - digitalWrite(Y_STEP_PIN, HIGH); - previous_micros_y += interval; - //delayMicroseconds(3); - digitalWrite(Y_STEP_PIN, LOW); -} - -inline void do_z_step() -{ - digitalWrite(Z_STEP_PIN, HIGH); - previous_micros_z += z_interval; - //delayMicroseconds(3); - digitalWrite(Z_STEP_PIN, LOW); -} - -inline void do_e_step() -{ - digitalWrite(E_STEP_PIN, HIGH); - previous_micros_e += e_interval; - //delayMicroseconds(3); - digitalWrite(E_STEP_PIN, LOW); +inline void do_step(int axis) { + digitalWrite(STEP_PIN[axis], HIGH); + //TODO: the following check is ugly and not the best thing to do here, but this will be sorted out more easily when all + // axis will be under Bresenham + if(axis < 2) axis_previous_micros[axis] += interval; + else axis_previous_micros[axis] += axis_interval[axis]; + digitalWrite(STEP_PIN[axis], LOW); } inline void disable_x() { if(X_ENABLE_PIN > -1) digitalWrite(X_ENABLE_PIN,!X_ENABLE_ON); } -- cgit v1.2.1 From ae56481873211746266c2ac93ffa6560f9122efa Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Tue, 17 May 2011 21:39:45 +0200 Subject: Fixed type of axis_previous_micros array to unsigned long --- Tonokip_Firmware/Tonokip_Firmware.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 63babc5..308746a 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -59,7 +59,7 @@ //Stepper Movement Variables bool direction_x, direction_y, direction_z, direction_e; const int STEP_PIN[NUM_AXIS] = {X_STEP_PIN, Y_STEP_PIN, Z_STEP_PIN, E_STEP_PIN}; -long axis_previous_micros[NUM_AXIS]; +unsigned long axis_previous_micros[NUM_AXIS]; unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take; #ifdef RAMP_ACCELERATION -- cgit v1.2.1 From 8060d4da56d0dc3928e787537d7b9e0ad4763d6b Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 02:33:48 +0200 Subject: Added possibility to print move time to serial, to help debugging --- Tonokip_Firmware/Tonokip_Firmware.pde | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 308746a..46a6ca9 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -934,8 +934,13 @@ inline void prepare_move() Serial.println(""); } #endif - + #ifdef PRINT_MOVE_TIME + unsigned long startmove = micros(); + #endif linear_move(x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take); // make the move + #ifdef PRINT_MOVE_TIME + Serial.println(micros()-startmove); + #endif } void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remaining, unsigned long z_steps_remaining, unsigned long e_steps_remaining) // make linear move with preset speeds and destinations, see G0 and G1 -- cgit v1.2.1 From 222f2e80820f4e5b8fdc40c3de99008ad2d4649b Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 02:56:38 +0200 Subject: Refactored linear_move() to take an array instead of single axis steps_to_take. This is needed to later integrate N bresenham in --- Tonokip_Firmware/Tonokip_Firmware.pde | 69 ++++++++++++++++++----------------- 1 file changed, 35 insertions(+), 34 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 46a6ca9..788db5f 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -937,13 +937,14 @@ inline void prepare_move() #ifdef PRINT_MOVE_TIME unsigned long startmove = micros(); #endif - linear_move(x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take); // make the move + unsigned long axis_steps_to_take[NUM_AXIS] = {x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take}; + linear_move(axis_steps_to_take); // make the move #ifdef PRINT_MOVE_TIME Serial.println(micros()-startmove); #endif } -void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remaining, unsigned long z_steps_remaining, unsigned long e_steps_remaining) // make linear move with preset speeds and destinations, see G0 and G1 +void linear_move(unsigned long axis_steps_remaining[]) // make linear move with preset speeds and destinations, see G0 and G1 { //Determine direction of movement if (destination_x > current_x) digitalWrite(X_DIR_PIN,!INVERT_X_DIR); @@ -955,26 +956,26 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin if (destination_e > current_e) digitalWrite(E_DIR_PIN,!INVERT_E_DIR); else digitalWrite(E_DIR_PIN,INVERT_E_DIR); - if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) x_steps_remaining=0; - if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) y_steps_remaining=0; - if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) z_steps_remaining=0; - if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) x_steps_remaining=0; - if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) y_steps_remaining=0; - if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) z_steps_remaining=0; + if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; + if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; + if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; + if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; + if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; + if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. - if(x_steps_remaining) enable_x(); - if(y_steps_remaining) enable_y(); - if(z_steps_remaining) { enable_z(); do_step(2); z_steps_remaining--; } - if(e_steps_remaining) { enable_e(); do_step(3); e_steps_remaining--; } + if(axis_steps_remaining[0]) enable_x(); + if(axis_steps_remaining[1]) enable_y(); + if(axis_steps_remaining[2]) { enable_z(); do_step(2); axis_steps_remaining[2]--; } + if(axis_steps_remaining[3]) { enable_e(); do_step(3); axis_steps_remaining[3]--; } //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. - unsigned int delta_x = x_steps_remaining; + unsigned int delta_x = axis_steps_remaining[0]; unsigned long x_interval_nanos; - unsigned int delta_y = y_steps_remaining; + unsigned int delta_y = axis_steps_remaining[1]; unsigned long y_interval_nanos; - unsigned int delta_z = z_steps_remaining; + unsigned int delta_z = axis_steps_remaining[2]; unsigned long z_interval_nanos; boolean steep_y = delta_y > delta_x;// && delta_y > delta_e && delta_y > delta_z; boolean steep_x = delta_x >= delta_y;// && delta_x > delta_e && delta_x > delta_z; @@ -1066,7 +1067,7 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin } //move until no more steps remain - while(x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) { + while(axis_steps_remaining[0] + axis_steps_remaining[1] + axis_steps_remaining[2] + axis_steps_remaining[3] > 0) { //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp manage_heater(); manage_inactivity(2); @@ -1128,21 +1129,21 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin #endif //If there are x or y steps remaining, perform Bresenham algorithm - if(x_steps_remaining || y_steps_remaining) { + if(axis_steps_remaining[0] || axis_steps_remaining[1]) { if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break; if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; if(steep_y) { timediff = micros() * 100 - axis_previous_micros[1]; - while(timediff >= interval && y_steps_remaining > 0) { + while(timediff >= interval && axis_steps_remaining[1] > 0) { steps_done++; steps_remaining--; - y_steps_remaining--; timediff -= interval; + axis_steps_remaining[1]--; timediff -= interval; error_x = error_x - delta_x; do_step(1); if(error_x < 0) { - do_step(0); x_steps_remaining--; + do_step(0); axis_steps_remaining[0]--; error_x = error_x + delta_y; } #ifdef RAMP_ACCELERATION @@ -1157,14 +1158,14 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin } } else if (steep_x) { timediff=micros() * 100 - axis_previous_micros[0]; - while(timediff >= interval && x_steps_remaining>0) { + while(timediff >= interval && axis_steps_remaining[0]>0) { steps_done++; steps_remaining--; - x_steps_remaining--; timediff -= interval; + axis_steps_remaining[0]--; timediff -= interval; error_y = error_y - delta_y; do_step(0); if(error_y < 0) { - do_step(1); y_steps_remaining--; + do_step(1); axis_steps_remaining[1]--; error_y = error_y + delta_x; } #ifdef RAMP_ACCELERATION @@ -1180,19 +1181,19 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin } } #ifdef RAMP_ACCELERATION - if((x_steps_remaining>0 || y_steps_remaining>0) && + if((axis_steps_remaining[0]>0 || axis_steps_remaining[1]>0) && steps_to_take > 0 && (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating))) continue; #endif //If there are z steps remaining, check if z steps must be taken - if(z_steps_remaining) { + if(axis_steps_remaining[2]) { if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; timediff = micros() * 100-axis_previous_micros[2]; - while(timediff >= axis_interval[2] && z_steps_remaining) { + while(timediff >= axis_interval[2] && axis_steps_remaining[2]) { do_step(2); - z_steps_remaining--; + axis_steps_remaining[2]--; timediff -= axis_interval[2]; #ifdef STEP_DELAY_RATIO if(timediff >= axis_interval[2]) delayMicroseconds(long_step_delay_ratio * axis_interval[2] / 10000); @@ -1204,18 +1205,18 @@ void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remainin } //If there are e steps remaining, check if e steps must be taken - if(e_steps_remaining){ + if(axis_steps_remaining[3]){ if (x_steps_to_take + y_steps_to_take <= 0) timediff = micros()*100 - axis_previous_micros[3]; unsigned int final_e_steps_remaining = 0; - if (steep_x && x_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * x_steps_remaining / x_steps_to_take; - else if (steep_y && y_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * y_steps_remaining / y_steps_to_take; + if (steep_x && x_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[0] / x_steps_to_take; + else if (steep_y && y_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[1] / y_steps_to_take; //If this move has X or Y steps, let E follow the Bresenham pace - if (final_e_steps_remaining > 0) while(e_steps_remaining > final_e_steps_remaining) { do_step(3); e_steps_remaining--;} - else if (x_steps_to_take + y_steps_to_take > 0) while(e_steps_remaining) { do_step(3); e_steps_remaining--;} + if (final_e_steps_remaining > 0) while(axis_steps_remaining[3] > final_e_steps_remaining) { do_step(3); axis_steps_remaining[3]--;} + else if (x_steps_to_take + y_steps_to_take > 0) while(axis_steps_remaining[3]) { do_step(3); axis_steps_remaining[3]--;} //Else, normally check if e steps must be taken - else while (timediff >= axis_interval[3] && e_steps_remaining) { + else while (timediff >= axis_interval[3] && axis_steps_remaining[3]) { do_step(3); - e_steps_remaining--; + axis_steps_remaining[3]--; timediff -= axis_interval[3]; #ifdef STEP_DELAY_RATIO if(timediff >= axis_interval[3]) delayMicroseconds(long_step_delay_ratio * axis_interval[3] / 10000); -- cgit v1.2.1 From 8b7c5a64c886826146311edc42319807e04b26ab Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 03:20:47 +0200 Subject: Refactored errors and deltas variable into array, needed for N bresenham implementation --- Tonokip_Firmware/Tonokip_Firmware.pde | 45 +++++++++++++++-------------------- 1 file changed, 19 insertions(+), 26 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 788db5f..0ac491d 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -971,18 +971,11 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(axis_steps_remaining[3]) { enable_e(); do_step(3); axis_steps_remaining[3]--; } //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. - unsigned int delta_x = axis_steps_remaining[0]; - unsigned long x_interval_nanos; - unsigned int delta_y = axis_steps_remaining[1]; - unsigned long y_interval_nanos; - unsigned int delta_z = axis_steps_remaining[2]; - unsigned long z_interval_nanos; - boolean steep_y = delta_y > delta_x;// && delta_y > delta_e && delta_y > delta_z; - boolean steep_x = delta_x >= delta_y;// && delta_x > delta_e && delta_x > delta_z; - //boolean steep_z = delta_z > delta_x && delta_z > delta_y && delta_z > delta_e; - int error_x; - int error_y; - int error_z; + unsigned int delta[NUM_AXIS] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes + boolean steep_y = delta[1] > delta[0];// && delta[1] > delta[3] && delta[1] > delta[2]; + boolean steep_x = delta[0] >= delta[1];// && delta[0] > delta[3] && delta[0] > delta[2]; + //boolean steep_z = delta[2] > delta[0] && delta[2] > delta[1] && delta[2] > delta[3]; + int axis_error[NUM_AXIS]; #ifdef RAMP_ACCELERATION long max_speed_steps_per_second; long min_speed_steps_per_second; @@ -996,7 +989,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //Do some Bresenham calculations depending on which axis will lead it. if(steep_y) { - error_x = delta_y / 2; + axis_error[0] = delta[1] / 2; interval = axis_interval[1]; #ifdef RAMP_ACCELERATION max_interval = max_y_interval; @@ -1010,14 +1003,14 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #ifdef EXP_ACCELERATION if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * y_steps_per_unit /100; else virtual_full_velocity_steps = long_travel_move_full_velocity_units * y_steps_per_unit /100; - full_velocity_steps = min(virtual_full_velocity_steps, (delta_y - y_min_constant_speed_steps) / 2); + full_velocity_steps = min(virtual_full_velocity_steps, (delta[1] - y_min_constant_speed_steps) / 2); max_interval = max_y_interval; min_constant_speed_steps = y_min_constant_speed_steps; #endif - steps_remaining = delta_y; - steps_to_take = delta_y; + steps_remaining = delta[1]; + steps_to_take = delta[1]; } else if (steep_x) { - error_y = delta_x / 2; + axis_error[1] = delta[0] / 2; interval = axis_interval[0]; #ifdef RAMP_ACCELERATION max_interval = max_x_interval; @@ -1031,12 +1024,12 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #ifdef EXP_ACCELERATION if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * x_steps_per_unit /100; else virtual_full_velocity_steps = long_travel_move_full_velocity_units * x_steps_per_unit /100; - full_velocity_steps = min(virtual_full_velocity_steps, (delta_x - x_min_constant_speed_steps) / 2); + full_velocity_steps = min(virtual_full_velocity_steps, (delta[0] - x_min_constant_speed_steps) / 2); max_interval = max_x_interval; min_constant_speed_steps = x_min_constant_speed_steps; #endif - steps_remaining = delta_x; - steps_to_take = delta_x; + steps_remaining = delta[0]; + steps_to_take = delta[0]; } unsigned long steps_done = 0; #ifdef RAMP_ACCELERATION @@ -1140,11 +1133,11 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with steps_done++; steps_remaining--; axis_steps_remaining[1]--; timediff -= interval; - error_x = error_x - delta_x; + axis_error[0] = axis_error[0] - delta[0]; do_step(1); - if(error_x < 0) { + if(axis_error[0] < 0) { do_step(0); axis_steps_remaining[0]--; - error_x = error_x + delta_y; + axis_error[0] = axis_error[0] + delta[1]; } #ifdef RAMP_ACCELERATION if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; @@ -1162,11 +1155,11 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with steps_done++; steps_remaining--; axis_steps_remaining[0]--; timediff -= interval; - error_y = error_y - delta_y; + axis_error[1] = axis_error[1] - delta[1]; do_step(0); - if(error_y < 0) { + if(axis_error[1] < 0) { do_step(1); axis_steps_remaining[1]--; - error_y = error_y + delta_x; + axis_error[1] = axis_error[1] + delta[0]; } #ifdef RAMP_ACCELERATION if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; -- cgit v1.2.1 From 6d2fdf16b6f61a9aa7d58509073ce82c8033f65c Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 03:50:59 +0200 Subject: N Bresenham is ready for constant speed, though we still enforce only X and Y use it --- Tonokip_Firmware/Tonokip_Firmware.pde | 81 ++++++++++++----------------------- 1 file changed, 28 insertions(+), 53 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 0ac491d..f281e9e 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -976,6 +976,9 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with boolean steep_x = delta[0] >= delta[1];// && delta[0] > delta[3] && delta[0] > delta[2]; //boolean steep_z = delta[2] > delta[0] && delta[2] > delta[1] && delta[2] > delta[3]; int axis_error[NUM_AXIS]; + unsigned int primary_axis; + if(steep_x) primary_axis = 0; + else primary_axis = 1; #ifdef RAMP_ACCELERATION long max_speed_steps_per_second; long min_speed_steps_per_second; @@ -984,13 +987,13 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with unsigned long virtual_full_velocity_steps; unsigned long full_velocity_steps; #endif - unsigned long steps_remaining; - unsigned long steps_to_take; - + unsigned long steps_remaining = delta[primary_axis]; + unsigned long steps_to_take = steps_remaining; + for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis) axis_error[i] = delta[primary_axis] / 2; + interval = axis_interval[primary_axis]; + //Do some Bresenham calculations depending on which axis will lead it. if(steep_y) { - axis_error[0] = delta[1] / 2; - interval = axis_interval[1]; #ifdef RAMP_ACCELERATION max_interval = max_y_interval; if(e_steps_to_take > 0) steps_per_sqr_second = y_steps_per_sqr_second; @@ -1007,11 +1010,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with max_interval = max_y_interval; min_constant_speed_steps = y_min_constant_speed_steps; #endif - steps_remaining = delta[1]; - steps_to_take = delta[1]; } else if (steep_x) { - axis_error[1] = delta[0] / 2; - interval = axis_interval[0]; #ifdef RAMP_ACCELERATION max_interval = max_x_interval; if(e_steps_to_take > 0) steps_per_sqr_second = x_steps_per_sqr_second; @@ -1028,8 +1027,6 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with max_interval = max_x_interval; min_constant_speed_steps = x_min_constant_speed_steps; #endif - steps_remaining = delta[0]; - steps_to_take = delta[0]; } unsigned long steps_done = 0; #ifdef RAMP_ACCELERATION @@ -1127,50 +1124,28 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; - if(steep_y) { - timediff = micros() * 100 - axis_previous_micros[1]; - while(timediff >= interval && axis_steps_remaining[1] > 0) { - steps_done++; - steps_remaining--; - axis_steps_remaining[1]--; timediff -= interval; - axis_error[0] = axis_error[0] - delta[0]; - do_step(1); - if(axis_error[0] < 0) { - do_step(0); axis_steps_remaining[0]--; - axis_error[0] = axis_error[0] + delta[1]; + timediff = micros() * 100 - axis_previous_micros[primary_axis]; + while(timediff >= interval && axis_steps_remaining[primary_axis] > 0) { + steps_done++; + steps_remaining--; + axis_steps_remaining[primary_axis]--; timediff -= interval; + do_step(primary_axis); + for(int i=0; i < 2; i++) if(i != primary_axis) {//TODO change "2" to NUM_AXIS when other axes gets added to bresenham + axis_error[i] = axis_error[i] - delta[i]; + if(axis_error[i] < 0) { + do_step(i); axis_steps_remaining[i]--; + axis_error[i] = axis_error[i] + delta[primary_axis]; } - #ifdef RAMP_ACCELERATION - if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; - #endif - #ifdef STEP_DELAY_RATIO - if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000); - #endif - #ifdef STEP_DELAY_MICROS - if(timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS); - #endif - } - } else if (steep_x) { - timediff=micros() * 100 - axis_previous_micros[0]; - while(timediff >= interval && axis_steps_remaining[0]>0) { - steps_done++; - steps_remaining--; - axis_steps_remaining[0]--; timediff -= interval; - axis_error[1] = axis_error[1] - delta[1]; - do_step(0); - if(axis_error[1] < 0) { - do_step(1); axis_steps_remaining[1]--; - axis_error[1] = axis_error[1] + delta[0]; - } - #ifdef RAMP_ACCELERATION - if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; - #endif - #ifdef STEP_DELAY_RATIO - if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000); - #endif - #ifdef STEP_DELAY_MICROS - if(timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS); - #endif } + #ifdef RAMP_ACCELERATION + if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; + #endif + #ifdef STEP_DELAY_RATIO + if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000); + #endif + #ifdef STEP_DELAY_MICROS + if(timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS); + #endif } } #ifdef RAMP_ACCELERATION -- cgit v1.2.1 From 1b1e060bffc9c79d1783ca6d6d62347fb7230c1b Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 04:58:33 +0200 Subject: Refactored ramp and exp acceleration variables to arrays and changed ramp acceleration math code to be axis generic --- Tonokip_Firmware/Tonokip_Firmware.pde | 66 ++++++++++++++--------------------- 1 file changed, 27 insertions(+), 39 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index f281e9e..902c0e0 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -63,23 +63,19 @@ unsigned long axis_previous_micros[NUM_AXIS]; unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take; #ifdef RAMP_ACCELERATION - unsigned long max_x_interval = 100000000.0 / (min_units_per_second * x_steps_per_unit); - unsigned long max_y_interval = 100000000.0 / (min_units_per_second * y_steps_per_unit); + unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), 100000000.0 / (min_units_per_second * y_steps_per_unit)}; unsigned long max_interval; - unsigned long x_steps_per_sqr_second = max_acceleration_units_per_sq_second * x_steps_per_unit; - unsigned long y_steps_per_sqr_second = max_acceleration_units_per_sq_second * y_steps_per_unit; - unsigned long x_travel_steps_per_sqr_second = max_travel_acceleration_units_per_sq_second * x_steps_per_unit; - unsigned long y_travel_steps_per_sqr_second = max_travel_acceleration_units_per_sq_second * y_steps_per_unit; + unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second * x_steps_per_unit, max_acceleration_units_per_sq_second * y_steps_per_unit}; + unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second * x_steps_per_unit, max_travel_acceleration_units_per_sq_second * y_steps_per_unit}; unsigned long steps_per_sqr_second, plateau_steps; #endif #ifdef EXP_ACCELERATION unsigned long long_full_velocity_units = full_velocity_units * 100; unsigned long long_travel_move_full_velocity_units = travel_move_full_velocity_units * 100; - unsigned long max_x_interval = 100000000.0 / (min_units_per_second * x_steps_per_unit); - unsigned long max_y_interval = 100000000.0 / (min_units_per_second * y_steps_per_unit); + unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), 100000000.0 / (min_units_per_second * y_steps_per_unit)}; unsigned long max_interval; - unsigned long x_min_constant_speed_steps = min_constant_speed_units * x_steps_per_unit, - y_min_constant_speed_steps = min_constant_speed_units * y_steps_per_unit, min_constant_speed_steps; + unsigned long axis_min_constant_speed_steps[] = {min_constant_speed_units * x_steps_per_unit, min_constant_speed_units * y_steps_per_unit}; + unsigned long min_constant_speed_steps; #endif boolean acceleration_enabled = false, accelerating = false; unsigned long interval; @@ -778,12 +774,12 @@ inline void process_commands() break; #ifdef RAMP_ACCELERATION case 201: // M201 - if(code_seen('X')) x_steps_per_sqr_second = code_value() * x_steps_per_unit; - if(code_seen('Y')) y_steps_per_sqr_second = code_value() * y_steps_per_unit; + if(code_seen('X')) axis_steps_per_sqr_second[0] = code_value() * x_steps_per_unit; + if(code_seen('Y')) axis_steps_per_sqr_second[1] = code_value() * y_steps_per_unit; break; case 202: // M202 - if(code_seen('X')) x_travel_steps_per_sqr_second = code_value() * x_steps_per_unit; - if(code_seen('Y')) y_travel_steps_per_sqr_second = code_value() * y_steps_per_unit; + if(code_seen('X')) axis_travel_steps_per_sqr_second[0] = code_value() * x_steps_per_unit; + if(code_seen('Y')) axis_travel_steps_per_sqr_second[1] = code_value() * y_steps_per_unit; break; #endif } @@ -971,7 +967,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(axis_steps_remaining[3]) { enable_e(); do_step(3); axis_steps_remaining[3]--; } //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. - unsigned int delta[NUM_AXIS] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes + unsigned int delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes boolean steep_y = delta[1] > delta[0];// && delta[1] > delta[3] && delta[1] > delta[2]; boolean steep_x = delta[0] >= delta[1];// && delta[0] > delta[3] && delta[0] > delta[2]; //boolean steep_z = delta[2] > delta[0] && delta[2] > delta[1] && delta[2] > delta[3]; @@ -992,40 +988,32 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis) axis_error[i] = delta[primary_axis] / 2; interval = axis_interval[primary_axis]; + #ifdef RAMP_ACCELERATION + max_interval = axis_max_interval[primary_axis]; + if(e_steps_to_take > 0) steps_per_sqr_second = axis_steps_per_sqr_second[primary_axis]; + else steps_per_sqr_second = axis_travel_steps_per_sqr_second[primary_axis]; + max_speed_steps_per_second = 100000000 / interval; + min_speed_steps_per_second = 100000000 / max_interval; + float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second; + plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time); + #endif + //Do some Bresenham calculations depending on which axis will lead it. if(steep_y) { - #ifdef RAMP_ACCELERATION - max_interval = max_y_interval; - if(e_steps_to_take > 0) steps_per_sqr_second = y_steps_per_sqr_second; - else steps_per_sqr_second = y_travel_steps_per_sqr_second; - max_speed_steps_per_second = 100000000 / interval; - min_speed_steps_per_second = 100000000 / max_interval; - float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second; - plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time); - #endif #ifdef EXP_ACCELERATION if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * y_steps_per_unit /100; else virtual_full_velocity_steps = long_travel_move_full_velocity_units * y_steps_per_unit /100; - full_velocity_steps = min(virtual_full_velocity_steps, (delta[1] - y_min_constant_speed_steps) / 2); - max_interval = max_y_interval; - min_constant_speed_steps = y_min_constant_speed_steps; + full_velocity_steps = min(virtual_full_velocity_steps, (delta[1] - axis_min_constant_speed_steps[1]) / 2); + max_interval = axis_max_interval[1]; + min_constant_speed_steps = axis_min_constant_speed_steps[1]; #endif } else if (steep_x) { - #ifdef RAMP_ACCELERATION - max_interval = max_x_interval; - if(e_steps_to_take > 0) steps_per_sqr_second = x_steps_per_sqr_second; - else steps_per_sqr_second = x_travel_steps_per_sqr_second; - max_speed_steps_per_second = 100000000 / interval; - min_speed_steps_per_second = 100000000 / max_interval; - float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second; - plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time); - #endif #ifdef EXP_ACCELERATION if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * x_steps_per_unit /100; else virtual_full_velocity_steps = long_travel_move_full_velocity_units * x_steps_per_unit /100; - full_velocity_steps = min(virtual_full_velocity_steps, (delta[0] - x_min_constant_speed_steps) / 2); - max_interval = max_x_interval; - min_constant_speed_steps = x_min_constant_speed_steps; + full_velocity_steps = min(virtual_full_velocity_steps, (delta[0] - axis_min_constant_speed_steps[0]) / 2); + max_interval = axis_max_interval[0]; + min_constant_speed_steps = axis_min_constant_speed_steps[0]; #endif } unsigned long steps_done = 0; -- cgit v1.2.1 From 2e6cc78372d0543cce431cb29fe2ac82d89a9cec Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 05:36:09 +0200 Subject: Refactored exp variables to arrays and changed exp acceleration math to be axis generic --- Tonokip_Firmware/Tonokip_Firmware.pde | 36 ++++++++++++++--------------------- 1 file changed, 14 insertions(+), 22 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 902c0e0..9d7e0ba 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -70,9 +70,11 @@ unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take unsigned long steps_per_sqr_second, plateau_steps; #endif #ifdef EXP_ACCELERATION - unsigned long long_full_velocity_units = full_velocity_units * 100; - unsigned long long_travel_move_full_velocity_units = travel_move_full_velocity_units * 100; - unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), 100000000.0 / (min_units_per_second * y_steps_per_unit)}; + unsigned long axis_virtual_full_velocity_steps[] = {full_velocity_units * x_steps_per_unit, full_velocity_units * y_steps_per_unit}; + unsigned long axis_travel_virtual_full_velocity_steps[] = {travel_move_full_velocity_units * x_steps_per_unit, + travel_move_full_velocity_units * y_steps_per_unit}; + unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), + 100000000.0 / (min_units_per_second * y_steps_per_unit)}; unsigned long max_interval; unsigned long axis_min_constant_speed_steps[] = {min_constant_speed_units * x_steps_per_unit, min_constant_speed_units * y_steps_per_unit}; unsigned long min_constant_speed_steps; @@ -988,6 +990,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis) axis_error[i] = delta[primary_axis] / 2; interval = axis_interval[primary_axis]; + //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. #ifdef RAMP_ACCELERATION max_interval = axis_max_interval[primary_axis]; if(e_steps_to_take > 0) steps_per_sqr_second = axis_steps_per_sqr_second[primary_axis]; @@ -997,25 +1000,14 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second; plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time); #endif - - //Do some Bresenham calculations depending on which axis will lead it. - if(steep_y) { - #ifdef EXP_ACCELERATION - if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * y_steps_per_unit /100; - else virtual_full_velocity_steps = long_travel_move_full_velocity_units * y_steps_per_unit /100; - full_velocity_steps = min(virtual_full_velocity_steps, (delta[1] - axis_min_constant_speed_steps[1]) / 2); - max_interval = axis_max_interval[1]; - min_constant_speed_steps = axis_min_constant_speed_steps[1]; - #endif - } else if (steep_x) { - #ifdef EXP_ACCELERATION - if(e_steps_to_take > 0) virtual_full_velocity_steps = long_full_velocity_units * x_steps_per_unit /100; - else virtual_full_velocity_steps = long_travel_move_full_velocity_units * x_steps_per_unit /100; - full_velocity_steps = min(virtual_full_velocity_steps, (delta[0] - axis_min_constant_speed_steps[0]) / 2); - max_interval = axis_max_interval[0]; - min_constant_speed_steps = axis_min_constant_speed_steps[0]; - #endif - } + #ifdef EXP_ACCELERATION + if(e_steps_to_take > 0) virtual_full_velocity_steps = axis_virtual_full_velocity_steps[primary_axis]; + else virtual_full_velocity_steps = axis_travel_virtual_full_velocity_steps[primary_axis]; + full_velocity_steps = min(virtual_full_velocity_steps, (delta[primary_axis] - axis_min_constant_speed_steps[primary_axis]) / 2); + max_interval = axis_max_interval[primary_axis]; + min_constant_speed_steps = axis_min_constant_speed_steps[primary_axis]; + #endif + unsigned long steps_done = 0; #ifdef RAMP_ACCELERATION plateau_steps *= 1.01; // This is to compensate we use discrete intervals -- cgit v1.2.1 From 7b90a1f0f89f588247051a76d849584aa515cebc Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 07:26:51 +0200 Subject: X and Y axis now have their constant acceleration, and that is taken into account when calculating the leading axis acceleration for the move --- Tonokip_Firmware/Tonokip_Firmware.pde | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 9d7e0ba..c1e3d17 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -65,8 +65,8 @@ unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take #ifdef RAMP_ACCELERATION unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), 100000000.0 / (min_units_per_second * y_steps_per_unit)}; unsigned long max_interval; - unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second * x_steps_per_unit, max_acceleration_units_per_sq_second * y_steps_per_unit}; - unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second * x_steps_per_unit, max_travel_acceleration_units_per_sq_second * y_steps_per_unit}; + unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * x_steps_per_unit, max_acceleration_units_per_sq_second[1] * y_steps_per_unit}; + unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * x_steps_per_unit, max_travel_acceleration_units_per_sq_second[1] * y_steps_per_unit}; unsigned long steps_per_sqr_second, plateau_steps; #endif #ifdef EXP_ACCELERATION @@ -991,14 +991,20 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with interval = axis_interval[primary_axis]; //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. + //NOTE: if later, axis dependent min speed is introduced, we probably must ensure it is respected here... or maybe not #ifdef RAMP_ACCELERATION max_interval = axis_max_interval[primary_axis]; - if(e_steps_to_take > 0) steps_per_sqr_second = axis_steps_per_sqr_second[primary_axis]; - else steps_per_sqr_second = axis_travel_steps_per_sqr_second[primary_axis]; max_speed_steps_per_second = 100000000 / interval; min_speed_steps_per_second = 100000000 / max_interval; - float plateau_time = (max_speed_steps_per_second - min_speed_steps_per_second) / (float) steps_per_sqr_second; - plateau_steps = (long) ((steps_per_sqr_second / 2.0 * plateau_time + min_speed_steps_per_second) * plateau_time); + //Calculate slowest axis plateau time + float slowest_axis_plateau_time = 0; + for(int i=0; i < 2 ; i++) { //TODO: change to NUM_AXIS as axes get added to bresenham + if(e_steps_to_take > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / axis_max_interval[i]) / (float) axis_steps_per_sqr_second[i]); + else if(axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / axis_max_interval[i]) / (float) axis_travel_steps_per_sqr_second[i]); + } + //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated + steps_per_sqr_second = (100000000.0 / axis_interval[primary_axis] - 100000000.0 / axis_max_interval[primary_axis]) / slowest_axis_plateau_time; + plateau_steps = (long) ((steps_per_sqr_second / 2.0 * slowest_axis_plateau_time + min_speed_steps_per_second) * slowest_axis_plateau_time); #endif #ifdef EXP_ACCELERATION if(e_steps_to_take > 0) virtual_full_velocity_steps = axis_virtual_full_velocity_steps[primary_axis]; -- cgit v1.2.1 From a1e6fe387581b8ad981ecf29f901cc74982762e6 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 15:57:29 +0200 Subject: Some refactoring in preparation of axis with less start speed constraint check. See next commit for more info. --- Tonokip_Firmware/Tonokip_Firmware.pde | 134 +++++++++++++++++++++++++--------- 1 file changed, 99 insertions(+), 35 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index c1e3d17..f654ada 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -63,20 +63,24 @@ unsigned long axis_previous_micros[NUM_AXIS]; unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take; #ifdef RAMP_ACCELERATION - unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), 100000000.0 / (min_units_per_second * y_steps_per_unit)}; + unsigned int max_start_speed_axis_asc_order[NUM_AXIS]; + unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), + 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1]), + 100000000.0 / (max_start_speed_units_per_second[2] * axis_steps_per_unit[2]), + 100000000.0 / (max_start_speed_units_per_second[3] * axis_steps_per_unit[3])}; //TODO: refactor all things like this in a function unsigned long max_interval; - unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * x_steps_per_unit, max_acceleration_units_per_sq_second[1] * y_steps_per_unit}; - unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * x_steps_per_unit, max_travel_acceleration_units_per_sq_second[1] * y_steps_per_unit}; + unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], max_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1]}; + unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], max_travel_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1]}; unsigned long steps_per_sqr_second, plateau_steps; #endif #ifdef EXP_ACCELERATION - unsigned long axis_virtual_full_velocity_steps[] = {full_velocity_units * x_steps_per_unit, full_velocity_units * y_steps_per_unit}; - unsigned long axis_travel_virtual_full_velocity_steps[] = {travel_move_full_velocity_units * x_steps_per_unit, - travel_move_full_velocity_units * y_steps_per_unit}; - unsigned long axis_max_interval[] = {100000000.0 / (min_units_per_second * x_steps_per_unit), - 100000000.0 / (min_units_per_second * y_steps_per_unit)}; + unsigned long axis_virtual_full_velocity_steps[] = {full_velocity_units * axis_steps_per_unit[0], full_velocity_units * axis_steps_per_unit[1]}; + unsigned long axis_travel_virtual_full_velocity_steps[] = {travel_move_full_velocity_units * axis_steps_per_unit[0], + travel_move_full_velocity_units * axis_steps_per_unit[1]}; + unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), + 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1])}; unsigned long max_interval; - unsigned long axis_min_constant_speed_steps[] = {min_constant_speed_units * x_steps_per_unit, min_constant_speed_units * y_steps_per_unit}; + unsigned long axis_min_constant_speed_steps[] = {min_constant_speed_units * axis_steps_per_unit[0], min_constant_speed_units * axis_steps_per_unit[1]}; unsigned long min_constant_speed_steps; #endif boolean acceleration_enabled = false, accelerating = false; @@ -265,6 +269,36 @@ void setup() initsd(); #endif + + //Sort the axis with ascending max start speed in steps/s. The insertion sort + // algorithm is used, since it's the less expensive in code size. + #ifdef RAMP_ACCELERATION + long temp_max_intervals[NUM_AXIS]; + for(int i=0; i < NUM_AXIS; i++) { + temp_max_intervals[i] = axis_max_interval[i]; + max_start_speed_axis_asc_order[i] = i; + } + for(int i=1; i < NUM_AXIS; i++) { + int j=i-1; + long axis_value = temp_max_intervals[i]; + Serial.print("Axis value: "); Serial.println(axis_value); + while(j >= 0 && temp_max_intervals[j] < axis_value) { + temp_max_intervals[j+1] = temp_max_intervals[j]; + max_start_speed_axis_asc_order[j+1] = max_start_speed_axis_asc_order[j]; + j--; + } + Serial.print("Axis value: "); Serial.println(axis_value); + temp_max_intervals[j+1] = axis_value; + max_start_speed_axis_asc_order[j+1] = i; + } + #ifdef DEBUGGING + Serial.println("New start speed axes order :"); + Serial.println(max_start_speed_axis_asc_order[0]); + Serial.println(max_start_speed_axis_asc_order[1]); + Serial.println(max_start_speed_axis_asc_order[2]); + Serial.println(max_start_speed_axis_asc_order[3]); + #endif + #endif } @@ -480,7 +514,7 @@ inline void process_commands() if((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)) { current_x = 0; destination_x = 1.5 * X_MAX_LENGTH * X_HOME_DIR; - feedrate = min_units_per_second * 60; + feedrate = max_start_speed_units_per_second[0] * 60; prepare_move(); current_x = 0; @@ -498,7 +532,7 @@ inline void process_commands() if((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)) { current_y = 0; destination_y = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR; - feedrate = min_units_per_second * 60; + feedrate = max_start_speed_units_per_second[1] * 60; prepare_move(); current_y = 0; @@ -756,10 +790,39 @@ inline void process_commands() max_inactive_time = code_value() * 1000; break; case 92: // M92 - if(code_seen('X')) x_steps_per_unit = code_value(); - if(code_seen('Y')) y_steps_per_unit = code_value(); - if(code_seen('Z')) z_steps_per_unit = code_value(); - if(code_seen('E')) e_steps_per_unit = code_value(); + if(code_seen('X')) axis_steps_per_unit[0] = code_value(); + if(code_seen('Y')) axis_steps_per_unit[1] = code_value(); + if(code_seen('Z')) axis_steps_per_unit[2] = code_value(); + if(code_seen('E')) axis_steps_per_unit[3] = code_value(); + //Update start speed intervals and axis order. TODO: refactor array sorting in + // a function, refactor axis_max_interval[] calculation into a function, as they + // can be used in setup() as well + #ifdef RAMP_ACCELERATION + long temp_max_intervals[NUM_AXIS]; + for(int i=0; i < NUM_AXIS; i++) { + axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]); + temp_max_intervals[i] = axis_max_interval[i]; + max_start_speed_axis_asc_order[i] = i; + } + for(int i=1; i < NUM_AXIS; i++) { + int j=i-1; + long axis_value = temp_max_intervals[i]; + while(j >= 0 && temp_max_intervals[j] < axis_value) { + temp_max_intervals[j+1] = temp_max_intervals[j]; + max_start_speed_axis_asc_order[j+1] = max_start_speed_axis_asc_order[j]; + j--; + } + temp_max_intervals[j+1] = axis_value; + max_start_speed_axis_asc_order[j+1] = i; + } + #ifdef DEBUGGING + Serial.println("New start speed axes order :"); + Serial.println(max_start_speed_axis_asc_order[0]); + Serial.println(max_start_speed_axis_asc_order[1]); + Serial.println(max_start_speed_axis_asc_order[2]); + Serial.println(max_start_speed_axis_asc_order[3]); + #endif + #endif break; case 115: // M115 Serial.println("FIRMWARE_NAME:Sprinter FIRMWARE_URL:http%%3A/github.com/kliment/Sprinter/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1"); @@ -775,13 +838,14 @@ inline void process_commands() Serial.println(current_e); break; #ifdef RAMP_ACCELERATION + //TODO: update for all axis, use for loop case 201: // M201 - if(code_seen('X')) axis_steps_per_sqr_second[0] = code_value() * x_steps_per_unit; - if(code_seen('Y')) axis_steps_per_sqr_second[1] = code_value() * y_steps_per_unit; + if(code_seen('X')) axis_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; + if(code_seen('Y')) axis_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; break; case 202: // M202 - if(code_seen('X')) axis_travel_steps_per_sqr_second[0] = code_value() * x_steps_per_unit; - if(code_seen('Y')) axis_travel_steps_per_sqr_second[1] = code_value() * y_steps_per_unit; + if(code_seen('X')) axis_travel_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; + if(code_seen('Y')) axis_travel_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; break; #endif } @@ -865,10 +929,10 @@ inline void prepare_move() ydiff = (destination_y - current_y); zdiff = (destination_z - current_z); ediff = (destination_e - current_e); - x_steps_to_take = abs(xdiff) * x_steps_per_unit; - y_steps_to_take = abs(ydiff) * y_steps_per_unit; - z_steps_to_take = abs(zdiff) * z_steps_per_unit; - e_steps_to_take = abs(ediff) * e_steps_per_unit; + x_steps_to_take = abs(xdiff) * axis_steps_per_unit[0]; + y_steps_to_take = abs(ydiff) * axis_steps_per_unit[1]; + z_steps_to_take = abs(zdiff) * axis_steps_per_unit[2]; + e_steps_to_take = abs(ediff) * axis_steps_per_unit[3]; if(feedrate < 10) feedrate = 10; /* @@ -889,10 +953,10 @@ inline void prepare_move() //int feedz = (60000000 * zdiff) / time_for_move; //if(feedz > maxfeed) */ - #define X_TIME_FOR_MOVE ((float)x_steps_to_take / (x_steps_per_unit*feedrate/60000000)) - #define Y_TIME_FOR_MOVE ((float)y_steps_to_take / (y_steps_per_unit*feedrate/60000000)) - #define Z_TIME_FOR_MOVE ((float)z_steps_to_take / (z_steps_per_unit*z_feedrate/60000000)) - #define E_TIME_FOR_MOVE ((float)e_steps_to_take / (e_steps_per_unit*feedrate/60000000)) + #define X_TIME_FOR_MOVE ((float)x_steps_to_take / (axis_steps_per_unit[0]*feedrate/60000000)) + #define Y_TIME_FOR_MOVE ((float)y_steps_to_take / (axis_steps_per_unit[1]*feedrate/60000000)) + #define Z_TIME_FOR_MOVE ((float)z_steps_to_take / (axis_steps_per_unit[2]*z_feedrate/60000000)) + #define E_TIME_FOR_MOVE ((float)e_steps_to_take / (axis_steps_per_unit[3]*feedrate/60000000)) time_for_move = max(X_TIME_FOR_MOVE, Y_TIME_FOR_MOVE); time_for_move = max(time_for_move, Z_TIME_FOR_MOVE); @@ -1188,14 +1252,14 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(DISABLE_E) disable_e(); // Update current position partly based on direction, we probably can combine this with the direction code above... - if (destination_x > current_x) current_x = current_x + x_steps_to_take / x_steps_per_unit; - else current_x = current_x - x_steps_to_take / x_steps_per_unit; - if (destination_y > current_y) current_y = current_y + y_steps_to_take / y_steps_per_unit; - else current_y = current_y - y_steps_to_take / y_steps_per_unit; - if (destination_z > current_z) current_z = current_z + z_steps_to_take / z_steps_per_unit; - else current_z = current_z - z_steps_to_take / z_steps_per_unit; - if (destination_e > current_e) current_e = current_e + e_steps_to_take / e_steps_per_unit; - else current_e = current_e - e_steps_to_take / e_steps_per_unit; + if (destination_x > current_x) current_x = current_x + x_steps_to_take / axis_steps_per_unit[0]; + else current_x = current_x - x_steps_to_take / axis_steps_per_unit[0]; + if (destination_y > current_y) current_y = current_y + y_steps_to_take / axis_steps_per_unit[1]; + else current_y = current_y - y_steps_to_take / axis_steps_per_unit[1]; + if (destination_z > current_z) current_z = current_z + z_steps_to_take / axis_steps_per_unit[2]; + else current_z = current_z - z_steps_to_take / axis_steps_per_unit[2]; + if (destination_e > current_e) current_e = current_e + e_steps_to_take / axis_steps_per_unit[3]; + else current_e = current_e - e_steps_to_take / axis_steps_per_unit[3]; } -- cgit v1.2.1 From 05274fd21825f03619711200a95302354b51cf29 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 19:55:27 +0200 Subject: The start speed of the leading (X or Y) axis is now scaled to the speed of the limiting (X, Y, Z or E) axis in that move based on his start speed --- Tonokip_Firmware/Tonokip_Firmware.pde | 72 +++++++++++++++++++---------------- 1 file changed, 39 insertions(+), 33 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index f654ada..767473a 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -63,7 +63,6 @@ unsigned long axis_previous_micros[NUM_AXIS]; unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take; #ifdef RAMP_ACCELERATION - unsigned int max_start_speed_axis_asc_order[NUM_AXIS]; unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1]), 100000000.0 / (max_start_speed_units_per_second[2] * axis_steps_per_unit[2]), @@ -270,6 +269,8 @@ void setup() #endif + +/* //Sort the axis with ascending max start speed in steps/s. The insertion sort // algorithm is used, since it's the less expensive in code size. #ifdef RAMP_ACCELERATION @@ -281,13 +282,11 @@ void setup() for(int i=1; i < NUM_AXIS; i++) { int j=i-1; long axis_value = temp_max_intervals[i]; - Serial.print("Axis value: "); Serial.println(axis_value); while(j >= 0 && temp_max_intervals[j] < axis_value) { temp_max_intervals[j+1] = temp_max_intervals[j]; max_start_speed_axis_asc_order[j+1] = max_start_speed_axis_asc_order[j]; j--; } - Serial.print("Axis value: "); Serial.println(axis_value); temp_max_intervals[j+1] = axis_value; max_start_speed_axis_asc_order[j+1] = i; } @@ -298,7 +297,7 @@ void setup() Serial.println(max_start_speed_axis_asc_order[2]); Serial.println(max_start_speed_axis_asc_order[3]); #endif - #endif + #endif*/ } @@ -794,34 +793,15 @@ inline void process_commands() if(code_seen('Y')) axis_steps_per_unit[1] = code_value(); if(code_seen('Z')) axis_steps_per_unit[2] = code_value(); if(code_seen('E')) axis_steps_per_unit[3] = code_value(); - //Update start speed intervals and axis order. TODO: refactor array sorting in - // a function, refactor axis_max_interval[] calculation into a function, as they - // can be used in setup() as well + + //Update start speed intervals and axis order. TODO: refactor axis_max_interval[] calculation into a function, as it + // should also be used in setup() as well #ifdef RAMP_ACCELERATION long temp_max_intervals[NUM_AXIS]; for(int i=0; i < NUM_AXIS; i++) { - axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]); - temp_max_intervals[i] = axis_max_interval[i]; - max_start_speed_axis_asc_order[i] = i; - } - for(int i=1; i < NUM_AXIS; i++) { - int j=i-1; - long axis_value = temp_max_intervals[i]; - while(j >= 0 && temp_max_intervals[j] < axis_value) { - temp_max_intervals[j+1] = temp_max_intervals[j]; - max_start_speed_axis_asc_order[j+1] = max_start_speed_axis_asc_order[j]; - j--; - } - temp_max_intervals[j+1] = axis_value; - max_start_speed_axis_asc_order[j+1] = i; + axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);//TODO: do this for + // all steps_per_unit related variables } - #ifdef DEBUGGING - Serial.println("New start speed axes order :"); - Serial.println(max_start_speed_axis_asc_order[0]); - Serial.println(max_start_speed_axis_asc_order[1]); - Serial.println(max_start_speed_axis_asc_order[2]); - Serial.println(max_start_speed_axis_asc_order[3]); - #endif #endif break; case 115: // M115 @@ -1027,6 +1007,8 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. + // TODO: maybe it's better to refactor into a generic enable(int axis) function, that will probably take more ram, + // but will reduce code size if(axis_steps_remaining[0]) enable_x(); if(axis_steps_remaining[1]) enable_y(); if(axis_steps_remaining[2]) { enable_z(); do_step(2); axis_steps_remaining[2]--; } @@ -1055,19 +1037,43 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with interval = axis_interval[primary_axis]; //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. - //NOTE: if later, axis dependent min speed is introduced, we probably must ensure it is respected here... or maybe not #ifdef RAMP_ACCELERATION max_interval = axis_max_interval[primary_axis]; + unsigned long new_axis_max_intervals[NUM_AXIS]; max_speed_steps_per_second = 100000000 / interval; - min_speed_steps_per_second = 100000000 / max_interval; + min_speed_steps_per_second = 100000000 / max_interval; //TODO: can this be deleted? + //Calculate start speeds based on moving axes max start speed constraints. TODO: delete all println + int slowest_start_axis = primary_axis; + unsigned long slowest_start_axis_max_interval = max_interval; + for(int i = 0; i < NUM_AXIS; i++) + if (axis_steps_remaining[i] >0 && i != primary_axis && axis_max_interval[i] * axis_steps_remaining[i] + / axis_steps_remaining[slowest_start_axis] > slowest_start_axis_max_interval) { + slowest_start_axis = i; + slowest_start_axis_max_interval = axis_max_interval[i]; + } + for(int i = 0; i < NUM_AXIS; i++) + if(axis_steps_remaining[i] >0) { + new_axis_max_intervals[i] = slowest_start_axis_max_interval * axis_steps_remaining[i] / axis_steps_remaining[slowest_start_axis]; + //Serial.print("new_axis_max_intervals[i] :"); Serial.println(new_axis_max_intervals[i]); //TODO: delete this println when finished + if(i == primary_axis) { + max_interval = new_axis_max_intervals[i]; + min_speed_steps_per_second = 100000000 / max_interval; + } + } + max_interval = new_axis_max_intervals[primary_axis]; + //Serial.print("Max interval :"); Serial.println(max_interval); //TODO: delete this println when finished //Calculate slowest axis plateau time float slowest_axis_plateau_time = 0; for(int i=0; i < 2 ; i++) { //TODO: change to NUM_AXIS as axes get added to bresenham - if(e_steps_to_take > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / axis_max_interval[i]) / (float) axis_steps_per_sqr_second[i]); - else if(axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / axis_max_interval[i]) / (float) axis_travel_steps_per_sqr_second[i]); + if(axis_steps_remaining[i] > 0) { + if(e_steps_to_take > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, + (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); + else if(axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, + (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_travel_steps_per_sqr_second[i]); + } } //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated - steps_per_sqr_second = (100000000.0 / axis_interval[primary_axis] - 100000000.0 / axis_max_interval[primary_axis]) / slowest_axis_plateau_time; + steps_per_sqr_second = (100000000.0 / axis_interval[primary_axis] - 100000000.0 / new_axis_max_intervals[primary_axis]) / slowest_axis_plateau_time; plateau_steps = (long) ((steps_per_sqr_second / 2.0 * slowest_axis_plateau_time + min_speed_steps_per_second) * slowest_axis_plateau_time); #endif #ifdef EXP_ACCELERATION -- cgit v1.2.1 From 0cf824857bc05deb36ed0b9b3c52c4b2c9ef673c Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Thu, 19 May 2011 21:52:30 +0200 Subject: Z now has its own max acceleration, and it is now fully integrated into Bresenham --- Tonokip_Firmware/Tonokip_Firmware.pde | 54 +++++++++++++++-------------------- 1 file changed, 23 insertions(+), 31 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 767473a..8d14217 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -66,10 +66,12 @@ unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1]), 100000000.0 / (max_start_speed_units_per_second[2] * axis_steps_per_unit[2]), - 100000000.0 / (max_start_speed_units_per_second[3] * axis_steps_per_unit[3])}; //TODO: refactor all things like this in a function + 100000000.0 / (max_start_speed_units_per_second[3] * axis_steps_per_unit[3])}; //TODO: refactor all things like this in a function, or move to setup() unsigned long max_interval; - unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], max_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1]}; - unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], max_travel_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1]}; + unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], + max_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2]}; + unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], + max_travel_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_travel_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2]}; unsigned long steps_per_sqr_second, plateau_steps; #endif #ifdef EXP_ACCELERATION @@ -822,10 +824,12 @@ inline void process_commands() case 201: // M201 if(code_seen('X')) axis_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; if(code_seen('Y')) axis_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; + if(code_seen('Z')) axis_steps_per_sqr_second[2] = code_value() * axis_steps_per_unit[2]; break; case 202: // M202 if(code_seen('X')) axis_travel_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; if(code_seen('Y')) axis_travel_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; + if(code_seen('Z')) axis_travel_steps_per_sqr_second[2] = code_value() * axis_steps_per_unit[2]; break; #endif } @@ -1011,18 +1015,19 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with // but will reduce code size if(axis_steps_remaining[0]) enable_x(); if(axis_steps_remaining[1]) enable_y(); - if(axis_steps_remaining[2]) { enable_z(); do_step(2); axis_steps_remaining[2]--; } + if(axis_steps_remaining[2]) enable_z(); if(axis_steps_remaining[3]) { enable_e(); do_step(3); axis_steps_remaining[3]--; } //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. unsigned int delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes - boolean steep_y = delta[1] > delta[0];// && delta[1] > delta[3] && delta[1] > delta[2]; - boolean steep_x = delta[0] >= delta[1];// && delta[0] > delta[3] && delta[0] > delta[2]; - //boolean steep_z = delta[2] > delta[0] && delta[2] > delta[1] && delta[2] > delta[3]; + boolean steep_y = delta[1] > delta[0] && delta[1] > delta[2];// && delta[1] > delta[3]; + boolean steep_x = delta[0] >= delta[1] && delta[0] > delta[2];// && delta[0] > delta[3]]; + boolean steep_z = delta[2] >= delta[0] && delta[2] >= delta[1]; // && delta[2] > delta[3]; int axis_error[NUM_AXIS]; unsigned int primary_axis; if(steep_x) primary_axis = 0; - else primary_axis = 1; + else if (steep_y) primary_axis = 1; + else primary_axis = 2; #ifdef RAMP_ACCELERATION long max_speed_steps_per_second; long min_speed_steps_per_second; @@ -1064,7 +1069,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //Serial.print("Max interval :"); Serial.println(max_interval); //TODO: delete this println when finished //Calculate slowest axis plateau time float slowest_axis_plateau_time = 0; - for(int i=0; i < 2 ; i++) { //TODO: change to NUM_AXIS as axes get added to bresenham + for(int i=0; i < 3 ; i++) { //TODO: change to NUM_AXIS as axes get added to bresenham if(axis_steps_remaining[i] > 0) { if(e_steps_to_take > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); @@ -1153,6 +1158,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #endif #ifdef EXP_ACCELERATION //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval + // TODO: is this any useful? -> steps_done % steps_acceleration_check == 0 if (acceleration_enabled && steps_done < full_velocity_steps && steps_done / full_velocity_steps < 1 && (steps_done % steps_acceleration_check == 0)) { if(steps_done == 0) { interval = max_interval; @@ -1175,18 +1181,20 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #endif //If there are x or y steps remaining, perform Bresenham algorithm - if(axis_steps_remaining[0] || axis_steps_remaining[1]) { + if(axis_steps_remaining[0] || axis_steps_remaining[1] || axis_steps_remaining[2]) { if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break; if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; + if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; + if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; timediff = micros() * 100 - axis_previous_micros[primary_axis]; while(timediff >= interval && axis_steps_remaining[primary_axis] > 0) { steps_done++; steps_remaining--; axis_steps_remaining[primary_axis]--; timediff -= interval; do_step(primary_axis); - for(int i=0; i < 2; i++) if(i != primary_axis) {//TODO change "2" to NUM_AXIS when other axes gets added to bresenham + for(int i=0; i < 3; i++) if(i != primary_axis) {//TODO change "3" to NUM_AXIS when other axes gets added to bresenham axis_error[i] = axis_error[i] - delta[i]; if(axis_error[i] < 0) { do_step(i); axis_steps_remaining[i]--; @@ -1194,6 +1202,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with } } #ifdef RAMP_ACCELERATION + //TODO: may this check be dangerous? -> steps_remaining == plateau_steps if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; #endif #ifdef STEP_DELAY_RATIO @@ -1210,33 +1219,16 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating))) continue; #endif - //If there are z steps remaining, check if z steps must be taken - if(axis_steps_remaining[2]) { - if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; - timediff = micros() * 100-axis_previous_micros[2]; - while(timediff >= axis_interval[2] && axis_steps_remaining[2]) { - do_step(2); - axis_steps_remaining[2]--; - timediff -= axis_interval[2]; - #ifdef STEP_DELAY_RATIO - if(timediff >= axis_interval[2]) delayMicroseconds(long_step_delay_ratio * axis_interval[2] / 10000); - #endif - #ifdef STEP_DELAY_MICROS - if(timediff >= axis_interval[2]) delayMicroseconds(STEP_DELAY_MICROS); - #endif - } - } - //If there are e steps remaining, check if e steps must be taken if(axis_steps_remaining[3]){ - if (x_steps_to_take + y_steps_to_take <= 0) timediff = micros()*100 - axis_previous_micros[3]; + if (x_steps_to_take + y_steps_to_take + z_steps_to_take<= 0) timediff = micros()*100 - axis_previous_micros[3]; unsigned int final_e_steps_remaining = 0; if (steep_x && x_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[0] / x_steps_to_take; else if (steep_y && y_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[1] / y_steps_to_take; + else if (steep_z && z_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[2] / z_steps_to_take; //If this move has X or Y steps, let E follow the Bresenham pace if (final_e_steps_remaining > 0) while(axis_steps_remaining[3] > final_e_steps_remaining) { do_step(3); axis_steps_remaining[3]--;} - else if (x_steps_to_take + y_steps_to_take > 0) while(axis_steps_remaining[3]) { do_step(3); axis_steps_remaining[3]--;} + else if (x_steps_to_take + y_steps_to_take + z_steps_to_take > 0) while(axis_steps_remaining[3]) { do_step(3); axis_steps_remaining[3]--;} //Else, normally check if e steps must be taken else while (timediff >= axis_interval[3] && axis_steps_remaining[3]) { do_step(3); -- cgit v1.2.1 From 181df1fe733aca5264c0f63425779c767b37e54e Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Fri, 20 May 2011 10:25:34 +0200 Subject: All axes are now controlled in Bresenham. Now, also E has its own max acceleration and start speed. Also added some function useful for debugging. --- Tonokip_Firmware/Tonokip_Firmware.pde | 217 ++++++++++++++++++---------------- 1 file changed, 115 insertions(+), 102 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 8d14217..bc65755 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -67,11 +67,14 @@ unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1]), 100000000.0 / (max_start_speed_units_per_second[2] * axis_steps_per_unit[2]), 100000000.0 / (max_start_speed_units_per_second[3] * axis_steps_per_unit[3])}; //TODO: refactor all things like this in a function, or move to setup() + // in a for loop unsigned long max_interval; unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], - max_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2]}; + max_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2], + max_acceleration_units_per_sq_second[3] * axis_steps_per_unit[3]}; unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], - max_travel_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_travel_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2]}; + max_travel_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_travel_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2], + max_travel_acceleration_units_per_sq_second[3] * axis_steps_per_unit[3]}; unsigned long steps_per_sqr_second, plateau_steps; #endif #ifdef EXP_ACCELERATION @@ -270,36 +273,6 @@ void setup() initsd(); #endif - - -/* - //Sort the axis with ascending max start speed in steps/s. The insertion sort - // algorithm is used, since it's the less expensive in code size. - #ifdef RAMP_ACCELERATION - long temp_max_intervals[NUM_AXIS]; - for(int i=0; i < NUM_AXIS; i++) { - temp_max_intervals[i] = axis_max_interval[i]; - max_start_speed_axis_asc_order[i] = i; - } - for(int i=1; i < NUM_AXIS; i++) { - int j=i-1; - long axis_value = temp_max_intervals[i]; - while(j >= 0 && temp_max_intervals[j] < axis_value) { - temp_max_intervals[j+1] = temp_max_intervals[j]; - max_start_speed_axis_asc_order[j+1] = max_start_speed_axis_asc_order[j]; - j--; - } - temp_max_intervals[j+1] = axis_value; - max_start_speed_axis_asc_order[j+1] = i; - } - #ifdef DEBUGGING - Serial.println("New start speed axes order :"); - Serial.println(max_start_speed_axis_asc_order[0]); - Serial.println(max_start_speed_axis_asc_order[1]); - Serial.println(max_start_speed_axis_asc_order[2]); - Serial.println(max_start_speed_axis_asc_order[3]); - #endif - #endif*/ } @@ -825,11 +798,13 @@ inline void process_commands() if(code_seen('X')) axis_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; if(code_seen('Y')) axis_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; if(code_seen('Z')) axis_steps_per_sqr_second[2] = code_value() * axis_steps_per_unit[2]; + if(code_seen('E')) axis_steps_per_sqr_second[3] = code_value() * axis_steps_per_unit[3]; break; case 202: // M202 if(code_seen('X')) axis_travel_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; if(code_seen('Y')) axis_travel_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; if(code_seen('Z')) axis_travel_steps_per_sqr_second[2] = code_value() * axis_steps_per_unit[2]; + if(code_seen('E')) axis_travel_steps_per_sqr_second[3] = code_value() * axis_steps_per_unit[3]; break; #endif } @@ -951,9 +926,7 @@ inline void prepare_move() if(z_steps_to_take) axis_interval[2] = time_for_move / z_steps_to_take * 100; if(e_steps_to_take && (x_steps_to_take + y_steps_to_take <= 0) ) axis_interval[3] = time_for_move / e_steps_to_take * 100; - //#define DEBUGGING false - #if 0 - if(0) { + #ifdef DEBUG_PREPARE_MOVE Serial.print("destination_x: "); Serial.println(destination_x); Serial.print("current_x: "); Serial.println(current_x); Serial.print("x_steps_to_take: "); Serial.println(x_steps_to_take); @@ -978,16 +951,9 @@ inline void prepare_move() Serial.print("E_TIME_FOR_MVE: "); Serial.println(E_TIME_FOR_MOVE); Serial.print("axis_interval[3]: "); Serial.println(axis_interval[3]); Serial.println(""); - } - #endif - #ifdef PRINT_MOVE_TIME - unsigned long startmove = micros(); #endif unsigned long axis_steps_to_take[NUM_AXIS] = {x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take}; linear_move(axis_steps_to_take); // make the move - #ifdef PRINT_MOVE_TIME - Serial.println(micros()-startmove); - #endif } void linear_move(unsigned long axis_steps_remaining[]) // make linear move with preset speeds and destinations, see G0 and G1 @@ -1016,38 +982,37 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(axis_steps_remaining[0]) enable_x(); if(axis_steps_remaining[1]) enable_y(); if(axis_steps_remaining[2]) enable_z(); - if(axis_steps_remaining[3]) { enable_e(); do_step(3); axis_steps_remaining[3]--; } + if(axis_steps_remaining[3]) enable_e(); //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. unsigned int delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes - boolean steep_y = delta[1] > delta[0] && delta[1] > delta[2];// && delta[1] > delta[3]; - boolean steep_x = delta[0] >= delta[1] && delta[0] > delta[2];// && delta[0] > delta[3]]; - boolean steep_z = delta[2] >= delta[0] && delta[2] >= delta[1]; // && delta[2] > delta[3]; int axis_error[NUM_AXIS]; unsigned int primary_axis; - if(steep_x) primary_axis = 0; - else if (steep_y) primary_axis = 1; - else primary_axis = 2; - #ifdef RAMP_ACCELERATION - long max_speed_steps_per_second; - long min_speed_steps_per_second; - #endif - #ifdef EXP_ACCELERATION - unsigned long virtual_full_velocity_steps; - unsigned long full_velocity_steps; - #endif + if(delta[1] > delta[0] && delta[1] > delta[2] && delta[1] > delta[3]) primary_axis = 1; + else if (delta[0] >= delta[1] && delta[0] > delta[2] && delta[0] > delta[3]) primary_axis = 0; + else if (delta[2] >= delta[0] && delta[2] >= delta[1] && delta[2] > delta[3]) primary_axis = 2; + else primary_axis = 3; unsigned long steps_remaining = delta[primary_axis]; unsigned long steps_to_take = steps_remaining; for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis) axis_error[i] = delta[primary_axis] / 2; interval = axis_interval[primary_axis]; + #ifdef DEBUG_BRESENHAM + log_int("_BRESENHAM - Primary axis", primary_axis); + log_int("_BRESENHAM - Primary axis full speed interval", interval); + #endif //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. #ifdef RAMP_ACCELERATION + long max_speed_steps_per_second; + long min_speed_steps_per_second; max_interval = axis_max_interval[primary_axis]; + #ifdef DEBUG_RAMP_ACCELERATION + log_ulong_array("_RAMP_ACCELERATION - Teoric step intervals at move start", axis_max_interval, NUM_AXIS); + #endif unsigned long new_axis_max_intervals[NUM_AXIS]; max_speed_steps_per_second = 100000000 / interval; min_speed_steps_per_second = 100000000 / max_interval; //TODO: can this be deleted? - //Calculate start speeds based on moving axes max start speed constraints. TODO: delete all println + //Calculate start speeds based on moving axes max start speed constraints. int slowest_start_axis = primary_axis; unsigned long slowest_start_axis_max_interval = max_interval; for(int i = 0; i < NUM_AXIS; i++) @@ -1059,17 +1024,17 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with for(int i = 0; i < NUM_AXIS; i++) if(axis_steps_remaining[i] >0) { new_axis_max_intervals[i] = slowest_start_axis_max_interval * axis_steps_remaining[i] / axis_steps_remaining[slowest_start_axis]; - //Serial.print("new_axis_max_intervals[i] :"); Serial.println(new_axis_max_intervals[i]); //TODO: delete this println when finished if(i == primary_axis) { max_interval = new_axis_max_intervals[i]; min_speed_steps_per_second = 100000000 / max_interval; } } - max_interval = new_axis_max_intervals[primary_axis]; - //Serial.print("Max interval :"); Serial.println(max_interval); //TODO: delete this println when finished + #ifdef DEBUG_RAMP_ACCELERATION + log_ulong_array("_RAMP_ACCELERATION - Actual step intervals at move start", new_axis_max_intervals, NUM_AXIS); + #endif //Calculate slowest axis plateau time float slowest_axis_plateau_time = 0; - for(int i=0; i < 3 ; i++) { //TODO: change to NUM_AXIS as axes get added to bresenham + for(int i=0; i < NUM_AXIS ; i++) { if(axis_steps_remaining[i] > 0) { if(e_steps_to_take > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); @@ -1082,6 +1047,8 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with plateau_steps = (long) ((steps_per_sqr_second / 2.0 * slowest_axis_plateau_time + min_speed_steps_per_second) * slowest_axis_plateau_time); #endif #ifdef EXP_ACCELERATION + unsigned long virtual_full_velocity_steps; + unsigned long full_velocity_steps; if(e_steps_to_take > 0) virtual_full_velocity_steps = axis_virtual_full_velocity_steps[primary_axis]; else virtual_full_velocity_steps = axis_travel_virtual_full_velocity_steps[primary_axis]; full_velocity_steps = min(virtual_full_velocity_steps, (delta[primary_axis] - axis_min_constant_speed_steps[primary_axis]) / 2); @@ -1116,6 +1083,10 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with for(int i = 0; i < NUM_AXIS; i++) { axis_previous_micros[i] = start_move_micros * 100; } + + #ifdef DEBUG_MOVE_TIME + unsigned long startmove = micros(); + #endif //move until no more steps remain while(axis_steps_remaining[0] + axis_steps_remaining[1] + axis_steps_remaining[2] + axis_steps_remaining[3] > 0) { @@ -1181,7 +1152,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #endif //If there are x or y steps remaining, perform Bresenham algorithm - if(axis_steps_remaining[0] || axis_steps_remaining[1] || axis_steps_remaining[2]) { + if(axis_steps_remaining[primary_axis]) { if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break; if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; @@ -1193,18 +1164,14 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with steps_done++; steps_remaining--; axis_steps_remaining[primary_axis]--; timediff -= interval; - do_step(primary_axis); - for(int i=0; i < 3; i++) if(i != primary_axis) {//TODO change "3" to NUM_AXIS when other axes gets added to bresenham + do_step_update_micros(primary_axis); + for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis && axis_steps_remaining[i] > 0) { axis_error[i] = axis_error[i] - delta[i]; if(axis_error[i] < 0) { do_step(i); axis_steps_remaining[i]--; axis_error[i] = axis_error[i] + delta[primary_axis]; } } - #ifdef RAMP_ACCELERATION - //TODO: may this check be dangerous? -> steps_remaining == plateau_steps - if (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating)) break; - #endif #ifdef STEP_DELAY_RATIO if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000); #endif @@ -1213,36 +1180,10 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #endif } } - #ifdef RAMP_ACCELERATION - if((axis_steps_remaining[0]>0 || axis_steps_remaining[1]>0) && - steps_to_take > 0 && - (steps_remaining == plateau_steps || (steps_done >= steps_to_take / 2 && accelerating && !decelerating))) continue; - #endif - - //If there are e steps remaining, check if e steps must be taken - if(axis_steps_remaining[3]){ - if (x_steps_to_take + y_steps_to_take + z_steps_to_take<= 0) timediff = micros()*100 - axis_previous_micros[3]; - unsigned int final_e_steps_remaining = 0; - if (steep_x && x_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[0] / x_steps_to_take; - else if (steep_y && y_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[1] / y_steps_to_take; - else if (steep_z && z_steps_to_take > 0) final_e_steps_remaining = e_steps_to_take * axis_steps_remaining[2] / z_steps_to_take; - //If this move has X or Y steps, let E follow the Bresenham pace - if (final_e_steps_remaining > 0) while(axis_steps_remaining[3] > final_e_steps_remaining) { do_step(3); axis_steps_remaining[3]--;} - else if (x_steps_to_take + y_steps_to_take + z_steps_to_take > 0) while(axis_steps_remaining[3]) { do_step(3); axis_steps_remaining[3]--;} - //Else, normally check if e steps must be taken - else while (timediff >= axis_interval[3] && axis_steps_remaining[3]) { - do_step(3); - axis_steps_remaining[3]--; - timediff -= axis_interval[3]; - #ifdef STEP_DELAY_RATIO - if(timediff >= axis_interval[3]) delayMicroseconds(long_step_delay_ratio * axis_interval[3] / 10000); - #endif - #ifdef STEP_DELAY_MICROS - if(timediff >= axis_interval[3]) delayMicroseconds(STEP_DELAY_MICROS); - #endif - } - } } + #ifdef DEBUG_MOVE_TIME + log_ulong("_MOVE_TIME - This move took", micros()-startmove); + #endif if(DISABLE_X) disable_x(); if(DISABLE_Y) disable_y(); @@ -1260,13 +1201,14 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with else current_e = current_e - e_steps_to_take / axis_steps_per_unit[3]; } +inline void do_step_update_micros(int axis) { + digitalWrite(STEP_PIN[axis], HIGH); + axis_previous_micros[axis] += interval; + digitalWrite(STEP_PIN[axis], LOW); +} inline void do_step(int axis) { digitalWrite(STEP_PIN[axis], HIGH); - //TODO: the following check is ugly and not the best thing to do here, but this will be sorted out more easily when all - // axis will be under Bresenham - if(axis < 2) axis_previous_micros[axis] += interval; - else axis_previous_micros[axis] += axis_interval[axis]; digitalWrite(STEP_PIN[axis], LOW); } @@ -1581,3 +1523,74 @@ inline void manage_inactivity(byte debug) { if( (millis()-previous_millis_cmd) > max_inactive_time ) if(max_inactive_time) kill(); if( (millis()-previous_millis_cmd) > stepper_inactive_time ) if(stepper_inactive_time) { disable_x(); disable_y(); disable_z(); disable_e(); } } + +#ifdef DEBUG +void log_message(char* message) { + Serial.println(message); +} + +void log_int(char* message, int value) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); +} + +void log_long(char* message, long value) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); +} + +void log_float(char* message, float value) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); +} + +void log_uint(char* message, unsigned int value) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); +} + +void log_ulong(char* message, unsigned long value) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); +} + +void log_int_array(char* message, int value[], int array_lenght) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); + for(int i=0; i < array_lenght; i++){ + Serial.print(value[i]); + if(i != array_lenght-1) Serial.print(", "); + } + Serial.println("}"); +} + +void log_long_array(char* message, long value[], int array_lenght) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); + for(int i=0; i < array_lenght; i++){ + Serial.print(value[i]); + if(i != array_lenght-1) Serial.print(", "); + } + Serial.println("}"); +} + +void log_float_array(char* message, float value[], int array_lenght) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); + for(int i=0; i < array_lenght; i++){ + Serial.print(value[i]); + if(i != array_lenght-1) Serial.print(", "); + } + Serial.println("}"); +} + +void log_uint_array(char* message, unsigned int value[], int array_lenght) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); + for(int i=0; i < array_lenght; i++){ + Serial.print(value[i]); + if(i != array_lenght-1) Serial.print(", "); + } + Serial.println("}"); +} + +void log_ulong_array(char* message, unsigned long value[], int array_lenght) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); + for(int i=0; i < array_lenght; i++){ + Serial.print(value[i]); + if(i != array_lenght-1) Serial.print(", "); + } + Serial.println("}"); +} +#endif -- cgit v1.2.1 From a89f443eb2b635d67daaa01962561f452f7d8a80 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sat, 21 May 2011 01:51:29 +0200 Subject: Added options that allow to disable heating management during acceleration or during the whole move --- Tonokip_Firmware/Tonokip_Firmware.pde | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index bc65755..e708910 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -1090,9 +1090,19 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //move until no more steps remain while(axis_steps_remaining[0] + axis_steps_remaining[1] + axis_steps_remaining[2] + axis_steps_remaining[3] > 0) { - //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp - manage_heater(); - manage_inactivity(2); + #ifdef DISABLE_CHECK_DURING_ACC + if(!accelerating && !decelerating) { + //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp + manage_heater(); + } + #else + #ifdef DISABLE_CHECK_DURING_MOVE + {} //Do nothing + #else + //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp + manage_heater(); + #endif + #endif #ifdef RAMP_ACCELERATION //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval if (acceleration_enabled && steps_done == 0) { -- cgit v1.2.1 From 1bbdc1970601efe630db4afc6445886bb942abe3 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sun, 22 May 2011 19:52:00 +0200 Subject: Time for move is now correctly calculated in the XYZ space. Fixed a safety bug that caused heating management not to be performed in case DISABLE_CHECK_DURING_MOVE was enabled. Fixed a bug in the moving axis start speed checking. --- Tonokip_Firmware/Tonokip_Firmware.pde | 326 +++++++++++++++------------------- 1 file changed, 143 insertions(+), 183 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index e708910..f71d6d9 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -57,11 +57,12 @@ //Stepper Movement Variables -bool direction_x, direction_y, direction_z, direction_e; +char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'}; +bool move_direction[NUM_AXIS]; const int STEP_PIN[NUM_AXIS] = {X_STEP_PIN, Y_STEP_PIN, Z_STEP_PIN, E_STEP_PIN}; unsigned long axis_previous_micros[NUM_AXIS]; unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; -unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take; +unsigned long move_steps_to_take[NUM_AXIS]; #ifdef RAMP_ACCELERATION unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1]), @@ -89,15 +90,18 @@ unsigned long x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take #endif boolean acceleration_enabled = false, accelerating = false; unsigned long interval; -float destination_x = 0.0, destination_y = 0.0, destination_z = 0.0, destination_e = 0.0; -float current_x = 0.0, current_y = 0.0, current_z = 0.0, current_e = 0.0; +float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; +float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; long axis_interval[NUM_AXIS]; // for speed delay -float feedrate = 1500, next_feedrate, z_feedrate, saved_feedrate; +float feedrate = 1500, next_feedrate, saved_feedrate; float time_for_move; long gcode_N, gcode_LastN; bool relative_mode = false; //Determines Absolute or Relative Coordinates bool relative_mode_e = false; //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode. long timediff = 0; +//experimental feedrate calc +float d = 0; +float axis_diff[NUM_AXIS] = {0, 0, 0, 0}; #ifdef STEP_DELAY_RATIO long long_step_delay_ratio = STEP_DELAY_RATIO * 100; #endif @@ -442,9 +446,6 @@ inline bool code_seen(char code) strchr_pointer = strchr(cmdbuffer[bufindr], code); return (strchr_pointer != NULL); //Return True if a character was found } - //experimental feedrate calc -float d = 0; -float xdiff = 0, ydiff = 0, zdiff = 0, ediff = 0; inline void process_commands() { @@ -457,6 +458,9 @@ inline void process_commands() { case 0: // G0 -> G1 case 1: // G1 + #ifdef DISABLE_CHECK_DURING_ACC || DISABLE_CHECK_DURING_MOVE + manage_heater(); + #endif get_coordinates(); // For X Y Z E F prepare_move(); previous_millis_cmd = millis(); @@ -474,68 +478,64 @@ inline void process_commands() break; case 28: //G28 Home all Axis one at a time saved_feedrate = feedrate; - destination_x = 0; - current_x = 0; - destination_y = 0; - current_y = 0; - destination_z = 0; - current_z = 0; - destination_e = 0; - current_e = 0; + for(int i=0; i < NUM_AXIS; i++) { + destination[i] = 0; + current_position[i] = 0; + } feedrate = 0; if((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)) { - current_x = 0; - destination_x = 1.5 * X_MAX_LENGTH * X_HOME_DIR; + current_position[0] = 0; + destination[0] = 1.5 * X_MAX_LENGTH * X_HOME_DIR; feedrate = max_start_speed_units_per_second[0] * 60; prepare_move(); - current_x = 0; - destination_x = -1 * X_HOME_DIR; + current_position[0] = 0; + destination[0] = -1 * X_HOME_DIR; prepare_move(); - destination_x = 10 * X_HOME_DIR; + destination[0] = 10 * X_HOME_DIR; prepare_move(); - current_x = 0; - destination_x = 0; + current_position[0] = 0; + destination[0] = 0; feedrate = 0; } if((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)) { - current_y = 0; - destination_y = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR; + current_position[1] = 0; + destination[1] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR; feedrate = max_start_speed_units_per_second[1] * 60; prepare_move(); - current_y = 0; - destination_y = -1 * Y_HOME_DIR; + current_position[1] = 0; + destination[1] = -1 * Y_HOME_DIR; prepare_move(); - destination_y = 10 * Y_HOME_DIR; + destination[1] = 10 * Y_HOME_DIR; prepare_move(); - current_y = 0; - destination_y = 0; + current_position[1] = 0; + destination[1] = 0; feedrate = 0; } if((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1)) { - current_z = 0; - destination_z = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR; - feedrate = max_z_feedrate/2; + current_position[2] = 0; + destination[2] = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR; + feedrate = max_feedrate[2]/2; prepare_move(); - current_z = 0; - destination_z = -1 * Z_HOME_DIR; + current_position[2] = 0; + destination[2] = -1 * Z_HOME_DIR; prepare_move(); - destination_z = 10 * Z_HOME_DIR; + destination[2] = 10 * Z_HOME_DIR; prepare_move(); - current_z = 0; - destination_z = 0; + current_position[2] = 0; + destination[2] = 0; feedrate = 0; } @@ -549,10 +549,9 @@ inline void process_commands() relative_mode = true; break; case 92: // G92 - if(code_seen('X')) current_x = code_value(); - if(code_seen('Y')) current_y = code_value(); - if(code_seen('Z')) current_z = code_value(); - if(code_seen('E')) current_e = code_value(); + for(int i=0; i < NUM_AXIS; i++) { + if(code_seen(axis_codes[i])) current_position[i] = code_value(); + } break; } @@ -750,10 +749,10 @@ inline void process_commands() if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT); //Floating break; case 82: - relative_mode_e = false; + axis_relative_modes[3] = false; break; case 83: - relative_mode_e = true; + axis_relative_modes[3] = true; break; case 84: if(code_seen('S')){ stepper_inactive_time = code_value() * 1000; } @@ -764,10 +763,9 @@ inline void process_commands() max_inactive_time = code_value() * 1000; break; case 92: // M92 - if(code_seen('X')) axis_steps_per_unit[0] = code_value(); - if(code_seen('Y')) axis_steps_per_unit[1] = code_value(); - if(code_seen('Z')) axis_steps_per_unit[2] = code_value(); - if(code_seen('E')) axis_steps_per_unit[3] = code_value(); + for(int i=0; i < NUM_AXIS; i++) { + if(code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value(); + } //Update start speed intervals and axis order. TODO: refactor axis_max_interval[] calculation into a function, as it // should also be used in setup() as well @@ -784,27 +782,25 @@ inline void process_commands() break; case 114: // M114 Serial.print("X:"); - Serial.print(current_x); + Serial.print(current_position[0]); Serial.print("Y:"); - Serial.print(current_y); + Serial.print(current_position[1]); Serial.print("Z:"); - Serial.print(current_z); + Serial.print(current_position[2]); Serial.print("E:"); - Serial.println(current_e); + Serial.println(current_position[3]); break; #ifdef RAMP_ACCELERATION //TODO: update for all axis, use for loop case 201: // M201 - if(code_seen('X')) axis_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; - if(code_seen('Y')) axis_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; - if(code_seen('Z')) axis_steps_per_sqr_second[2] = code_value() * axis_steps_per_unit[2]; - if(code_seen('E')) axis_steps_per_sqr_second[3] = code_value() * axis_steps_per_unit[3]; + for(int i=0; i < NUM_AXIS; i++) { + if(code_seen(axis_codes[i])) axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; + } break; case 202: // M202 - if(code_seen('X')) axis_travel_steps_per_sqr_second[0] = code_value() * axis_steps_per_unit[0]; - if(code_seen('Y')) axis_travel_steps_per_sqr_second[1] = code_value() * axis_steps_per_unit[1]; - if(code_seen('Z')) axis_travel_steps_per_sqr_second[2] = code_value() * axis_steps_per_unit[2]; - if(code_seen('E')) axis_travel_steps_per_sqr_second[3] = code_value() * axis_steps_per_unit[3]; + for(int i=0; i < NUM_AXIS; i++) { + if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; + } break; #endif } @@ -840,14 +836,10 @@ inline void ClearToSend() inline void get_coordinates() { - if(code_seen('X')) destination_x = (float)code_value() + relative_mode*current_x; - else destination_x = current_x; //Are these else lines really needed? - if(code_seen('Y')) destination_y = (float)code_value() + relative_mode*current_y; - else destination_y = current_y; - if(code_seen('Z')) destination_z = (float)code_value() + relative_mode*current_z; - else destination_z = current_z; - if(code_seen('E')) destination_e = (float)code_value() + (relative_mode_e || relative_mode)*current_e; - else destination_e = current_e; + for(int i=0; i < NUM_AXIS; i++) { + if(code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i]; + else destination[i] = current_position[i]; //Are these else lines really needed? + } if(code_seen('F')) { next_feedrate = code_value(); if(next_feedrate > 0.0) feedrate = next_feedrate; @@ -857,123 +849,91 @@ inline void get_coordinates() inline void prepare_move() { //Find direction - if(destination_x >= current_x) direction_x = 1; - else direction_x = 0; - if(destination_y >= current_y) direction_y = 1; - else direction_y = 0; - if(destination_z >= current_z) direction_z = 1; - else direction_z = 0; - if(destination_e >= current_e) direction_e = 1; - else direction_e = 0; + for(int i=0; i < NUM_AXIS; i++) { + if(destination[i] >= current_position[i]) move_direction[i] = 1; + else move_direction[i] = 0; + } if (min_software_endstops) { - if (destination_x < 0) destination_x = 0.0; - if (destination_y < 0) destination_y = 0.0; - if (destination_z < 0) destination_z = 0.0; + if (destination[0] < 0) destination[0] = 0.0; + if (destination[1] < 0) destination[1] = 0.0; + if (destination[2] < 0) destination[2] = 0.0; } if (max_software_endstops) { - if (destination_x > X_MAX_LENGTH) destination_x = X_MAX_LENGTH; - if (destination_y > Y_MAX_LENGTH) destination_y = Y_MAX_LENGTH; - if (destination_z > Z_MAX_LENGTH) destination_z = Z_MAX_LENGTH; + if (destination[0] > X_MAX_LENGTH) destination[0] = X_MAX_LENGTH; + if (destination[1] > Y_MAX_LENGTH) destination[1] = Y_MAX_LENGTH; + if (destination[2] > Z_MAX_LENGTH) destination[2] = Z_MAX_LENGTH; } - - if(feedrate > max_feedrate) feedrate = max_feedrate; - if(feedrate > max_z_feedrate) z_feedrate = max_z_feedrate; - else z_feedrate = feedrate; - - xdiff = (destination_x - current_x); - ydiff = (destination_y - current_y); - zdiff = (destination_z - current_z); - ediff = (destination_e - current_e); - x_steps_to_take = abs(xdiff) * axis_steps_per_unit[0]; - y_steps_to_take = abs(ydiff) * axis_steps_per_unit[1]; - z_steps_to_take = abs(zdiff) * axis_steps_per_unit[2]; - e_steps_to_take = abs(ediff) * axis_steps_per_unit[3]; + for(int i=0; i < NUM_AXIS; i++) { + axis_diff[i] = destination[i] - current_position[i]; + move_steps_to_take[i] = abs(axis_diff[i]) * axis_steps_per_unit[i]; + } if(feedrate < 10) feedrate = 10; - /* - //experimental feedrate calc - if(abs(xdiff) > 0.1 && abs(ydiff) > 0.1) - d = sqrt(xdiff * xdiff + ydiff * ydiff); - else if(abs(xdiff) > 0.1) - d = abs(xdiff); - else if(abs(ydiff) > 0.1) - d = abs(ydiff); - else if(abs(zdiff) > 0.05) - d = abs(zdiff); - else if(abs(ediff) > 0.1) - d = abs(ediff); - else d = 1; //extremely slow move, should be okay for moves under 0.1mm - time_for_move = (xdiff / (feedrate / 60000000) ); - //time = 60000000 * dist / feedrate - //int feedz = (60000000 * zdiff) / time_for_move; - //if(feedz > maxfeed) - */ - #define X_TIME_FOR_MOVE ((float)x_steps_to_take / (axis_steps_per_unit[0]*feedrate/60000000)) - #define Y_TIME_FOR_MOVE ((float)y_steps_to_take / (axis_steps_per_unit[1]*feedrate/60000000)) - #define Z_TIME_FOR_MOVE ((float)z_steps_to_take / (axis_steps_per_unit[2]*z_feedrate/60000000)) - #define E_TIME_FOR_MOVE ((float)e_steps_to_take / (axis_steps_per_unit[3]*feedrate/60000000)) - time_for_move = max(X_TIME_FOR_MOVE, Y_TIME_FOR_MOVE); - time_for_move = max(time_for_move, Z_TIME_FOR_MOVE); - if(time_for_move <= 0) time_for_move = max(time_for_move, E_TIME_FOR_MOVE); - - if(x_steps_to_take) axis_interval[0] = time_for_move / x_steps_to_take * 100; - if(y_steps_to_take) axis_interval[1] = time_for_move / y_steps_to_take * 100; - if(z_steps_to_take) axis_interval[2] = time_for_move / z_steps_to_take * 100; - if(e_steps_to_take && (x_steps_to_take + y_steps_to_take <= 0) ) axis_interval[3] = time_for_move / e_steps_to_take * 100; + //Feedrate calc based on XYZ travel distance + float xy_d; + if(abs(axis_diff[0]) > 0 || abs(axis_diff[1]) > 0 || abs(axis_diff[2])) { + xy_d = sqrt(axis_diff[0] * axis_diff[0] + axis_diff[1] * axis_diff[1]); + d = sqrt(xy_d * xy_d + axis_diff[2] * axis_diff[2]); + } + else if(abs(axis_diff[3]) > 0) + d = abs(axis_diff[3]); + #ifdef DEBUG_PREPARE_MOVE + else { + log_message("_PREPARE_MOVE - No steps to take!"); + } + #endif + time_for_move = (d / (feedrate / 60000000.0) ); + //Check max feedrate for each axis is not violated, update time_for_move if necessary + for(int i = 0; i < NUM_AXIS; i++) { + if(move_steps_to_take[i] && abs(axis_diff[i]) / (time_for_move / 60000000.0) > max_feedrate[i]) { + time_for_move = time_for_move / max_feedrate[i] * (abs(axis_diff[i]) / (time_for_move / 60000000.0)); + } + } + //Calculate the full speed stepper interval for each axis + for(int i=0; i < NUM_AXIS; i++) { + if(move_steps_to_take[i]) axis_interval[i] = time_for_move / move_steps_to_take[i] * 100; + } - #ifdef DEBUG_PREPARE_MOVE - Serial.print("destination_x: "); Serial.println(destination_x); - Serial.print("current_x: "); Serial.println(current_x); - Serial.print("x_steps_to_take: "); Serial.println(x_steps_to_take); - Serial.print("X_TIME_FOR_MVE: "); Serial.println(X_TIME_FOR_MOVE); - Serial.print("axis_interval[0]: "); Serial.println(axis_interval[0]); - Serial.println(""); - Serial.print("destination_y: "); Serial.println(destination_y); - Serial.print("current_y: "); Serial.println(current_y); - Serial.print("y_steps_to_take: "); Serial.println(y_steps_to_take); - Serial.print("Y_TIME_FOR_MVE: "); Serial.println(Y_TIME_FOR_MOVE); - Serial.print("axis_interval[1]: "); Serial.println(axis_interval[1]); - Serial.println(""); - Serial.print("destination_z: "); Serial.println(destination_z); - Serial.print("current_z: "); Serial.println(current_z); - Serial.print("z_steps_to_take: "); Serial.println(z_steps_to_take); - Serial.print("Z_TIME_FOR_MVE: "); Serial.println(Z_TIME_FOR_MOVE); - Serial.print("axis_interval[2]: "); Serial.println(axis_interval[2]); - Serial.println(""); - Serial.print("destination_e: "); Serial.println(destination_e); - Serial.print("current_e: "); Serial.println(current_e); - Serial.print("e_steps_to_take: "); Serial.println(e_steps_to_take); - Serial.print("E_TIME_FOR_MVE: "); Serial.println(E_TIME_FOR_MOVE); - Serial.print("axis_interval[3]: "); Serial.println(axis_interval[3]); - Serial.println(""); + #ifdef DEBUG_PREPARE_MOVE + log_float("_PREPARE_MOVE - Move distance on the XY plane", xy_d); + log_float("_PREPARE_MOVE - Move distance on the XYZ space", d); + log_float("_PREPARE_MOVE - Commanded feedrate", feedrate); + log_float("_PREPARE_MOVE - Constant full speed move time", time_for_move); + log_float_array("_PREPARE_MOVE - Destination", destination, NUM_AXIS); + log_float_array("_PREPARE_MOVE - Current position", current_position, NUM_AXIS); + log_ulong_array("_PREPARE_MOVE - Steps to take", move_steps_to_take, NUM_AXIS); + log_long_array("_PREPARE_MOVE - Axes full speed intervals", axis_interval, NUM_AXIS); #endif - unsigned long axis_steps_to_take[NUM_AXIS] = {x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take}; - linear_move(axis_steps_to_take); // make the move + + unsigned long move_steps[NUM_AXIS]; + for(int i=0; i < NUM_AXIS; i++) + move_steps[i] = move_steps_to_take[i]; + linear_move(move_steps); // make the move } void linear_move(unsigned long axis_steps_remaining[]) // make linear move with preset speeds and destinations, see G0 and G1 { //Determine direction of movement - if (destination_x > current_x) digitalWrite(X_DIR_PIN,!INVERT_X_DIR); + if (destination[0] > current_position[0]) digitalWrite(X_DIR_PIN,!INVERT_X_DIR); else digitalWrite(X_DIR_PIN,INVERT_X_DIR); - if (destination_y > current_y) digitalWrite(Y_DIR_PIN,!INVERT_Y_DIR); + if (destination[1] > current_position[1]) digitalWrite(Y_DIR_PIN,!INVERT_Y_DIR); else digitalWrite(Y_DIR_PIN,INVERT_Y_DIR); - if (destination_z > current_z) digitalWrite(Z_DIR_PIN,!INVERT_Z_DIR); + if (destination[2] > current_position[2]) digitalWrite(Z_DIR_PIN,!INVERT_Z_DIR); else digitalWrite(Z_DIR_PIN,INVERT_Z_DIR); - if (destination_e > current_e) digitalWrite(E_DIR_PIN,!INVERT_E_DIR); + if (destination[3] > current_position[3]) digitalWrite(E_DIR_PIN,!INVERT_E_DIR); else digitalWrite(E_DIR_PIN,INVERT_E_DIR); - if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; - if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; - if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; - if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; - if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; - if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; + if(X_MIN_PIN > -1) if(!move_direction[0]) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; + if(Y_MIN_PIN > -1) if(!move_direction[1]) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; + if(Z_MIN_PIN > -1) if(!move_direction[2]) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; + if(X_MAX_PIN > -1) if(move_direction[0]) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; + if(Y_MAX_PIN > -1) if(move_direction[1]) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; + if(Z_MAX_PIN > -1) if(move_direction[2]) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. @@ -999,6 +959,8 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #ifdef DEBUG_BRESENHAM log_int("_BRESENHAM - Primary axis", primary_axis); log_int("_BRESENHAM - Primary axis full speed interval", interval); + log_uint_array("_BRESENHAM - Deltas", delta, NUM_AXIS); + log_int_array("_BRESENHAM - Errors", axis_error, NUM_AXIS); #endif //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. @@ -1023,20 +985,17 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with } for(int i = 0; i < NUM_AXIS; i++) if(axis_steps_remaining[i] >0) { - new_axis_max_intervals[i] = slowest_start_axis_max_interval * axis_steps_remaining[i] / axis_steps_remaining[slowest_start_axis]; + new_axis_max_intervals[i] = slowest_start_axis_max_interval * axis_steps_remaining[slowest_start_axis] / axis_steps_remaining[i]; if(i == primary_axis) { max_interval = new_axis_max_intervals[i]; min_speed_steps_per_second = 100000000 / max_interval; } } - #ifdef DEBUG_RAMP_ACCELERATION - log_ulong_array("_RAMP_ACCELERATION - Actual step intervals at move start", new_axis_max_intervals, NUM_AXIS); - #endif //Calculate slowest axis plateau time float slowest_axis_plateau_time = 0; for(int i=0; i < NUM_AXIS ; i++) { if(axis_steps_remaining[i] > 0) { - if(e_steps_to_take > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, + if(move_steps_to_take[3] > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); else if(axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_travel_steps_per_sqr_second[i]); @@ -1045,11 +1004,16 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated steps_per_sqr_second = (100000000.0 / axis_interval[primary_axis] - 100000000.0 / new_axis_max_intervals[primary_axis]) / slowest_axis_plateau_time; plateau_steps = (long) ((steps_per_sqr_second / 2.0 * slowest_axis_plateau_time + min_speed_steps_per_second) * slowest_axis_plateau_time); + #ifdef DEBUG_RAMP_ACCELERATION + log_int("_RAMP_ACCELERATION - Start speed limiting axis", slowest_start_axis); + log_ulong("_RAMP_ACCELERATION - Limiting axis start interval", slowest_start_axis_max_interval); + log_ulong_array("_RAMP_ACCELERATION - Actual step intervals at move start", new_axis_max_intervals, NUM_AXIS); + #endif #endif #ifdef EXP_ACCELERATION unsigned long virtual_full_velocity_steps; unsigned long full_velocity_steps; - if(e_steps_to_take > 0) virtual_full_velocity_steps = axis_virtual_full_velocity_steps[primary_axis]; + if(move_steps_to_take[3] > 0) virtual_full_velocity_steps = axis_virtual_full_velocity_steps[primary_axis]; else virtual_full_velocity_steps = axis_travel_virtual_full_velocity_steps[primary_axis]; full_velocity_steps = min(virtual_full_velocity_steps, (delta[primary_axis] - axis_min_constant_speed_steps[primary_axis]) / 2); max_interval = axis_max_interval[primary_axis]; @@ -1163,12 +1127,12 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //If there are x or y steps remaining, perform Bresenham algorithm if(axis_steps_remaining[primary_axis]) { - if(X_MIN_PIN > -1) if(!direction_x) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(Y_MIN_PIN > -1) if(!direction_y) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(X_MAX_PIN > -1) if(direction_x) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; - if(Y_MAX_PIN > -1) if(direction_y) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; - if(Z_MIN_PIN > -1) if(!direction_z) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(Z_MAX_PIN > -1) if(direction_z) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; + if(X_MIN_PIN > -1) if(!move_direction[0]) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break; + if(Y_MIN_PIN > -1) if(!move_direction[1]) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; + if(X_MAX_PIN > -1) if(move_direction[0]) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; + if(Y_MAX_PIN > -1) if(move_direction[1]) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; + if(Z_MIN_PIN > -1) if(!move_direction[2]) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; + if(Z_MAX_PIN > -1) if(move_direction[2]) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; timediff = micros() * 100 - axis_previous_micros[primary_axis]; while(timediff >= interval && axis_steps_remaining[primary_axis] > 0) { steps_done++; @@ -1201,14 +1165,10 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(DISABLE_E) disable_e(); // Update current position partly based on direction, we probably can combine this with the direction code above... - if (destination_x > current_x) current_x = current_x + x_steps_to_take / axis_steps_per_unit[0]; - else current_x = current_x - x_steps_to_take / axis_steps_per_unit[0]; - if (destination_y > current_y) current_y = current_y + y_steps_to_take / axis_steps_per_unit[1]; - else current_y = current_y - y_steps_to_take / axis_steps_per_unit[1]; - if (destination_z > current_z) current_z = current_z + z_steps_to_take / axis_steps_per_unit[2]; - else current_z = current_z - z_steps_to_take / axis_steps_per_unit[2]; - if (destination_e > current_e) current_e = current_e + e_steps_to_take / axis_steps_per_unit[3]; - else current_e = current_e - e_steps_to_take / axis_steps_per_unit[3]; + for(int i=0; i < NUM_AXIS; i++) { + if (destination[i] > current_position[i]) current_position[i] = current_position[i] + move_steps_to_take[i] / axis_steps_per_unit[i]; + else current_position[i] = current_position[i] - move_steps_to_take[i] / axis_steps_per_unit[i]; + } } inline void do_step_update_micros(int axis) { @@ -1536,7 +1496,7 @@ if( (millis()-previous_millis_cmd) > stepper_inactive_time ) if(stepper_inactiv #ifdef DEBUG void log_message(char* message) { - Serial.println(message); + Serial.print("DEBUG"); Serial.println(message); } void log_int(char* message, int value) { -- cgit v1.2.1 From 2433b0459a2f8b9767d401773ca3992f7efcc253 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sun, 22 May 2011 20:19:56 +0200 Subject: Fixed bug that caused deltas in Bresenham to be cut and axis intervals to be negative in such case --- Tonokip_Firmware/Tonokip_Firmware.pde | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index f71d6d9..bf82ae3 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -945,8 +945,8 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(axis_steps_remaining[3]) enable_e(); //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. - unsigned int delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes - int axis_error[NUM_AXIS]; + unsigned long delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes + long axis_error[NUM_AXIS]; unsigned int primary_axis; if(delta[1] > delta[0] && delta[1] > delta[2] && delta[1] > delta[3]) primary_axis = 1; else if (delta[0] >= delta[1] && delta[0] > delta[2] && delta[0] > delta[3]) primary_axis = 0; @@ -959,8 +959,8 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #ifdef DEBUG_BRESENHAM log_int("_BRESENHAM - Primary axis", primary_axis); log_int("_BRESENHAM - Primary axis full speed interval", interval); - log_uint_array("_BRESENHAM - Deltas", delta, NUM_AXIS); - log_int_array("_BRESENHAM - Errors", axis_error, NUM_AXIS); + log_ulong_array("_BRESENHAM - Deltas", delta, NUM_AXIS); + log_long_array("_BRESENHAM - Errors", axis_error, NUM_AXIS); #endif //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. -- cgit v1.2.1 From 08e61b287f5cddce693a722a467bceeaadb60d2d Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sun, 5 Jun 2011 04:22:57 +0200 Subject: Added debugging code for heat management. Changed acceleration values to more reliable ones --- Tonokip_Firmware/Tonokip_Firmware.pde | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index bf82ae3..ced29d9 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -277,6 +277,7 @@ void setup() initsd(); #endif + } @@ -1255,6 +1256,10 @@ inline void manage_heater() previous_millis_heater = millis(); #ifdef HEATER_USES_THERMISTOR current_raw = analogRead(TEMP_0_PIN); + #ifdef DEBUG_HEAT_MGMT + log_int("_HEAT_MGMT - analogRead(TEMP_0_PIN)", current_raw); + log_int("_HEAT_MGMT - NUMTEMPS", NUMTEMPS); + #endif // When using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, // this switches it up so that the reading appears lower than target for the control logic. current_raw = 1023 - current_raw; @@ -1317,7 +1322,11 @@ inline void manage_heater() #ifdef BED_USES_THERMISTOR - current_bed_raw = analogRead(TEMP_1_PIN); + current_bed_raw = analogRead(TEMP_1_PIN); + #ifdef DEBUG_HEAT_MGMT + log_int("_HEAT_MGMT - analogRead(TEMP_1_PIN)", current_bed_raw); + log_int("_HEAT_MGMT - BNUMTEMPS", BNUMTEMPS); + #endif // If using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, // this switches it up so that the reading appears lower than target for the control logic. -- cgit v1.2.1 From 4689ab10ef12951a20efb1713478056d17332421 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sun, 5 Jun 2011 06:24:20 +0200 Subject: Added option to disable heat management during travel moves, on by default. This helps a lot in avoiding missing steps, hence increasing reliability --- Tonokip_Firmware/Tonokip_Firmware.pde | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index ced29d9..535716e 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -1058,14 +1058,20 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #ifdef DISABLE_CHECK_DURING_ACC if(!accelerating && !decelerating) { //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp - manage_heater(); + #ifdef DISABLE_CHECK_DURING_TRAVEL + if(delta[3] > 0) + #endif + manage_heater(); } #else #ifdef DISABLE_CHECK_DURING_MOVE {} //Do nothing #else //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp - manage_heater(); + #ifdef DISABLE_CHECK_DURING_TRAVEL + if(delta[3] > 0) + #endif + manage_heater(); #endif #endif #ifdef RAMP_ACCELERATION -- cgit v1.2.1 From a7ed8f90215ed948a5cba8e9f78b5d00c7b5c488 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sun, 5 Jun 2011 06:27:31 +0200 Subject: Exponential acceleration discontinued. Deleted all related code --- Tonokip_Firmware/Tonokip_Firmware.pde | 56 ----------------------------------- 1 file changed, 56 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 535716e..933b6d2 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -78,16 +78,6 @@ unsigned long move_steps_to_take[NUM_AXIS]; max_travel_acceleration_units_per_sq_second[3] * axis_steps_per_unit[3]}; unsigned long steps_per_sqr_second, plateau_steps; #endif -#ifdef EXP_ACCELERATION - unsigned long axis_virtual_full_velocity_steps[] = {full_velocity_units * axis_steps_per_unit[0], full_velocity_units * axis_steps_per_unit[1]}; - unsigned long axis_travel_virtual_full_velocity_steps[] = {travel_move_full_velocity_units * axis_steps_per_unit[0], - travel_move_full_velocity_units * axis_steps_per_unit[1]}; - unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), - 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1])}; - unsigned long max_interval; - unsigned long axis_min_constant_speed_steps[] = {min_constant_speed_units * axis_steps_per_unit[0], min_constant_speed_units * axis_steps_per_unit[1]}; - unsigned long min_constant_speed_steps; -#endif boolean acceleration_enabled = false, accelerating = false; unsigned long interval; float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; @@ -1011,15 +1001,6 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with log_ulong_array("_RAMP_ACCELERATION - Actual step intervals at move start", new_axis_max_intervals, NUM_AXIS); #endif #endif - #ifdef EXP_ACCELERATION - unsigned long virtual_full_velocity_steps; - unsigned long full_velocity_steps; - if(move_steps_to_take[3] > 0) virtual_full_velocity_steps = axis_virtual_full_velocity_steps[primary_axis]; - else virtual_full_velocity_steps = axis_travel_virtual_full_velocity_steps[primary_axis]; - full_velocity_steps = min(virtual_full_velocity_steps, (delta[primary_axis] - axis_min_constant_speed_steps[primary_axis]) / 2); - max_interval = axis_max_interval[primary_axis]; - min_constant_speed_steps = axis_min_constant_speed_steps[primary_axis]; - #endif unsigned long steps_done = 0; #ifdef RAMP_ACCELERATION @@ -1029,20 +1010,6 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(interval > max_interval) acceleration_enabled = false; boolean decelerating = false; #endif - #ifdef EXP_ACCELERATION - acceleration_enabled = true; - if(full_velocity_steps == 0) full_velocity_steps++; - if(interval > max_interval) acceleration_enabled = false; - unsigned long full_interval = interval; - if(min_constant_speed_steps >= steps_to_take) { - acceleration_enabled = false; - full_interval = max(max_interval, interval); // choose the min speed between feedrate and acceleration start speed - } - if(full_velocity_steps < virtual_full_velocity_steps && acceleration_enabled) full_interval = max(interval, - max_interval - ((max_interval - full_interval) * full_velocity_steps / virtual_full_velocity_steps)); // choose the min speed between feedrate and speed at full steps - unsigned int steps_acceleration_check = 1; - accelerating = acceleration_enabled; - #endif unsigned long start_move_micros = micros(); for(int i = 0; i < NUM_AXIS; i++) { @@ -1108,29 +1075,6 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with accelerating = false; } #endif - #ifdef EXP_ACCELERATION - //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval - // TODO: is this any useful? -> steps_done % steps_acceleration_check == 0 - if (acceleration_enabled && steps_done < full_velocity_steps && steps_done / full_velocity_steps < 1 && (steps_done % steps_acceleration_check == 0)) { - if(steps_done == 0) { - interval = max_interval; - } else { - interval = max_interval - ((max_interval - full_interval) * steps_done / virtual_full_velocity_steps); - } - } else if (acceleration_enabled && steps_remaining < full_velocity_steps) { - //Else, if acceleration is enabled on this move and we are in the deceleration segment, calculate the current interval - if(steps_remaining == 0) { - interval = max_interval; - } else { - interval = max_interval - ((max_interval - full_interval) * steps_remaining / virtual_full_velocity_steps); - } - accelerating = true; - } else if (steps_done - full_velocity_steps >= 1 || !acceleration_enabled){ - //Else, we are just use the full speed interval as current interval - interval = full_interval; - accelerating = false; - } - #endif //If there are x or y steps remaining, perform Bresenham algorithm if(axis_steps_remaining[primary_axis]) { -- cgit v1.2.1 From 6e246d4ead55f27d8e2942d7fa6b75726badae3b Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Sun, 5 Jun 2011 07:35:35 +0200 Subject: Heat management now performed between moves if DISABLE_CHECK_DURING_TRAVEL is enabled --- Tonokip_Firmware/Tonokip_Firmware.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 933b6d2..2530eda 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -449,7 +449,7 @@ inline void process_commands() { case 0: // G0 -> G1 case 1: // G1 - #ifdef DISABLE_CHECK_DURING_ACC || DISABLE_CHECK_DURING_MOVE + #ifdef DISABLE_CHECK_DURING_ACC || DISABLE_CHECK_DURING_MOVE || DISABLE_CHECK_DURING_TRAVEL manage_heater(); #endif get_coordinates(); // For X Y Z E F -- cgit v1.2.1 From c475cc2062969380e15358e8c0cd3f7654a64878 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Mon, 6 Jun 2011 18:49:34 +0200 Subject: Optimized print/travel move check, as it is performed in the bresenham loop --- Tonokip_Firmware/Tonokip_Firmware.pde | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 2530eda..c68fc29 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -947,6 +947,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with unsigned long steps_to_take = steps_remaining; for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis) axis_error[i] = delta[primary_axis] / 2; interval = axis_interval[primary_axis]; + bool is_print_move = delta[3] > 0; #ifdef DEBUG_BRESENHAM log_int("_BRESENHAM - Primary axis", primary_axis); log_int("_BRESENHAM - Primary axis full speed interval", interval); @@ -986,7 +987,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with float slowest_axis_plateau_time = 0; for(int i=0; i < NUM_AXIS ; i++) { if(axis_steps_remaining[i] > 0) { - if(move_steps_to_take[3] > 0 && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, + if(is_print_move && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); else if(axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_travel_steps_per_sqr_second[i]); @@ -1026,7 +1027,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with if(!accelerating && !decelerating) { //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp #ifdef DISABLE_CHECK_DURING_TRAVEL - if(delta[3] > 0) + if(is_print_move) #endif manage_heater(); } @@ -1036,7 +1037,7 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with #else //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp #ifdef DISABLE_CHECK_DURING_TRAVEL - if(delta[3] > 0) + if(is_print_move) #endif manage_heater(); #endif -- cgit v1.2.1 From 8c4f65709535b76e322516d6d7506eb430ca6d0b Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Mon, 6 Jun 2011 19:59:47 +0200 Subject: Added safety feature to DISABLE_CHECK_DURING_TRAVEL feature, which allows to define a max time in milliseconds after which the travel move is not considered so --- Tonokip_Firmware/Tonokip_Firmware.pde | 13 +++++++++++++ 1 file changed, 13 insertions(+) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index c68fc29..a5630a8 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -1017,6 +1017,15 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with axis_previous_micros[i] = start_move_micros * 100; } + #ifdef DISABLE_CHECK_DURING_TRAVEL + //If the move time is more than allowed in DISABLE_CHECK_DURING_TRAVEL, let's + // consider this a print move and perform heat management during it + if(time_for_move / 1000 > DISABLE_CHECK_DURING_TRAVEL) is_print_move = true; + #ifdef DEBUG_DISABLE_CHECK_DURING_TRAVEL + log_bool("_DISABLE_CHECK_DURING_TRAVEL - is_print_move", is_print_move); + #endif + #endif + #ifdef DEBUG_MOVE_TIME unsigned long startmove = micros(); #endif @@ -1459,6 +1468,10 @@ void log_message(char* message) { Serial.print("DEBUG"); Serial.println(message); } +void log_bool(char* message, bool value) { + Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); +} + void log_int(char* message, int value) { Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); } -- cgit v1.2.1 From 57c05dde4219454c9d3a21bfc31a824141674cd5 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Wed, 22 Jun 2011 02:03:11 +0200 Subject: Now heat check is also disabled during retract moves, if DISABLE_CHECK_DURING_TRAVEL is enabled --- Tonokip_Firmware/Tonokip_Firmware.pde | 2 ++ 1 file changed, 2 insertions(+) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index a5630a8..1957cf9 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -1021,6 +1021,8 @@ void linear_move(unsigned long axis_steps_remaining[]) // make linear move with //If the move time is more than allowed in DISABLE_CHECK_DURING_TRAVEL, let's // consider this a print move and perform heat management during it if(time_for_move / 1000 > DISABLE_CHECK_DURING_TRAVEL) is_print_move = true; + //else, if the move is a retract, consider it as a travel move for the sake of this feature + else if(delta[3]>0 && delta[0] + delta[1] + delta[2] == 0) is_print_move = false; #ifdef DEBUG_DISABLE_CHECK_DURING_TRAVEL log_bool("_DISABLE_CHECK_DURING_TRAVEL - is_print_move", is_print_move); #endif -- cgit v1.2.1 From 4642d041d5daf0f59d588c8b2d6c0696a5ad64d0 Mon Sep 17 00:00:00 2001 From: Emanuele Caruso Date: Wed, 22 Jun 2011 02:21:34 +0200 Subject: Fixed merge bug, I forgot to declare the new variable home_all_axis --- Tonokip_Firmware/Tonokip_Firmware.pde | 1 + 1 file changed, 1 insertion(+) (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index cb8e552..4528324 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -83,6 +83,7 @@ unsigned long interval; float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; long axis_interval[NUM_AXIS]; // for speed delay +bool home_all_axis = true; float feedrate = 1500, next_feedrate, saved_feedrate; float time_for_move; long gcode_N, gcode_LastN; -- cgit v1.2.1 From e4af5e82f26fab59bb2bda540e1673317f48ce8a Mon Sep 17 00:00:00 2001 From: kliment Date: Mon, 4 Jul 2011 16:17:19 +0200 Subject: Rename to Sprinter --- Tonokip_Firmware/Tonokip_Firmware.pde | 1548 --------------------------------- 1 file changed, 1548 deletions(-) delete mode 100644 Tonokip_Firmware/Tonokip_Firmware.pde (limited to 'Tonokip_Firmware/Tonokip_Firmware.pde') diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde deleted file mode 100644 index 4528324..0000000 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ /dev/null @@ -1,1548 +0,0 @@ -// Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. -// Licence: GPL - -#include "Tonokip_Firmware.h" -#include "configuration.h" -#include "pins.h" - -#ifdef SDSUPPORT -#include "SdFat.h" -#endif - -// look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html -// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes - -//Implemented Codes -//------------------- -// G0 -> G1 -// G1 - Coordinated Movement X Y Z E -// G4 - Dwell S or P -// G28 - Home all Axis -// G90 - Use Absolute Coordinates -// G91 - Use Relative Coordinates -// G92 - Set current position to cordinates given - -//RepRap M Codes -// M104 - Set extruder target temp -// M105 - Read current temp -// M106 - Fan on -// M107 - Fan off -// M109 - Wait for extruder current temp to reach target temp. -// M114 - Display current position - -//Custom M Codes -// M80 - Turn on Power Supply -// M20 - List SD card -// M21 - Init SD card -// M22 - Release SD card -// M23 - Select SD file (M23 filename.g) -// M24 - Start/resume SD print -// M25 - Pause SD print -// M26 - Set SD position in bytes (M26 S12345) -// M27 - Report SD print status -// M28 - Start SD write (M28 filename.g) -// M29 - Stop SD write -// M81 - Turn off Power Supply -// M82 - Set E codes absolute (default) -// M83 - Set E codes relative while in Absolute Coordinates (G90) mode -// M84 - Disable steppers until next move, -// or use S to specify an inactivity timeout, after which the steppers will be disabled. S0 to disable the timeout. -// M85 - Set inactivity shutdown timer with parameter S. To disable set zero (default) -// M92 - Set axis_steps_per_unit - same syntax as G92 -// M115 - Capabilities string -// M140 - Set bed target temp -// M190 - Wait for bed current temp to reach target temp. -// M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000) -// M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) - - -//Stepper Movement Variables -char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'}; -bool move_direction[NUM_AXIS]; -const int STEP_PIN[NUM_AXIS] = {X_STEP_PIN, Y_STEP_PIN, Z_STEP_PIN, E_STEP_PIN}; -unsigned long axis_previous_micros[NUM_AXIS]; -unsigned long previous_micros = 0, previous_millis_heater, previous_millis_bed_heater; -unsigned long move_steps_to_take[NUM_AXIS]; -#ifdef RAMP_ACCELERATION - unsigned long axis_max_interval[] = {100000000.0 / (max_start_speed_units_per_second[0] * axis_steps_per_unit[0]), - 100000000.0 / (max_start_speed_units_per_second[1] * axis_steps_per_unit[1]), - 100000000.0 / (max_start_speed_units_per_second[2] * axis_steps_per_unit[2]), - 100000000.0 / (max_start_speed_units_per_second[3] * axis_steps_per_unit[3])}; //TODO: refactor all things like this in a function, or move to setup() - // in a for loop - unsigned long max_interval; - unsigned long axis_steps_per_sqr_second[] = {max_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], - max_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2], - max_acceleration_units_per_sq_second[3] * axis_steps_per_unit[3]}; - unsigned long axis_travel_steps_per_sqr_second[] = {max_travel_acceleration_units_per_sq_second[0] * axis_steps_per_unit[0], - max_travel_acceleration_units_per_sq_second[1] * axis_steps_per_unit[1], max_travel_acceleration_units_per_sq_second[2] * axis_steps_per_unit[2], - max_travel_acceleration_units_per_sq_second[3] * axis_steps_per_unit[3]}; - unsigned long steps_per_sqr_second, plateau_steps; -#endif -boolean acceleration_enabled = false, accelerating = false; -unsigned long interval; -float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; -float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0}; -long axis_interval[NUM_AXIS]; // for speed delay -bool home_all_axis = true; -float feedrate = 1500, next_feedrate, saved_feedrate; -float time_for_move; -long gcode_N, gcode_LastN; -bool relative_mode = false; //Determines Absolute or Relative Coordinates -bool relative_mode_e = false; //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode. -long timediff = 0; -//experimental feedrate calc -float d = 0; -float axis_diff[NUM_AXIS] = {0, 0, 0, 0}; -#ifdef STEP_DELAY_RATIO - long long_step_delay_ratio = STEP_DELAY_RATIO * 100; -#endif - - -// comm variables -#define MAX_CMD_SIZE 96 -#define BUFSIZE 8 -char cmdbuffer[BUFSIZE][MAX_CMD_SIZE]; -bool fromsd[BUFSIZE]; -int bufindr = 0; -int bufindw = 0; -int buflen = 0; -int i = 0; -char serial_char; -int serial_count = 0; -boolean comment_mode = false; -char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc - -// Manage heater variables. For a thermistor or AD595 thermocouple, raw values refer to the -// reading from the analog pin. For a MAX6675 thermocouple, the raw value is the temperature in 0.25 -// degree increments (i.e. 100=25 deg). - -int target_raw = 0; -int current_raw = 0; -int target_bed_raw = 0; -int current_bed_raw = 0; -float tt = 0, bt = 0; -#ifdef PIDTEMP - int temp_iState = 0; - int temp_dState = 0; - int pTerm; - int iTerm; - int dTerm; - //int output; - int error; - int temp_iState_min = 100 * -PID_INTEGRAL_DRIVE_MAX / PID_IGAIN; - int temp_iState_max = 100 * PID_INTEGRAL_DRIVE_MAX / PID_IGAIN; -#endif -#ifdef SMOOTHING - uint32_t nma = SMOOTHFACTOR * analogRead(TEMP_0_PIN); -#endif -#ifdef WATCHPERIOD - int watch_raw = -1000; - unsigned long watchmillis = 0; -#endif -#ifdef MINTEMP - int minttemp = temp2analog(MINTEMP); -#endif -#ifdef MAXTEMP -int maxttemp = temp2analog(MAXTEMP); -#endif - -//Inactivity shutdown variables -unsigned long previous_millis_cmd = 0; -unsigned long max_inactive_time = 0; -unsigned long stepper_inactive_time = 0; - -#ifdef SDSUPPORT - Sd2Card card; - SdVolume volume; - SdFile root; - SdFile file; - uint32_t filesize = 0; - uint32_t sdpos = 0; - bool sdmode = false; - bool sdactive = false; - bool savetosd = false; - int16_t n; - - void initsd(){ - sdactive = false; - #if SDSS >- 1 - if(root.isOpen()) - root.close(); - if (!card.init(SPI_FULL_SPEED,SDSS)){ - //if (!card.init(SPI_HALF_SPEED,SDSS)) - Serial.println("SD init fail"); - } - else if (!volume.init(&card)) - Serial.println("volume.init failed"); - else if (!root.openRoot(&volume)) - Serial.println("openRoot failed"); - else - sdactive = true; - #endif - } - - inline void write_command(char *buf){ - char* begin = buf; - char* npos = 0; - char* end = buf + strlen(buf) - 1; - - file.writeError = false; - if((npos = strchr(buf, 'N')) != NULL){ - begin = strchr(npos, ' ') + 1; - end = strchr(npos, '*') - 1; - } - end[1] = '\r'; - end[2] = '\n'; - end[3] = '\0'; - //Serial.println(begin); - file.write(begin); - if (file.writeError){ - Serial.println("error writing to file"); - } - } -#endif - - -void setup() -{ - Serial.begin(BAUDRATE); - Serial.println("start"); - for(int i = 0; i < BUFSIZE; i++){ - fromsd[i] = false; - } - - //Initialize Step Pins - for(int i=0; i < NUM_AXIS; i++) if(STEP_PIN[i] > -1) pinMode(STEP_PIN[i],OUTPUT); - - //Initialize Dir Pins - if(X_DIR_PIN > -1) pinMode(X_DIR_PIN,OUTPUT); - if(Y_DIR_PIN > -1) pinMode(Y_DIR_PIN,OUTPUT); - if(Z_DIR_PIN > -1) pinMode(Z_DIR_PIN,OUTPUT); - if(E_DIR_PIN > -1) pinMode(E_DIR_PIN,OUTPUT); - - //Steppers default to disabled. - if(X_ENABLE_PIN > -1) if(!X_ENABLE_ON) digitalWrite(X_ENABLE_PIN,HIGH); - if(Y_ENABLE_PIN > -1) if(!Y_ENABLE_ON) digitalWrite(Y_ENABLE_PIN,HIGH); - if(Z_ENABLE_PIN > -1) if(!Z_ENABLE_ON) digitalWrite(Z_ENABLE_PIN,HIGH); - if(E_ENABLE_PIN > -1) if(!E_ENABLE_ON) digitalWrite(E_ENABLE_PIN,HIGH); - - //endstop pullups - #ifdef ENDSTOPPULLUPS - if(X_MIN_PIN > -1) { pinMode(X_MIN_PIN,INPUT); digitalWrite(X_MIN_PIN,HIGH);} - if(Y_MIN_PIN > -1) { pinMode(Y_MIN_PIN,INPUT); digitalWrite(Y_MIN_PIN,HIGH);} - if(Z_MIN_PIN > -1) { pinMode(Z_MIN_PIN,INPUT); digitalWrite(Z_MIN_PIN,HIGH);} - if(X_MAX_PIN > -1) { pinMode(X_MAX_PIN,INPUT); digitalWrite(X_MAX_PIN,HIGH);} - if(Y_MAX_PIN > -1) { pinMode(Y_MAX_PIN,INPUT); digitalWrite(Y_MAX_PIN,HIGH);} - if(Z_MAX_PIN > -1) { pinMode(Z_MAX_PIN,INPUT); digitalWrite(Z_MAX_PIN,HIGH);} - #endif - //Initialize Enable Pins - if(X_ENABLE_PIN > -1) pinMode(X_ENABLE_PIN,OUTPUT); - if(Y_ENABLE_PIN > -1) pinMode(Y_ENABLE_PIN,OUTPUT); - if(Z_ENABLE_PIN > -1) pinMode(Z_ENABLE_PIN,OUTPUT); - if(E_ENABLE_PIN > -1) pinMode(E_ENABLE_PIN,OUTPUT); - - if(HEATER_0_PIN > -1) pinMode(HEATER_0_PIN,OUTPUT); - if(HEATER_1_PIN > -1) pinMode(HEATER_1_PIN,OUTPUT); - -#ifdef HEATER_USES_MAX6675 - digitalWrite(SCK_PIN,0); - pinMode(SCK_PIN,OUTPUT); - - digitalWrite(MOSI_PIN,1); - pinMode(MOSI_PIN,OUTPUT); - - digitalWrite(MISO_PIN,1); - pinMode(MISO_PIN,INPUT); - - digitalWrite(MAX6675_SS,1); - pinMode(MAX6675_SS,OUTPUT); -#endif - -#ifdef SDSUPPORT - - //power to SD reader - #if SDPOWER > -1 - pinMode(SDPOWER,OUTPUT); - digitalWrite(SDPOWER,HIGH); - #endif - initsd(); - -#endif - -} - - -void loop() -{ - if(buflen<3) - get_command(); - - if(buflen){ -#ifdef SDSUPPORT - if(savetosd){ - if(strstr(cmdbuffer[bufindr],"M29") == NULL){ - write_command(cmdbuffer[bufindr]); - Serial.println("ok"); - }else{ - file.sync(); - file.close(); - savetosd = false; - Serial.println("Done saving file."); - } - }else{ - process_commands(); - } -#else - process_commands(); -#endif - buflen = (buflen-1); - bufindr = (bufindr + 1)%BUFSIZE; - } - //check heater every n milliseconds - manage_heater(); - manage_inactivity(1); - } - - -inline void get_command() -{ - while( Serial.available() > 0 && buflen < BUFSIZE) { - serial_char = Serial.read(); - if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) ) - { - if(!serial_count) return; //if empty line - cmdbuffer[bufindw][serial_count] = 0; //terminate string - if(!comment_mode){ - fromsd[bufindw] = false; - if(strstr(cmdbuffer[bufindw], "N") != NULL) - { - strchr_pointer = strchr(cmdbuffer[bufindw], 'N'); - gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10)); - if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) ) { - Serial.print("Serial Error: Line Number is not Last Line Number+1, Last Line:"); - Serial.println(gcode_LastN); - //Serial.println(gcode_N); - FlushSerialRequestResend(); - serial_count = 0; - return; - } - - if(strstr(cmdbuffer[bufindw], "*") != NULL) - { - byte checksum = 0; - byte count = 0; - while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++]; - strchr_pointer = strchr(cmdbuffer[bufindw], '*'); - - if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) { - Serial.print("Error: checksum mismatch, Last Line:"); - Serial.println(gcode_LastN); - FlushSerialRequestResend(); - serial_count = 0; - return; - } - //if no errors, continue parsing - } - else - { - Serial.print("Error: No Checksum with line number, Last Line:"); - Serial.println(gcode_LastN); - FlushSerialRequestResend(); - serial_count = 0; - return; - } - - gcode_LastN = gcode_N; - //if no errors, continue parsing - } - else // if we don't receive 'N' but still see '*' - { - if((strstr(cmdbuffer[bufindw], "*") != NULL)) - { - Serial.print("Error: No Line Number with checksum, Last Line:"); - Serial.println(gcode_LastN); - serial_count = 0; - return; - } - } - if((strstr(cmdbuffer[bufindw], "G") != NULL)){ - strchr_pointer = strchr(cmdbuffer[bufindw], 'G'); - switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){ - case 0: - case 1: - #ifdef SDSUPPORT - if(savetosd) - break; - #endif - Serial.println("ok"); - break; - default: - break; - } - - } - bufindw = (bufindw + 1)%BUFSIZE; - buflen += 1; - - } - comment_mode = false; //for new command - serial_count = 0; //clear buffer - } - else - { - if(serial_char == ';') comment_mode = true; - if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char; - } - } -#ifdef SDSUPPORT -if(!sdmode || serial_count!=0){ - return; -} - while( filesize > sdpos && buflen < BUFSIZE) { - n = file.read(); - serial_char = (char)n; - if(serial_char == '\n' || serial_char == '\r' || serial_char == ':' || serial_count >= (MAX_CMD_SIZE - 1) || n == -1) - { - sdpos = file.curPosition(); - if(sdpos >= filesize){ - sdmode = false; - Serial.println("Done printing file"); - } - if(!serial_count) return; //if empty line - cmdbuffer[bufindw][serial_count] = 0; //terminate string - if(!comment_mode){ - fromsd[bufindw] = true; - buflen += 1; - bufindw = (bufindw + 1)%BUFSIZE; - } - comment_mode = false; //for new command - serial_count = 0; //clear buffer - } - else - { - if(serial_char == ';') comment_mode = true; - if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char; - } -} -#endif - -} - - -inline float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); } -inline long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); } -inline bool code_seen(char code_string[]) { return (strstr(cmdbuffer[bufindr], code_string) != NULL); } //Return True if the string was found - -inline bool code_seen(char code) -{ - strchr_pointer = strchr(cmdbuffer[bufindr], code); - return (strchr_pointer != NULL); //Return True if a character was found -} - -inline void process_commands() -{ - unsigned long codenum; //throw away variable - char *starpos = NULL; - - if(code_seen('G')) - { - switch((int)code_value()) - { - case 0: // G0 -> G1 - case 1: // G1 - #ifdef DISABLE_CHECK_DURING_ACC || DISABLE_CHECK_DURING_MOVE || DISABLE_CHECK_DURING_TRAVEL - manage_heater(); - #endif - get_coordinates(); // For X Y Z E F - prepare_move(); - previous_millis_cmd = millis(); - //ClearToSend(); - return; - //break; - case 4: // G4 dwell - codenum = 0; - if(code_seen('P')) codenum = code_value(); // milliseconds to wait - if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait - codenum += millis(); // keep track of when we started waiting - while(millis() < codenum ){ - manage_heater(); - } - break; - case 28: //G28 Home all Axis one at a time - saved_feedrate = feedrate; - for(int i=0; i < NUM_AXIS; i++) { - destination[i] = 0; - current_position[i] = 0; - } - feedrate = 0; - - home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2]))); - - if((home_all_axis) || (code_seen('X'))) { - if((X_MIN_PIN > -1 && X_HOME_DIR==-1) || (X_MAX_PIN > -1 && X_HOME_DIR==1)) { - current_position[0] = 0; - destination[0] = 1.5 * X_MAX_LENGTH * X_HOME_DIR; - feedrate = max_start_speed_units_per_second[0] * 60; - prepare_move(); - - current_position[0] = 0; - destination[0] = -1 * X_HOME_DIR; - prepare_move(); - - destination[0] = 10 * X_HOME_DIR; - prepare_move(); - - current_position[0] = 0; - destination[0] = 0; - feedrate = 0; - } - } - - if((home_all_axis) || (code_seen('X'))) { - if((Y_MIN_PIN > -1 && Y_HOME_DIR==-1) || (Y_MAX_PIN > -1 && Y_HOME_DIR==1)) { - current_position[1] = 0; - destination[1] = 1.5 * Y_MAX_LENGTH * Y_HOME_DIR; - feedrate = max_start_speed_units_per_second[1] * 60; - prepare_move(); - - current_position[1] = 0; - destination[1] = -1 * Y_HOME_DIR; - prepare_move(); - - destination[1] = 10 * Y_HOME_DIR; - prepare_move(); - - current_position[1] = 0; - destination[1] = 0; - feedrate = 0; - } - } - - if((home_all_axis) || (code_seen('X'))) { - if((Z_MIN_PIN > -1 && Z_HOME_DIR==-1) || (Z_MAX_PIN > -1 && Z_HOME_DIR==1)) { - current_position[2] = 0; - destination[2] = 1.5 * Z_MAX_LENGTH * Z_HOME_DIR; - feedrate = max_feedrate[2]/2; - prepare_move(); - - current_position[2] = 0; - destination[2] = -1 * Z_HOME_DIR; - prepare_move(); - - destination[2] = 10 * Z_HOME_DIR; - prepare_move(); - - current_position[2] = 0; - destination[2] = 0; - feedrate = 0; - } - } - - feedrate = saved_feedrate; - previous_millis_cmd = millis(); - break; - case 90: // G90 - relative_mode = false; - break; - case 91: // G91 - relative_mode = true; - break; - case 92: // G92 - for(int i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) current_position[i] = code_value(); - } - break; - - } - } - - else if(code_seen('M')) - { - - switch( (int)code_value() ) - { -#ifdef SDSUPPORT - - case 20: // M20 - list SD card - Serial.println("Begin file list"); - root.ls(); - Serial.println("End file list"); - break; - case 21: // M21 - init SD card - sdmode = false; - initsd(); - break; - case 22: //M22 - release SD card - sdmode = false; - sdactive = false; - break; - case 23: //M23 - Select file - if(sdactive){ - sdmode = false; - file.close(); - starpos = (strchr(strchr_pointer + 4,'*')); - if(starpos!=NULL) - *(starpos-1)='\0'; - if (file.open(&root, strchr_pointer + 4, O_READ)) { - Serial.print("File opened:"); - Serial.print(strchr_pointer + 4); - Serial.print(" Size:"); - Serial.println(file.fileSize()); - sdpos = 0; - filesize = file.fileSize(); - Serial.println("File selected"); - } - else{ - Serial.println("file.open failed"); - } - } - break; - case 24: //M24 - Start SD print - if(sdactive){ - sdmode = true; - } - break; - case 25: //M25 - Pause SD print - if(sdmode){ - sdmode = false; - } - break; - case 26: //M26 - Set SD index - if(sdactive && code_seen('S')){ - sdpos = code_value_long(); - file.seekSet(sdpos); - } - break; - case 27: //M27 - Get SD status - if(sdactive){ - Serial.print("SD printing byte "); - Serial.print(sdpos); - Serial.print("/"); - Serial.println(filesize); - }else{ - Serial.println("Not SD printing"); - } - break; - case 28: //M28 - Start SD write - if(sdactive){ - char* npos = 0; - file.close(); - sdmode = false; - starpos = (strchr(strchr_pointer + 4,'*')); - if(starpos != NULL){ - npos = strchr(cmdbuffer[bufindr], 'N'); - strchr_pointer = strchr(npos,' ') + 1; - *(starpos-1) = '\0'; - } - if (!file.open(&root, strchr_pointer+4, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) - { - Serial.print("open failed, File: "); - Serial.print(strchr_pointer + 4); - Serial.print("."); - }else{ - savetosd = true; - Serial.print("Writing to file: "); - Serial.println(strchr_pointer + 4); - } - } - break; - case 29: //M29 - Stop SD write - //processed in write to file routine above - //savetosd = false; - break; -#endif - case 104: // M104 - if (code_seen('S')) target_raw = temp2analog(code_value()); - #ifdef WATCHPERIOD - if(target_raw > current_raw){ - watchmillis = max(1,millis()); - watch_raw = current_raw; - }else{ - watchmillis = 0; - } - #endif - break; - case 140: // M140 set bed temp - if (code_seen('S')) target_bed_raw = temp2analogBed(code_value()); - break; - case 105: // M105 - #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675) - tt = analog2temp(current_raw); - #endif - #if TEMP_1_PIN > -1 - bt = analog2tempBed(current_bed_raw); - #endif - #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX6675) - Serial.print("ok T:"); - Serial.print(tt); - #if TEMP_1_PIN > -1 - Serial.print(" B:"); - Serial.println(bt); - #else - Serial.println(); - #endif - #else - Serial.println("No thermistors - no temp"); - #endif - return; - //break; - case 109: // M109 - Wait for extruder heater to reach target. - if (code_seen('S')) target_raw = temp2analog(code_value()); - #ifdef WATCHPERIOD - if(target_raw>current_raw){ - watchmillis = max(1,millis()); - watch_raw = current_raw; - }else{ - watchmillis = 0; - } - #endif - codenum = millis(); - while(current_raw < target_raw) { - if( (millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up. - { - Serial.print("T:"); - Serial.println( analog2temp(current_raw) ); - codenum = millis(); - } - manage_heater(); - } - break; - case 190: // M190 - Wait bed for heater to reach target. - #if TEMP_1_PIN > -1 - if (code_seen('S')) target_bed_raw = temp2analog(code_value()); - codenum = millis(); - while(current_bed_raw < target_bed_raw) { - if( (millis()-codenum) > 1000 ) //Print Temp Reading every 1 second while heating up. - { - tt=analog2temp(current_raw); - Serial.print("T:"); - Serial.println( tt ); - Serial.print("ok T:"); - Serial.print( tt ); - Serial.print(" B:"); - Serial.println( analog2temp(current_bed_raw) ); - codenum = millis(); - } - manage_heater(); - } - #endif - break; - case 106: //M106 Fan On - if (code_seen('S')){ - digitalWrite(FAN_PIN, HIGH); - analogWrite(FAN_PIN, constrain(code_value(),0,255) ); - } - else - digitalWrite(FAN_PIN, HIGH); - break; - case 107: //M107 Fan Off - analogWrite(FAN_PIN, 0); - - digitalWrite(FAN_PIN, LOW); - break; - case 80: // M81 - ATX Power On - if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,OUTPUT); //GND - break; - case 81: // M81 - ATX Power Off - if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT); //Floating - break; - case 82: - axis_relative_modes[3] = false; - break; - case 83: - axis_relative_modes[3] = true; - break; - case 84: - if(code_seen('S')){ stepper_inactive_time = code_value() * 1000; } - else{ disable_x(); disable_y(); disable_z(); disable_e(); } - break; - case 85: // M85 - code_seen('S'); - max_inactive_time = code_value() * 1000; - break; - case 92: // M92 - for(int i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value(); - } - - //Update start speed intervals and axis order. TODO: refactor axis_max_interval[] calculation into a function, as it - // should also be used in setup() as well - #ifdef RAMP_ACCELERATION - long temp_max_intervals[NUM_AXIS]; - for(int i=0; i < NUM_AXIS; i++) { - axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);//TODO: do this for - // all steps_per_unit related variables - } - #endif - break; - case 115: // M115 - Serial.println("FIRMWARE_NAME:Sprinter FIRMWARE_URL:http%%3A/github.com/kliment/Sprinter/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1"); - break; - case 114: // M114 - Serial.print("X:"); - Serial.print(current_position[0]); - Serial.print("Y:"); - Serial.print(current_position[1]); - Serial.print("Z:"); - Serial.print(current_position[2]); - Serial.print("E:"); - Serial.println(current_position[3]); - break; - #ifdef RAMP_ACCELERATION - //TODO: update for all axis, use for loop - case 201: // M201 - for(int i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; - } - break; - case 202: // M202 - for(int i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; - } - break; - #endif - } - - } - else{ - Serial.println("Unknown command:"); - Serial.println(cmdbuffer[bufindr]); - } - - ClearToSend(); - -} - -inline void FlushSerialRequestResend() -{ - //char cmdbuffer[bufindr][100]="Resend:"; - Serial.flush(); - Serial.print("Resend:"); - Serial.println(gcode_LastN + 1); - ClearToSend(); -} - -inline void ClearToSend() -{ - previous_millis_cmd = millis(); - #ifdef SDSUPPORT - if(fromsd[bufindr]) - return; - #endif - Serial.println("ok"); -} - -inline void get_coordinates() -{ - for(int i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i]; - else destination[i] = current_position[i]; //Are these else lines really needed? - } - if(code_seen('F')) { - next_feedrate = code_value(); - if(next_feedrate > 0.0) feedrate = next_feedrate; - } -} - -inline void prepare_move() -{ - //Find direction - for(int i=0; i < NUM_AXIS; i++) { - if(destination[i] >= current_position[i]) move_direction[i] = 1; - else move_direction[i] = 0; - } - - - if (min_software_endstops) { - if (destination[0] < 0) destination[0] = 0.0; - if (destination[1] < 0) destination[1] = 0.0; - if (destination[2] < 0) destination[2] = 0.0; - } - - if (max_software_endstops) { - if (destination[0] > X_MAX_LENGTH) destination[0] = X_MAX_LENGTH; - if (destination[1] > Y_MAX_LENGTH) destination[1] = Y_MAX_LENGTH; - if (destination[2] > Z_MAX_LENGTH) destination[2] = Z_MAX_LENGTH; - } - - for(int i=0; i < NUM_AXIS; i++) { - axis_diff[i] = destination[i] - current_position[i]; - move_steps_to_take[i] = abs(axis_diff[i]) * axis_steps_per_unit[i]; - } - if(feedrate < 10) - feedrate = 10; - - //Feedrate calc based on XYZ travel distance - float xy_d; - if(abs(axis_diff[0]) > 0 || abs(axis_diff[1]) > 0 || abs(axis_diff[2])) { - xy_d = sqrt(axis_diff[0] * axis_diff[0] + axis_diff[1] * axis_diff[1]); - d = sqrt(xy_d * xy_d + axis_diff[2] * axis_diff[2]); - } - else if(abs(axis_diff[3]) > 0) - d = abs(axis_diff[3]); - #ifdef DEBUG_PREPARE_MOVE - else { - log_message("_PREPARE_MOVE - No steps to take!"); - } - #endif - time_for_move = (d / (feedrate / 60000000.0) ); - //Check max feedrate for each axis is not violated, update time_for_move if necessary - for(int i = 0; i < NUM_AXIS; i++) { - if(move_steps_to_take[i] && abs(axis_diff[i]) / (time_for_move / 60000000.0) > max_feedrate[i]) { - time_for_move = time_for_move / max_feedrate[i] * (abs(axis_diff[i]) / (time_for_move / 60000000.0)); - } - } - //Calculate the full speed stepper interval for each axis - for(int i=0; i < NUM_AXIS; i++) { - if(move_steps_to_take[i]) axis_interval[i] = time_for_move / move_steps_to_take[i] * 100; - } - - #ifdef DEBUG_PREPARE_MOVE - log_float("_PREPARE_MOVE - Move distance on the XY plane", xy_d); - log_float("_PREPARE_MOVE - Move distance on the XYZ space", d); - log_float("_PREPARE_MOVE - Commanded feedrate", feedrate); - log_float("_PREPARE_MOVE - Constant full speed move time", time_for_move); - log_float_array("_PREPARE_MOVE - Destination", destination, NUM_AXIS); - log_float_array("_PREPARE_MOVE - Current position", current_position, NUM_AXIS); - log_ulong_array("_PREPARE_MOVE - Steps to take", move_steps_to_take, NUM_AXIS); - log_long_array("_PREPARE_MOVE - Axes full speed intervals", axis_interval, NUM_AXIS); - #endif - - unsigned long move_steps[NUM_AXIS]; - for(int i=0; i < NUM_AXIS; i++) - move_steps[i] = move_steps_to_take[i]; - linear_move(move_steps); // make the move -} - -void linear_move(unsigned long axis_steps_remaining[]) // make linear move with preset speeds and destinations, see G0 and G1 -{ - //Determine direction of movement - if (destination[0] > current_position[0]) digitalWrite(X_DIR_PIN,!INVERT_X_DIR); - else digitalWrite(X_DIR_PIN,INVERT_X_DIR); - if (destination[1] > current_position[1]) digitalWrite(Y_DIR_PIN,!INVERT_Y_DIR); - else digitalWrite(Y_DIR_PIN,INVERT_Y_DIR); - if (destination[2] > current_position[2]) digitalWrite(Z_DIR_PIN,!INVERT_Z_DIR); - else digitalWrite(Z_DIR_PIN,INVERT_Z_DIR); - if (destination[3] > current_position[3]) digitalWrite(E_DIR_PIN,!INVERT_E_DIR); - else digitalWrite(E_DIR_PIN,INVERT_E_DIR); - - if(X_MIN_PIN > -1) if(!move_direction[0]) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; - if(Y_MIN_PIN > -1) if(!move_direction[1]) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; - if(Z_MIN_PIN > -1) if(!move_direction[2]) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; - if(X_MAX_PIN > -1) if(move_direction[0]) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[0]=0; - if(Y_MAX_PIN > -1) if(move_direction[1]) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[1]=0; - if(Z_MAX_PIN > -1) if(move_direction[2]) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) axis_steps_remaining[2]=0; - - - //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. - // TODO: maybe it's better to refactor into a generic enable(int axis) function, that will probably take more ram, - // but will reduce code size - if(axis_steps_remaining[0]) enable_x(); - if(axis_steps_remaining[1]) enable_y(); - if(axis_steps_remaining[2]) enable_z(); - if(axis_steps_remaining[3]) enable_e(); - - //Define variables that are needed for the Bresenham algorithm. Please note that Z is not currently included in the Bresenham algorithm. - unsigned long delta[] = {axis_steps_remaining[0], axis_steps_remaining[1], axis_steps_remaining[2], axis_steps_remaining[3]}; //TODO: implement a "for" to support N axes - long axis_error[NUM_AXIS]; - unsigned int primary_axis; - if(delta[1] > delta[0] && delta[1] > delta[2] && delta[1] > delta[3]) primary_axis = 1; - else if (delta[0] >= delta[1] && delta[0] > delta[2] && delta[0] > delta[3]) primary_axis = 0; - else if (delta[2] >= delta[0] && delta[2] >= delta[1] && delta[2] > delta[3]) primary_axis = 2; - else primary_axis = 3; - unsigned long steps_remaining = delta[primary_axis]; - unsigned long steps_to_take = steps_remaining; - for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis) axis_error[i] = delta[primary_axis] / 2; - interval = axis_interval[primary_axis]; - bool is_print_move = delta[3] > 0; - #ifdef DEBUG_BRESENHAM - log_int("_BRESENHAM - Primary axis", primary_axis); - log_int("_BRESENHAM - Primary axis full speed interval", interval); - log_ulong_array("_BRESENHAM - Deltas", delta, NUM_AXIS); - log_long_array("_BRESENHAM - Errors", axis_error, NUM_AXIS); - #endif - - //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it. - #ifdef RAMP_ACCELERATION - long max_speed_steps_per_second; - long min_speed_steps_per_second; - max_interval = axis_max_interval[primary_axis]; - #ifdef DEBUG_RAMP_ACCELERATION - log_ulong_array("_RAMP_ACCELERATION - Teoric step intervals at move start", axis_max_interval, NUM_AXIS); - #endif - unsigned long new_axis_max_intervals[NUM_AXIS]; - max_speed_steps_per_second = 100000000 / interval; - min_speed_steps_per_second = 100000000 / max_interval; //TODO: can this be deleted? - //Calculate start speeds based on moving axes max start speed constraints. - int slowest_start_axis = primary_axis; - unsigned long slowest_start_axis_max_interval = max_interval; - for(int i = 0; i < NUM_AXIS; i++) - if (axis_steps_remaining[i] >0 && i != primary_axis && axis_max_interval[i] * axis_steps_remaining[i] - / axis_steps_remaining[slowest_start_axis] > slowest_start_axis_max_interval) { - slowest_start_axis = i; - slowest_start_axis_max_interval = axis_max_interval[i]; - } - for(int i = 0; i < NUM_AXIS; i++) - if(axis_steps_remaining[i] >0) { - new_axis_max_intervals[i] = slowest_start_axis_max_interval * axis_steps_remaining[slowest_start_axis] / axis_steps_remaining[i]; - if(i == primary_axis) { - max_interval = new_axis_max_intervals[i]; - min_speed_steps_per_second = 100000000 / max_interval; - } - } - //Calculate slowest axis plateau time - float slowest_axis_plateau_time = 0; - for(int i=0; i < NUM_AXIS ; i++) { - if(axis_steps_remaining[i] > 0) { - if(is_print_move && axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, - (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_steps_per_sqr_second[i]); - else if(axis_steps_remaining[i] > 0) slowest_axis_plateau_time = max(slowest_axis_plateau_time, - (100000000.0 / axis_interval[i] - 100000000.0 / new_axis_max_intervals[i]) / (float) axis_travel_steps_per_sqr_second[i]); - } - } - //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated - steps_per_sqr_second = (100000000.0 / axis_interval[primary_axis] - 100000000.0 / new_axis_max_intervals[primary_axis]) / slowest_axis_plateau_time; - plateau_steps = (long) ((steps_per_sqr_second / 2.0 * slowest_axis_plateau_time + min_speed_steps_per_second) * slowest_axis_plateau_time); - #ifdef DEBUG_RAMP_ACCELERATION - log_int("_RAMP_ACCELERATION - Start speed limiting axis", slowest_start_axis); - log_ulong("_RAMP_ACCELERATION - Limiting axis start interval", slowest_start_axis_max_interval); - log_ulong_array("_RAMP_ACCELERATION - Actual step intervals at move start", new_axis_max_intervals, NUM_AXIS); - #endif - #endif - - unsigned long steps_done = 0; - #ifdef RAMP_ACCELERATION - plateau_steps *= 1.01; // This is to compensate we use discrete intervals - acceleration_enabled = true; - long full_interval = interval; - if(interval > max_interval) acceleration_enabled = false; - boolean decelerating = false; - #endif - - unsigned long start_move_micros = micros(); - for(int i = 0; i < NUM_AXIS; i++) { - axis_previous_micros[i] = start_move_micros * 100; - } - - #ifdef DISABLE_CHECK_DURING_TRAVEL - //If the move time is more than allowed in DISABLE_CHECK_DURING_TRAVEL, let's - // consider this a print move and perform heat management during it - if(time_for_move / 1000 > DISABLE_CHECK_DURING_TRAVEL) is_print_move = true; - //else, if the move is a retract, consider it as a travel move for the sake of this feature - else if(delta[3]>0 && delta[0] + delta[1] + delta[2] == 0) is_print_move = false; - #ifdef DEBUG_DISABLE_CHECK_DURING_TRAVEL - log_bool("_DISABLE_CHECK_DURING_TRAVEL - is_print_move", is_print_move); - #endif - #endif - - #ifdef DEBUG_MOVE_TIME - unsigned long startmove = micros(); - #endif - - //move until no more steps remain - while(axis_steps_remaining[0] + axis_steps_remaining[1] + axis_steps_remaining[2] + axis_steps_remaining[3] > 0) { - #ifdef DISABLE_CHECK_DURING_ACC - if(!accelerating && !decelerating) { - //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp - #ifdef DISABLE_CHECK_DURING_TRAVEL - if(is_print_move) - #endif - manage_heater(); - } - #else - #ifdef DISABLE_CHECK_DURING_MOVE - {} //Do nothing - #else - //If more that HEATER_CHECK_INTERVAL ms have passed since previous heating check, adjust temp - #ifdef DISABLE_CHECK_DURING_TRAVEL - if(is_print_move) - #endif - manage_heater(); - #endif - #endif - #ifdef RAMP_ACCELERATION - //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval - if (acceleration_enabled && steps_done == 0) { - interval = max_interval; - } else if (acceleration_enabled && steps_done <= plateau_steps) { - long current_speed = (long) ((((long) steps_per_sqr_second) / 10000) - * ((micros() - start_move_micros) / 100) + (long) min_speed_steps_per_second); - interval = 100000000 / current_speed; - if (interval < full_interval) { - accelerating = false; - interval = full_interval; - } - if (steps_done >= steps_to_take / 2) { - plateau_steps = steps_done; - max_speed_steps_per_second = 100000000 / interval; - accelerating = false; - } - } else if (acceleration_enabled && steps_remaining <= plateau_steps) { //(interval > minInterval * 100) { - if (!accelerating) { - start_move_micros = micros(); - accelerating = true; - decelerating = true; - } - long current_speed = (long) ((long) max_speed_steps_per_second - ((((long) steps_per_sqr_second) / 10000) - * ((micros() - start_move_micros) / 100))); - interval = 100000000 / current_speed; - if (interval > max_interval) - interval = max_interval; - } else { - //Else, we are just use the full speed interval as current interval - interval = full_interval; - accelerating = false; - } - #endif - - //If there are x or y steps remaining, perform Bresenham algorithm - if(axis_steps_remaining[primary_axis]) { - if(X_MIN_PIN > -1) if(!move_direction[0]) if(digitalRead(X_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(Y_MIN_PIN > -1) if(!move_direction[1]) if(digitalRead(Y_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(X_MAX_PIN > -1) if(move_direction[0]) if(digitalRead(X_MAX_PIN) != ENDSTOPS_INVERTING) break; - if(Y_MAX_PIN > -1) if(move_direction[1]) if(digitalRead(Y_MAX_PIN) != ENDSTOPS_INVERTING) break; - if(Z_MIN_PIN > -1) if(!move_direction[2]) if(digitalRead(Z_MIN_PIN) != ENDSTOPS_INVERTING) break; - if(Z_MAX_PIN > -1) if(move_direction[2]) if(digitalRead(Z_MAX_PIN) != ENDSTOPS_INVERTING) break; - timediff = micros() * 100 - axis_previous_micros[primary_axis]; - while(timediff >= interval && axis_steps_remaining[primary_axis] > 0) { - steps_done++; - steps_remaining--; - axis_steps_remaining[primary_axis]--; timediff -= interval; - do_step_update_micros(primary_axis); - for(int i=0; i < NUM_AXIS; i++) if(i != primary_axis && axis_steps_remaining[i] > 0) { - axis_error[i] = axis_error[i] - delta[i]; - if(axis_error[i] < 0) { - do_step(i); axis_steps_remaining[i]--; - axis_error[i] = axis_error[i] + delta[primary_axis]; - } - } - #ifdef STEP_DELAY_RATIO - if(timediff >= interval) delayMicroseconds(long_step_delay_ratio * interval / 10000); - #endif - #ifdef STEP_DELAY_MICROS - if(timediff >= interval) delayMicroseconds(STEP_DELAY_MICROS); - #endif - } - } - } - #ifdef DEBUG_MOVE_TIME - log_ulong("_MOVE_TIME - This move took", micros()-startmove); - #endif - - if(DISABLE_X) disable_x(); - if(DISABLE_Y) disable_y(); - if(DISABLE_Z) disable_z(); - if(DISABLE_E) disable_e(); - - // Update current position partly based on direction, we probably can combine this with the direction code above... - for(int i=0; i < NUM_AXIS; i++) { - if (destination[i] > current_position[i]) current_position[i] = current_position[i] + move_steps_to_take[i] / axis_steps_per_unit[i]; - else current_position[i] = current_position[i] - move_steps_to_take[i] / axis_steps_per_unit[i]; - } -} - -inline void do_step_update_micros(int axis) { - digitalWrite(STEP_PIN[axis], HIGH); - axis_previous_micros[axis] += interval; - digitalWrite(STEP_PIN[axis], LOW); -} - -inline void do_step(int axis) { - digitalWrite(STEP_PIN[axis], HIGH); - digitalWrite(STEP_PIN[axis], LOW); -} - -inline void disable_x() { if(X_ENABLE_PIN > -1) digitalWrite(X_ENABLE_PIN,!X_ENABLE_ON); } -inline void disable_y() { if(Y_ENABLE_PIN > -1) digitalWrite(Y_ENABLE_PIN,!Y_ENABLE_ON); } -inline void disable_z() { if(Z_ENABLE_PIN > -1) digitalWrite(Z_ENABLE_PIN,!Z_ENABLE_ON); } -inline void disable_e() { if(E_ENABLE_PIN > -1) digitalWrite(E_ENABLE_PIN,!E_ENABLE_ON); } -inline void enable_x() { if(X_ENABLE_PIN > -1) digitalWrite(X_ENABLE_PIN, X_ENABLE_ON); } -inline void enable_y() { if(Y_ENABLE_PIN > -1) digitalWrite(Y_ENABLE_PIN, Y_ENABLE_ON); } -inline void enable_z() { if(Z_ENABLE_PIN > -1) digitalWrite(Z_ENABLE_PIN, Z_ENABLE_ON); } -inline void enable_e() { if(E_ENABLE_PIN > -1) digitalWrite(E_ENABLE_PIN, E_ENABLE_ON); } - -#define HEAT_INTERVAL 250 -#ifdef HEATER_USES_MAX6675 -unsigned long max6675_previous_millis = 0; -int max6675_temp = 2000; - -inline int read_max6675() -{ - if (millis() - max6675_previous_millis < HEAT_INTERVAL) - return max6675_temp; - - max6675_previous_millis = millis(); - - max6675_temp = 0; - - #ifdef PRR - PRR &= ~(1<> 3; - } - - return max6675_temp; -} -#endif - - -inline void manage_heater() -{ - if((millis() - previous_millis_heater) < HEATER_CHECK_INTERVAL ) - return; - previous_millis_heater = millis(); - #ifdef HEATER_USES_THERMISTOR - current_raw = analogRead(TEMP_0_PIN); - #ifdef DEBUG_HEAT_MGMT - log_int("_HEAT_MGMT - analogRead(TEMP_0_PIN)", current_raw); - log_int("_HEAT_MGMT - NUMTEMPS", NUMTEMPS); - #endif - // When using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, - // this switches it up so that the reading appears lower than target for the control logic. - current_raw = 1023 - current_raw; - #elif defined HEATER_USES_AD595 - current_raw = analogRead(TEMP_0_PIN); - #elif defined HEATER_USES_MAX6675 - current_raw = read_max6675(); - #endif - #ifdef SMOOTHING - nma = (nma + current_raw) - (nma / SMOOTHFACTOR); - current_raw = nma / SMOOTHFACTOR; - #endif - #ifdef WATCHPERIOD - if(watchmillis && millis() - watchmillis > WATCHPERIOD){ - if(watch_raw + 1 >= current_raw){ - target_raw = 0; - digitalWrite(HEATER_0_PIN,LOW); - digitalWrite(LED_PIN,LOW); - }else{ - watchmillis = 0; - } - } - #endif - #ifdef MINTEMP - if(current_raw <= minttemp) - target_raw = 0; - #endif - #ifdef MAXTEMP - if(current_raw >= maxttemp) { - target_raw = 0; - } - #endif - #if (TEMP_0_PIN > -1) || defined (HEATER_USES_MAX66675) - #ifdef PIDTEMP - error = target_raw - current_raw; - pTerm = (PID_PGAIN * error) / 100; - temp_iState += error; - temp_iState = constrain(temp_iState, temp_iState_min, temp_iState_max); - iTerm = (PID_IGAIN * temp_iState) / 100; - dTerm = (PID_DGAIN * (current_raw - temp_dState)) / 100; - temp_dState = current_raw; - analogWrite(HEATER_0_PIN, constrain(pTerm + iTerm - dTerm, 0, PID_MAX)); - #else - if(current_raw >= target_raw) - { - digitalWrite(HEATER_0_PIN,LOW); - digitalWrite(LED_PIN,LOW); - } - else - { - digitalWrite(HEATER_0_PIN,HIGH); - digitalWrite(LED_PIN,HIGH); - } - #endif - #endif - - if(millis() - previous_millis_bed_heater < BED_CHECK_INTERVAL) - return; - previous_millis_bed_heater = millis(); - - #ifdef BED_USES_THERMISTOR - - current_bed_raw = analogRead(TEMP_1_PIN); - #ifdef DEBUG_HEAT_MGMT - log_int("_HEAT_MGMT - analogRead(TEMP_1_PIN)", current_bed_raw); - log_int("_HEAT_MGMT - BNUMTEMPS", BNUMTEMPS); - #endif - - // If using thermistor, when the heater is colder than targer temp, we get a higher analog reading than target, - // this switches it up so that the reading appears lower than target for the control logic. - current_bed_raw = 1023 - current_bed_raw; - #elif defined BED_USES_AD595 - current_bed_raw = analogRead(TEMP_1_PIN); - - #endif - - - #if TEMP_1_PIN > -1 - if(current_bed_raw >= target_bed_raw) - { - digitalWrite(HEATER_1_PIN,LOW); - } - else - { - digitalWrite(HEATER_1_PIN,HIGH); - } - #endif -} - -// Takes hot end temperature value as input and returns corresponding raw value. -// For a thermistor, it uses the RepRap thermistor temp table. -// This is needed because PID in hydra firmware hovers around a given analog value, not a temp value. -// This function is derived from inversing the logic from a portion of getTemperature() in FiveD RepRap firmware. -float temp2analog(int celsius) { - #ifdef HEATER_USES_THERMISTOR - int raw = 0; - byte i; - - for (i=1; i raw) - { - celsius = temptable[i-1][1] + - (raw - temptable[i-1][0]) * - (temptable[i][1] - temptable[i-1][1]) / - (temptable[i][0] - temptable[i-1][0]); - - break; - } - } - - // Overflow: Set to last value in the table - if (i == NUMTEMPS) celsius = temptable[i-1][1]; - - return celsius; - #elif defined HEATER_USES_AD595 - return raw * ((5.0 * 100.0) / 1024.0); - #elif defined HEATER_USES_MAX6675 - return raw * 0.25; - #endif -} - -// Derived from RepRap FiveD extruder::getTemperature() -// For bed temperature measurement. -float analog2tempBed(int raw) { - #ifdef BED_USES_THERMISTOR - int celsius = 0; - byte i; - - raw = 1023 - raw; - - for (i=1; i raw) - { - celsius = bedtemptable[i-1][1] + - (raw - bedtemptable[i-1][0]) * - (bedtemptable[i][1] - bedtemptable[i-1][1]) / - (bedtemptable[i][0] - bedtemptable[i-1][0]); - - break; - } - } - - // Overflow: Set to last value in the table - if (i == NUMTEMPS) celsius = bedtemptable[i-1][1]; - - return celsius; - - #elif defined BED_USES_AD595 - return raw * ((5.0 * 100.0) / 1024.0); - #endif -} - -inline void kill() -{ - #if TEMP_0_PIN > -1 - target_raw=0; - digitalWrite(HEATER_0_PIN,LOW); - #endif - #if TEMP_1_PIN > -1 - target_bed_raw=0; - if(HEATER_1_PIN > -1) digitalWrite(HEATER_1_PIN,LOW); - #endif - disable_x(); - disable_y(); - disable_z(); - disable_e(); - - if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT); - -} - -inline void manage_inactivity(byte debug) { -if( (millis()-previous_millis_cmd) > max_inactive_time ) if(max_inactive_time) kill(); -if( (millis()-previous_millis_cmd) > stepper_inactive_time ) if(stepper_inactive_time) { disable_x(); disable_y(); disable_z(); disable_e(); } -} - -#ifdef DEBUG -void log_message(char* message) { - Serial.print("DEBUG"); Serial.println(message); -} - -void log_bool(char* message, bool value) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); -} - -void log_int(char* message, int value) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); -} - -void log_long(char* message, long value) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); -} - -void log_float(char* message, float value) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); -} - -void log_uint(char* message, unsigned int value) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); -} - -void log_ulong(char* message, unsigned long value) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value); -} - -void log_int_array(char* message, int value[], int array_lenght) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); - for(int i=0; i < array_lenght; i++){ - Serial.print(value[i]); - if(i != array_lenght-1) Serial.print(", "); - } - Serial.println("}"); -} - -void log_long_array(char* message, long value[], int array_lenght) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); - for(int i=0; i < array_lenght; i++){ - Serial.print(value[i]); - if(i != array_lenght-1) Serial.print(", "); - } - Serial.println("}"); -} - -void log_float_array(char* message, float value[], int array_lenght) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); - for(int i=0; i < array_lenght; i++){ - Serial.print(value[i]); - if(i != array_lenght-1) Serial.print(", "); - } - Serial.println("}"); -} - -void log_uint_array(char* message, unsigned int value[], int array_lenght) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); - for(int i=0; i < array_lenght; i++){ - Serial.print(value[i]); - if(i != array_lenght-1) Serial.print(", "); - } - Serial.println("}"); -} - -void log_ulong_array(char* message, unsigned long value[], int array_lenght) { - Serial.print("DEBUG"); Serial.print(message); Serial.print(": {"); - for(int i=0; i < array_lenght; i++){ - Serial.print(value[i]); - if(i != array_lenght-1) Serial.print(", "); - } - Serial.println("}"); -} -#endif -- cgit v1.2.1