From 0d4dadba39250196fbfab2d3fc2a354b52e7d971 Mon Sep 17 00:00:00 2001 From: Sam Ward Date: Wed, 11 May 2011 21:08:09 +0800 Subject: Added G28 homing code. Re factored main movement routines --- Tonokip_Firmware/Tonokip_Firmware.pde | 223 ++++++++++++++++++++++------------ 1 file changed, 146 insertions(+), 77 deletions(-) diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 4dcd2aa..24557a2 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -264,15 +264,11 @@ void setup() initsd(); #endif - - } void loop() { - - if(buflen<3) get_command(); @@ -453,78 +449,7 @@ inline void process_commands() case 0: // G0 -> G1 case 1: // G1 get_coordinates(); // For X Y Z E F - 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)*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; - 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 / (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)) - - 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) 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; - - //#define DEBUGGING false - #if 0 - if(0) { - 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("x_interval: "); Serial.println(x_interval); - 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.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.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.println(""); - } - #endif - linear_move(x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take); // make the move + prepare_move(); previous_millis_cmd = millis(); //ClearToSend(); return; @@ -536,6 +461,73 @@ inline void process_commands() previous_millis_heater = millis(); // keep track of when we started waiting while((millis() - previous_millis_heater) < codenum ) manage_heater(); //manage heater until time is up break; + case 28: //G28 Home all Axis one at a time + destination_x = 0; + current_x = 0; + destination_y = 0; + current_y = 0; + destination_z = 0; + current_z = 0; + destination_e = 0; + current_e = 0; + feedrate = 0; + + if(X_MIN_PIN > -1) { + current_x = 0; + destination_x = -250; + feedrate = min_units_per_second*60; + prepare_move(); + + current_x = 0; + destination_x = 1; + prepare_move(); + + destination_x = -10; + prepare_move(); + + current_x = 0; + destination_x = 0; + feedrate = 0; + } + + if(Y_MIN_PIN > -1) { + current_y = 0; + destination_y = -250; + feedrate = min_units_per_second*60; + prepare_move(); + + current_y = 0; + destination_y = 1; + prepare_move(); + + destination_y = -10; + prepare_move(); + + current_y = 0; + destination_y = 0; + feedrate = 0; + } + + if(Z_MIN_PIN > -1) { + current_z = 0; + destination_z = -250; + feedrate = max_z_feedrate/2; + prepare_move(); + + current_z = 0; + destination_z = 1; + prepare_move(); + + destination_z = -10; + prepare_move(); + + current_z = 0; + destination_z = 0; + feedrate = 0; + } + + previous_millis_cmd = millis(); + break; case 90: // G90 relative_mode = false; break; @@ -836,7 +828,10 @@ inline void get_coordinates() next_feedrate = code_value(); if(next_feedrate > 0.0) feedrate = next_feedrate; } - +} + +void prepare_move() +{ //Find direction if(destination_x >= current_x) direction_x=1; else direction_x=0; @@ -864,6 +859,80 @@ inline void get_coordinates() 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)*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; + 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 / (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)) + + 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) 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; + + //#define DEBUGGING false + #if 0 + if(0) { + 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("x_interval: "); Serial.println(x_interval); + 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.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.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.println(""); + } + #endif + + linear_move(x_steps_to_take, y_steps_to_take, z_steps_to_take, e_steps_to_take); // make the move } 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 838eb694109cf62a0bc2b9236596abc623f354a7 Mon Sep 17 00:00:00 2001 From: Sam Ward Date: Wed, 11 May 2011 21:20:54 +0800 Subject: Added some more intelligent homing logic --- Tonokip_Firmware/Tonokip_Firmware.pde | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index 24557a2..d5b3b41 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -84,7 +84,7 @@ 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 -float feedrate = 1500, next_feedrate, z_feedrate; +float feedrate = 1500, next_feedrate, z_feedrate, saved_feedrate; float time_for_move; long gcode_N, gcode_LastN; bool relative_mode = false; //Determines Absolute or Relative Coordinates @@ -462,6 +462,7 @@ inline void process_commands() while((millis() - previous_millis_heater) < codenum ) manage_heater(); //manage heater until time is up break; case 28: //G28 Home all Axis one at a time + saved_feedrate = feedrate; destination_x = 0; current_x = 0; destination_y = 0; @@ -474,7 +475,7 @@ inline void process_commands() if(X_MIN_PIN > -1) { current_x = 0; - destination_x = -250; + destination_x = -1.5 * X_MAX_LENGTH; feedrate = min_units_per_second*60; prepare_move(); @@ -492,7 +493,7 @@ inline void process_commands() if(Y_MIN_PIN > -1) { current_y = 0; - destination_y = -250; + destination_y = -1.5 * Y_MAX_LENGTH; feedrate = min_units_per_second*60; prepare_move(); @@ -510,7 +511,7 @@ inline void process_commands() if(Z_MIN_PIN > -1) { current_z = 0; - destination_z = -250; + destination_z = -1.5 * Z_MAX_LENGTH; feedrate = max_z_feedrate/2; prepare_move(); @@ -525,7 +526,8 @@ inline void process_commands() destination_z = 0; feedrate = 0; } - + + feedrate = saved_feedrate; previous_millis_cmd = millis(); break; case 90: // G90 -- cgit v1.2.1 From 3b3af86ec6a68ebd7ed79f6ddf5026d8c7c6aa5e Mon Sep 17 00:00:00 2001 From: Sam Ward Date: Thu, 12 May 2011 13:07:52 +0800 Subject: Fixed prepare_move function prototype and inlining --- Tonokip_Firmware/Tonokip_Firmware.h | 1 + Tonokip_Firmware/Tonokip_Firmware.pde | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Tonokip_Firmware/Tonokip_Firmware.h b/Tonokip_Firmware/Tonokip_Firmware.h index 65bfccd..4e3c7ea 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.h +++ b/Tonokip_Firmware/Tonokip_Firmware.h @@ -17,6 +17,7 @@ void FlushSerialRequestResend(); void ClearToSend(); void get_coordinates(); +void prepare_move(); void linear_move(unsigned long x_steps_remaining, unsigned long y_steps_remaining, unsigned long z_steps_remaining, unsigned long e_steps_remaining); void disable_x(); void disable_y(); diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index d5b3b41..d4a28d9 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -832,7 +832,7 @@ inline void get_coordinates() } } -void prepare_move() +inline void prepare_move() { //Find direction if(destination_x >= current_x) direction_x=1; -- cgit v1.2.1 From 17ac6123014baf933f202dcad75f7a47308bc31d Mon Sep 17 00:00:00 2001 From: Sam Ward Date: Thu, 12 May 2011 14:36:19 +0800 Subject: Added G28 command description to top of file. --- Tonokip_Firmware/Tonokip_Firmware.pde | 1 + 1 file changed, 1 insertion(+) diff --git a/Tonokip_Firmware/Tonokip_Firmware.pde b/Tonokip_Firmware/Tonokip_Firmware.pde index d4a28d9..06a8011 100644 --- a/Tonokip_Firmware/Tonokip_Firmware.pde +++ b/Tonokip_Firmware/Tonokip_Firmware.pde @@ -17,6 +17,7 @@ // 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 -- cgit v1.2.1