diff options
| -rw-r--r-- | Sprinter/Configuration.h | 68 | ||||
| -rw-r--r-- | Sprinter/Sprinter.h | 31 | ||||
| -rw-r--r-- | Sprinter/Sprinter.pde | 1301 |
3 files changed, 905 insertions, 495 deletions
diff --git a/Sprinter/Configuration.h b/Sprinter/Configuration.h index 25b3be6..86b7115 100644 --- a/Sprinter/Configuration.h +++ b/Sprinter/Configuration.h @@ -56,8 +56,13 @@ const bool Z_ENDSTOP_INVERT = false; // Uncomment to make run init.g from SD on boot //#define SDINITFILE +//Only work with Atmega1284 you need +1 kb ram //#define SD_FAST_XFER_AKTIV +//Uncomment to aktivate the arc (circle) function (G2/G3 Command) +//Without SD function an ARC function the used Flash is smaller 31 kb +#define USE_ARC_FUNCTION + //----------------------------------------------------------------------- //// ADVANCED SETTINGS - to tweak parameters //----------------------------------------------------------------------- @@ -69,8 +74,9 @@ const bool Z_ENDSTOP_INVERT = false; #endif #endif - +//----------------------------------------------------------------------- // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1 +//----------------------------------------------------------------------- #define X_ENABLE_ON 0 #define Y_ENABLE_ON 0 #define Z_ENABLE_ON 0 @@ -79,40 +85,57 @@ const bool Z_ENDSTOP_INVERT = false; //Uncomment if you have problems with a stepper driver enabeling too late, this will also set how many microseconds delay there will be after enabeling the driver //#define DELAY_ENABLE 15 +//----------------------------------------------------------------------- // Disables axis when it's not being used. +//----------------------------------------------------------------------- const bool DISABLE_X = false; const bool DISABLE_Y = false; const bool DISABLE_Z = true; const bool DISABLE_E = false; +//----------------------------------------------------------------------- // Inverting axis direction +//----------------------------------------------------------------------- const bool INVERT_X_DIR = false; const bool INVERT_Y_DIR = false; const bool INVERT_Z_DIR = true; const bool INVERT_E_DIR = false; - +//----------------------------------------------------------------------- //// ENDSTOP SETTINGS: +//----------------------------------------------------------------------- // Sets direction of endstops when homing; 1=MAX, -1=MIN #define X_HOME_DIR -1 #define Y_HOME_DIR -1 #define Z_HOME_DIR -1 +//#define ENDSTOPS_ONLY_FOR_HOMING // If defined the endstops will only be used for homing const bool min_software_endstops = false; //If true, axis won't move to coordinates less than zero. const bool max_software_endstops = true; //If true, axis won't move to coordinates greater than the defined lengths below. + +//----------------------------------------------------------------------- //Max Length for Prusa Mendel, check the ways of your axis and set this Values +//----------------------------------------------------------------------- const int X_MAX_LENGTH = 200; const int Y_MAX_LENGTH = 200; const int Z_MAX_LENGTH = 100; +//----------------------------------------------------------------------- //// MOVEMENT SETTINGS +//----------------------------------------------------------------------- const int NUM_AXIS = 4; // The axis order in all axis related arrays is X, Y, Z, E -#define _MAX_FEEDRATE {200000, 200000, 240, 500000} -#define _HOMING_FEEDRATE {1500,1500,120} +#define _MAX_FEEDRATE {400, 400, 4, 45} // (mm/sec) +#define _HOMING_FEEDRATE {1500,1500,120} // (mm/min) !! #define _AXIS_RELATIVE_MODES {false, false, false, false} +#define MAX_STEP_FREQUENCY 30000 // Max step frequency + + +//----------------------------------------------------------------------- +//// Not used at the Moment +//----------------------------------------------------------------------- // Min step delay in microseconds. If you are experiencing missing steps, try to raise the delay microseconds, but be aware this // If you enable this, make sure STEP_DELAY_RATIO is disabled. @@ -128,23 +151,35 @@ const int NUM_AXIS = 4; // The axis order in all axis related arrays is X, Y, Z, long min_time_before_dir_change = 30; //milliseconds #endif -// Comment this to disable ramp acceleration -#define RAMP_ACCELERATION - +//----------------------------------------------------------------------- //// Acceleration settings -#ifdef RAMP_ACCELERATION +//----------------------------------------------------------------------- // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for skeinforge 40+, for older versions raise them a lot. -#define _ACCELERATION 2000 // Normal acceleration mm/s^2 -#define _RETRACT_ACCELERATION 7000 // Normal acceleration mm/s^2 -#define _MAX_XY_JERK (20.0*60) -#define _MAX_Z_JERK (0.4*60) -#define _MAX_START_SPEED_UNITS_PER_SECOND {25.0,25.0,0.2,10.0} -#define _MAX_ACCELERATION_UNITS_PER_SQ_SECOND {500,500,50,500} // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts -#define _MAX_TRAVEL_ACCELERATION_UNITS_PER_SQ_SECOND {500,500,50,500} // X, Y, Z max acceleration in mm/s^2 for travel moves -#endif +#define _ACCELERATION 1000 // Axis Normal acceleration mm/s^2 +#define _RETRACT_ACCELERATION 2000 // Extruder Normal acceleration mm/s^2 +#define _MAX_XY_JERK 20.0 +#define _MAX_Z_JERK 0.4 +//#define _MAX_START_SPEED_UNITS_PER_SECOND {25.0,25.0,0.2,10.0} +#define _MAX_ACCELERATION_UNITS_PER_SQ_SECOND {5000,5000,50,5000} // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec) + +#define DEFAULT_MINIMUMFEEDRATE 0.0 // minimum feedrate +#define DEFAULT_MINTRAVELFEEDRATE 0.0 + +// If defined the movements slow down when the look ahead buffer is only half full +#define SLOWDOWN + + +const int dropsegments=5; //everything with less than this number of steps will be ignored as move and joined with the next movement + +//----------------------------------------------------------------------- // Machine UUID +//----------------------------------------------------------------------- // This may be useful if you have multiple machines and wish to identify them by using the M115 command. // By default we set it to zeros. #define _DEF_CHAR_UUID "00000000-0000-0000-0000-000000000000" @@ -290,7 +325,6 @@ long min_time_before_dir_change = 30; //milliseconds //#define DEBUG #ifdef DEBUG //#define DEBUG_PREPARE_MOVE //Enable this to debug prepare_move() function - //#define DEBUG_RAMP_ACCELERATION //Enable this to debug all constant acceleration info //#define DEBUG_MOVE_TIME //Enable this to time each move and print the result //#define DEBUG_HEAT_MGMT //Enable this to debug heat management. WARNING, this will cause axes to jitter! //#define DEBUG_DISABLE_CHECK_DURING_TRAVEL //Debug the namesake feature, see above in this file diff --git a/Sprinter/Sprinter.h b/Sprinter/Sprinter.h index 844fc34..b70d1fd 100644 --- a/Sprinter/Sprinter.h +++ b/Sprinter/Sprinter.h @@ -1,10 +1,18 @@ // Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. // Licence: GPL -#include <WProgram.h> + +//Check Version of Arduino and then include the right libraries +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include <WProgram.h> +#endif + #include "fastio.h" extern "C" void __cxa_pure_virtual(); +#define FORCE_INLINE __attribute__((always_inline)) inline #if X_ENABLE_PIN > -1 #define enable_x() WRITE(X_ENABLE_PIN, X_ENABLE_ON) @@ -47,10 +55,10 @@ typedef struct { // Fields used by the bresenham algorithm for tracing the line long steps_x, steps_y, steps_z, steps_e; // Step count along each axis - long step_event_count; // The number of step events required to complete this block - volatile long accelerate_until; // The index of the step event on which to stop acceleration - volatile long decelerate_after; // The index of the step event on which to start decelerating - volatile long acceleration_rate; // The acceleration rate used for acceleration calculation + unsigned long step_event_count; // The number of step events required to complete this block + long accelerate_until; // The index of the step event on which to stop acceleration + long decelerate_after; // The index of the step event on which to start decelerating + long acceleration_rate; // The acceleration rate used for acceleration calculation unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) #ifdef ADVANCE @@ -61,16 +69,20 @@ typedef struct { #endif // Fields used by the motion planner to manage acceleration - float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis +// float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis float nominal_speed; // The nominal speed for this block in mm/min + float entry_speed; // Entry speed at previous-current junction in mm/min + float max_entry_speed; // Maximum allowable junction entry speed in mm/min float millimeters; // The total travel of this block in mm - float entry_speed; float acceleration; // acceleration mm/sec^2 + unsigned char recalculate_flag; // Planner flag to recalculate trapezoids on entry junction + unsigned char nominal_length_flag; // Planner flag for nominal speed always reached + // Settings for the trapezoid generator long nominal_rate; // The nominal step rate for this block in step_events/sec - volatile long initial_rate; // The jerk-adjusted step rate at start of block - volatile long final_rate; // The minimal rate at exit + long initial_rate; // The jerk-adjusted step rate at start of block + long final_rate; // The minimal rate at exit long acceleration_st; // acceleration steps/sec^2 volatile char busy; } block_t; @@ -87,7 +99,6 @@ void manage_inactivity(byte debug); void get_coordinates(); void prepare_move(); void prepare_arc_move(char isclockwise); -void plan_buffer_line(float x, float y, float z, float e, float feed_rate); void kill(byte debug); diff --git a/Sprinter/Sprinter.pde b/Sprinter/Sprinter.pde index 8cfe691..d33480f 100644 --- a/Sprinter/Sprinter.pde +++ b/Sprinter/Sprinter.pde @@ -1,6 +1,6 @@ /* Reprap firmware based on Sprinter - Optimize for Sanguinololu 1.2 and above + Optimize for Sanguinololu 1.2 and above, RAMPS This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -27,8 +27,7 @@ Parts of Marlin Firmware from ErikZalm https://github.com/ErikZalm/Marlin-non-gen6 - Sprinter V2 - + Sprinter Changelog - Look forward function --> calculate 16 Steps forward, get from Firmaware Marlin and Grbl - Stepper control with Timer 1 (Interrupt) - Extruder heating with PID use a Softpwm (Timer 2) with 500 hz to free Timer1 für Steppercontrol @@ -40,7 +39,19 @@ - move string to flash to free RAM vor forward planner - M203 Temperature monitor for Repetier - + + - Implement Plannercode from Marlin V1 big thanks to Erik + - Stepper interrupt with Step loops + - Stepperfrequenz 30 Khz + - New Command + * M202 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec + * M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2 + * M205 - advanced settings: minimum travel speed S=while printing T=travel only, X= maximum xy jerk, Z=maximum Z jerk + - Remove unused Variables + - Check Uart Puffer while circle processing (CMD: G2 / G3) + - Fast Xfer Function --> move Text to Flash + - Option to deaktivate ARC (G2/G3) function (save flash) + - Removed modulo (%) operator, which uses an expensive divide */ @@ -52,7 +63,9 @@ #include "pins.h" #include "Sprinter.h" #include "speed_lookuptable.h" -#include "arc_func.h" +#ifdef USE_ARC_FUNCTION + #include "arc_func.h" +#endif #include "heater.h" #ifdef SDSUPPORT @@ -115,9 +128,12 @@ void __cxa_pure_virtual(){}; // M119 - Show Endstopper State // 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) +// M201 - Set maximum acceleration in units/s^2 for print moves (M201 X1000 Y1000) +// M202 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec // M203 - Set temperture monitor to Sx +// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2 +// M205 - advanced settings: minimum travel speed S=while printing T=travel only, X= maximum xy jerk, Z=maximum Z jerk + // M220 - set speed factor override percentage S:factor in percent // Debug feature / Testing the PID for Hotend @@ -126,7 +142,7 @@ void __cxa_pure_virtual(){}; // M603 - Show Free Ram -#define _VERSION_TEXT "1.2.20T / 27.01.2012" +#define _VERSION_TEXT "1.3.04T / 04.02.2012" //Stepper Movement Variables char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'}; @@ -136,52 +152,61 @@ float max_feedrate[] = _MAX_FEEDRATE; float homing_feedrate[] = _HOMING_FEEDRATE; bool axis_relative_modes[] = _AXIS_RELATIVE_MODES; -bool move_direction[NUM_AXIS]; -unsigned long axis_previous_micros[NUM_AXIS]; -unsigned long previous_micros = 0; -unsigned long move_steps_to_take[NUM_AXIS]; - -#ifdef RAMP_ACCELERATION - float acceleration = _ACCELERATION; // Normal acceleration mm/s^2 - float retract_acceleration = _RETRACT_ACCELERATION; // Normal acceleration mm/s^2 - float max_xy_jerk = _MAX_XY_JERK; - float max_z_jerk = _MAX_Z_JERK; - float max_start_speed_units_per_second[] = _MAX_START_SPEED_UNITS_PER_SECOND; - long max_acceleration_units_per_sq_second[] = _MAX_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts - long max_travel_acceleration_units_per_sq_second[] = _MAX_TRAVEL_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z max acceleration in mm/s^2 for travel moves - - unsigned long axis_max_interval[NUM_AXIS]; - unsigned long axis_steps_per_sqr_second[NUM_AXIS]; - unsigned long axis_travel_steps_per_sqr_second[NUM_AXIS]; - unsigned long max_interval; - unsigned long steps_per_sqr_second, plateau_steps; -#endif +float move_acceleration = _ACCELERATION; // Normal acceleration mm/s^2 +float retract_acceleration = _RETRACT_ACCELERATION; // Normal acceleration mm/s^2 +float max_xy_jerk = _MAX_XY_JERK; +float max_z_jerk = _MAX_Z_JERK; + +long max_acceleration_units_per_sq_second[] = _MAX_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts + +//float max_start_speed_units_per_second[] = _MAX_START_SPEED_UNITS_PER_SECOND; +//long max_travel_acceleration_units_per_sq_second[] = _MAX_TRAVEL_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z max acceleration in mm/s^2 for travel moves + +float mintravelfeedrate = DEFAULT_MINTRAVELFEEDRATE; +float minimumfeedrate = DEFAULT_MINIMUMFEEDRATE; + +unsigned long axis_steps_per_sqr_second[NUM_AXIS]; +unsigned long plateau_steps; + +//unsigned long axis_max_interval[NUM_AXIS]; +//unsigned long axis_travel_steps_per_sqr_second[NUM_AXIS]; +//unsigned long max_interval; +//unsigned long steps_per_sqr_second; + //adjustable feed faktor for online tuning printerspeed volatile int feedmultiply=100; //100->original / 200-> Faktor 2 / 50 -> Faktor 0.5 int saved_feedmultiply; volatile bool feedmultiplychanged=false; -boolean acceleration_enabled = false, accelerating = false; -unsigned long interval; +//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}; -unsigned long steps_taken[NUM_AXIS]; -long axis_interval[NUM_AXIS]; // for speed delay + + bool home_all_axis = true; +//unsigned ?? ToDo: Check int 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; + +//unsigned long steps_taken[NUM_AXIS]; +//long axis_interval[NUM_AXIS]; // for speed delay +//float time_for_move; +//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 USE_ARC_FUNCTION //For arc centerpont, send bei Command G2/G3 float offset[3] = {0.0, 0.0, 0.0}; +#endif #ifdef STEP_DELAY_RATIO long long_step_delay_ratio = STEP_DELAY_RATIO * 100; @@ -195,9 +220,9 @@ float offset[3] = {0.0, 0.0, 0.0}; #endif // comm variables and Commandbuffer -// BUFSIZE is reduced from 8 to 5 to free more RAM for the PLANNER +// BUFSIZE is reduced from 8 to 6 to free more RAM for the PLANNER #define MAX_CMD_SIZE 96 -#define BUFSIZE 5 //8 +#define BUFSIZE 6 //8 char cmdbuffer[BUFSIZE][MAX_CMD_SIZE]; bool fromsd[BUFSIZE]; @@ -208,9 +233,9 @@ bool fromsd[BUFSIZE]; long xferbytes; #endif -int bufindr = 0; -int bufindw = 0; -int buflen = 0; +unsigned char bufindr = 0; +unsigned char bufindw = 0; +unsigned char buflen = 0; char serial_char; int serial_count = 0; boolean comment_mode = false; @@ -276,6 +301,11 @@ unsigned char manage_monitor = 255; } #ifdef SD_FAST_XFER_AKTIV + + #ifdef PIDTEMP + extern int g_heater_pwm_val; + #endif + void fast_xfer() { char *pstr; @@ -285,6 +315,8 @@ unsigned char manage_monitor = 255; if(HEATER_0_PIN > -1) WRITE(HEATER_0_PIN,LOW); if(HEATER_1_PIN > -1) WRITE(HEATER_1_PIN,LOW); + g_heater_pwm_val = 0; + lastxferchar = 1; xferbytes = 0; @@ -292,7 +324,7 @@ unsigned char manage_monitor = 255; if(pstr == NULL) { - Serial.println("invalid command"); + showString(PSTR("invalid command\r\n")); return; } @@ -301,24 +333,24 @@ unsigned char manage_monitor = 255; //check mode (currently only RAW is supported if(strcmp(strchr_pointer+4, "RAW") != 0) { - Serial.println("Invalid transfer codec"); + showString(PSTR("Invalid transfer codec\r\n")); return; }else{ - Serial.print("Selected codec: "); + showString(PSTR("Selected codec: ")); Serial.println(strchr_pointer+4); } if (!file.open(&root, pstr+1, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) { - Serial.print("open failed, File: "); + showString(PSTR("open failed, File: ")); Serial.print(pstr+1); - Serial.print("."); + showString(PSTR(".")); }else{ - Serial.print("Writing to file: "); + showString(PSTR("Writing to file: ")); Serial.println(pstr+1); } - Serial.println("ok"); + showString(PSTR("ok\r\n")); //RAW transfer codec //Host sends \0 then up to SD_FAST_XFER_CHUNK_SIZE then \0 @@ -331,15 +363,14 @@ unsigned char manage_monitor = 255; while(!Serial.available()) { } - if(Serial.peek() != 0) + if(Serial.read() != 0) { //host has failed, this isn't a RAW chunk, it's an actual command file.sync(); file.close(); return; } - //clear the initial 0 - Serial.read(); + for(int i=0;i<SD_FAST_XFER_CHUNK_SIZE+1;i++) { while(!Serial.available()) @@ -359,11 +390,11 @@ unsigned char manage_monitor = 255; { fastxferbuffer[SD_FAST_XFER_CHUNK_SIZE] = 0; file.write(fastxferbuffer); - Serial.println("ok"); + showString(PSTR("ok\r\n")); }else{ - Serial.print("Wrote "); + showString(PSTR("Wrote ")); Serial.print(xferbytes); - Serial.println(" bytes."); + showString(PSTR(" bytes.\r\n")); done = true; } } @@ -417,7 +448,7 @@ unsigned char manage_monitor = 255; - inline void write_command(char *buf) + FORCE_INLINE void write_command(char *buf) { char* begin = buf; char* npos = 0; @@ -622,13 +653,16 @@ void setup() SET_OUTPUT(E_STEP_PIN); #endif - #ifdef RAMP_ACCELERATION - 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]); - axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; -// axis_travel_steps_per_sqr_second[i] = max_travel_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; - } - #endif + for(int8_t i=0; i < NUM_AXIS; i++) + { + axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; + } + +// 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]); +// axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; +// axis_travel_steps_per_sqr_second[i] = max_travel_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; +// } #ifdef HEATER_USES_MAX6675 SET_OUTPUT(SCK_PIN); @@ -716,7 +750,10 @@ void loop() #endif buflen = (buflen-1); - bufindr = (bufindr + 1)%BUFSIZE; + //bufindr = (bufindr + 1)%BUFSIZE; + //Removed modulo (%) operator, which uses an expensive divide and multiplication + bufindr++; + if(bufindr == BUFSIZE) bufindr = 0; } //check heater every n milliseconds @@ -812,8 +849,10 @@ void get_command() { case 0: case 1: + #ifdef USE_ARC_FUNCTION case 2: //G2 case 3: //G3 arc func + #endif #ifdef SDSUPPORT if(savetosd) break; @@ -826,7 +865,10 @@ void get_command() break; } } - bufindw = (bufindw + 1)%BUFSIZE; + //Removed modulo (%) operator, which uses an expensive divide and multiplication + //bufindw = (bufindw + 1)%BUFSIZE; + bufindw++; + if(bufindw == BUFSIZE) bufindw = 0; buflen += 1; } comment_mode = false; //for new command @@ -861,7 +903,11 @@ void get_command() { fromsd[bufindw] = true; buflen += 1; - bufindw = (bufindw + 1)%BUFSIZE; + //Removed modulo (%) operator, which uses an expensive divide and multiplication + //bufindw = (bufindw + 1)%BUFSIZE; + bufindw++; + if(bufindw == BUFSIZE) bufindw = 0; + } comment_mode = false; //for new command serial_count = 0; //clear buffer @@ -877,11 +923,11 @@ void get_command() } -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 +FORCE_INLINE float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); } +FORCE_INLINE long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); } +FORCE_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) +FORCE_INLINE bool code_seen(char code) { strchr_pointer = strchr(cmdbuffer[bufindr], code); return (strchr_pointer != NULL); //Return True if a character was found @@ -890,7 +936,7 @@ inline bool code_seen(char code) //------------------------------------------------ // CHECK COMMAND AND CONVERT VALUES //------------------------------------------------ -inline void process_commands() +FORCE_INLINE void process_commands() { unsigned long codenum; //throw away variable char *starpos = NULL; @@ -910,6 +956,7 @@ inline void process_commands() //ClearToSend(); return; //break; + #ifdef USE_ARC_FUNCTION case 2: // G2 - CW ARC get_arc_coordinates(); prepare_arc_move(true); @@ -922,6 +969,7 @@ inline void process_commands() previous_millis_cmd = millis(); //break; return; + #endif case 4: // G4 dwell codenum = 0; if(code_seen('P')) codenum = code_value(); // milliseconds to wait @@ -973,7 +1021,7 @@ inline void process_commands() feedrate = 0; } } - showString(PSTR("HOME X AXIS\r\n")); + //showString(PSTR("HOME X AXIS\r\n")); if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) { @@ -1003,7 +1051,7 @@ inline void process_commands() feedrate = 0; } } - showString(PSTR("HOME Y AXIS\r\n")); + //showString(PSTR("HOME Y AXIS\r\n")); if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) { @@ -1034,7 +1082,7 @@ inline void process_commands() } } - showString(PSTR("HOME Z AXIS\r\n")); + //showString(PSTR("HOME Z AXIS\r\n")); feedrate = saved_feedrate; feedmultiply = saved_feedmultiply; @@ -1266,12 +1314,14 @@ inline void process_commands() #ifdef PIDTEMP showString(PSTR(" @:")); Serial.print(heater_duty); + /* showString(PSTR(",P:")); Serial.print(pTerm); showString(PSTR(",I:")); Serial.print(iTerm); showString(PSTR(",D:")); Serial.print(dTerm); + */ #ifdef AUTOTEMP showString(PSTR(",AU:")); Serial.print(autotemp_setpoint); @@ -1410,16 +1460,14 @@ inline void process_commands() 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 + // 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 +// 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 +// } break; case 115: // M115 showString(PSTR("FIRMWARE_NAME: SprinterV2 PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1\r\n")); @@ -1466,25 +1514,47 @@ inline void process_commands() showString(PSTR("\r\n")); break; - #ifdef RAMP_ACCELERATION - //TODO: update for all axis, use for loop case 201: // M201 - for(int i=0; i < NUM_AXIS; i++) + + for(int8_t 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]; + if(code_seen(axis_codes[i])) + { + max_acceleration_units_per_sq_second[i] = code_value(); + axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i]; + } } - break; + + #if 0 // Not used for Sprinter/grbl gen6 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; + #else + case 202: // M202 max feedrate mm/sec + for(int8_t i=0; i < NUM_AXIS; i++) + { + if(code_seen(axis_codes[i])) max_feedrate[i] = code_value(); + } + break; #endif case 203: // M203 Temperature monitor - if(code_seen('S')) manage_monitor = code_value(); - if(manage_monitor==100) manage_monitor=1; // Set 100 to heated bed - break; + if(code_seen('S')) manage_monitor = code_value(); + if(manage_monitor==100) manage_monitor=1; // Set 100 to heated bed + break; + case 204: // M204 acclereration S normal moves T filmanent only moves + if(code_seen('S')) move_acceleration = code_value() ; + if(code_seen('T')) retract_acceleration = code_value() ; + break; + case 205: //M205 advanced settings: minimum travel speed S=while printing T=travel only, B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk + if(code_seen('S')) minimumfeedrate = code_value(); + if(code_seen('T')) mintravelfeedrate = code_value(); + //if(code_seen('B')) minsegmenttime = code_value() ; + if(code_seen('X')) max_xy_jerk = code_value() ; + if(code_seen('Z')) max_z_jerk = code_value() ; + break; case 220: // M220 S<factor in percent>- set speed factor override percentage { if(code_seen('S')) @@ -1563,7 +1633,7 @@ void ClearToSend() //Serial.println("ok"); } -inline void get_coordinates() +FORCE_INLINE void get_coordinates() { for(int i=0; i < NUM_AXIS; i++) { @@ -1578,12 +1648,14 @@ inline void get_coordinates() } } -inline void get_arc_coordinates() +#ifdef USE_ARC_FUNCTION +FORCE_INLINE void get_arc_coordinates() { get_coordinates(); if(code_seen('I')) offset[0] = code_value(); if(code_seen('J')) offset[1] = code_value(); } +#endif void prepare_move() @@ -1614,7 +1686,7 @@ void prepare_move() } - +#ifdef USE_ARC_FUNCTION void prepare_arc_move(char isclockwise) { @@ -1634,9 +1706,9 @@ void prepare_arc_move(char isclockwise) current_position[i] = destination[i]; } } +#endif - -inline void kill() +FORCE_INLINE void kill() { #if TEMP_0_PIN > -1 target_raw=0; @@ -1657,7 +1729,7 @@ inline void kill() } -inline void manage_inactivity(byte debug) +FORCE_INLINE void manage_inactivity(byte debug) { if( (millis()-previous_millis_cmd) > max_inactive_time ) if(max_inactive_time) kill(); @@ -1712,19 +1784,43 @@ static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for static volatile unsigned char block_buffer_head; // Index of the next block to be pushed static volatile unsigned char block_buffer_tail; // Index of the block to process now +//=========================================================================== +//=============================private variables ============================ +//=========================================================================== + +// Returns the index of the next block in the ring buffer +// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. +static int8_t next_block_index(int8_t block_index) { + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } + return(block_index); +} + + +// Returns the index of the previous block in the ring buffer +static int8_t prev_block_index(int8_t block_index) { + if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } + block_index--; + return(block_index); +} + // The current position of the tool in absolute steps static long position[4]; +static float previous_speed[4]; // Speed of previous path line segment +static float previous_nominal_speed; // Nominal speed of previous path line segment -#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: -inline long estimate_acceleration_distance(long initial_rate, long target_rate, long acceleration) +FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) { - return( - (target_rate*target_rate-initial_rate*initial_rate)/ - (2L*acceleration) - ); + if (acceleration!=0) { + return((target_rate*target_rate-initial_rate*initial_rate)/ + (2.0*acceleration)); + } + else { + return 0.0; // acceleration was 0, set acceleration distance to 0 + } } // This function gives you the point at which you must start braking (at the rate of -acceleration) if @@ -1732,166 +1828,143 @@ inline long estimate_acceleration_distance(long initial_rate, long target_rate, // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) -inline long intersection_distance(long initial_rate, long final_rate, long acceleration, long distance) +FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) { - return( - (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ - (4*acceleration) - ); + if (acceleration!=0) { + return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ + (4.0*acceleration) ); + } + else { + return 0.0; // acceleration was 0, set intersection distance to 0 + } } // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) -{ - if(block->busy == true) return; // If block is busy then bail out. - float entry_factor = entry_speed / block->nominal_speed; - float exit_factor = exit_speed / block->nominal_speed; - long initial_rate = ceil(block->nominal_rate*entry_factor); - long final_rate = ceil(block->nominal_rate*exit_factor); -#ifdef ADVANCE - long initial_advance = block->advance*entry_factor*entry_factor; - long final_advance = block->advance*exit_factor*exit_factor; -#endif // ADVANCE +void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) { + unsigned long initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min) + unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min) // Limit minimal step rate (Otherwise the timer will overflow.) - if(initial_rate <120) initial_rate=120; - if(final_rate < 120) final_rate=120; + if(initial_rate <120) {initial_rate=120; } + if(final_rate < 120) {final_rate=120; } - // Calculate the acceleration steps long acceleration = block->acceleration_st; - long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration); - long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration); - // Calculate the size of Plateau of Nominal Rate. - long plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; - + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will - // have to use intersection_distance() to calculate when to abort acceleration and start braking + // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the final_rate exactly at the end of this block. - if (plateau_steps < 0) { - accelerate_steps = intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count); + if (plateau_steps < 0) { + accelerate_steps = ceil( + intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count)); + accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off + accelerate_steps = min(accelerate_steps,block->step_event_count); plateau_steps = 0; - } - - long decelerate_after = accelerate_steps+plateau_steps; - long acceleration_rate = (long)((float)acceleration * 8.388608); + } + #ifdef ADVANCE + volatile long initial_advance = block->advance*entry_factor*entry_factor; + volatile long final_advance = block->advance*exit_factor*exit_factor; + #endif // ADVANCE + + // block->accelerate_until = accelerate_steps; + // block->decelerate_after = accelerate_steps+plateau_steps; CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section if(block->busy == false) { // Don't update variables if block is busy. block->accelerate_until = accelerate_steps; - block->decelerate_after = decelerate_after; - block->acceleration_rate = acceleration_rate; + block->decelerate_after = accelerate_steps+plateau_steps; block->initial_rate = initial_rate; block->final_rate = final_rate; -#ifdef ADVANCE - block->initial_advance = initial_advance; - block->final_advance = final_advance; -#endif ADVANCE + #ifdef ADVANCE + block->initial_advance = initial_advance; + block->final_advance = final_advance; + #endif //ADVANCE } CRITICAL_SECTION_END; } // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. -inline float max_allowable_speed(float acceleration, float target_velocity, float distance) { - return( - sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) - ); +FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) { + return sqrt(target_velocity*target_velocity-2*acceleration*distance); } // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. // This method will calculate the junction jerk as the euclidean distance between the nominal // velocities of the respective blocks. -inline float junction_jerk(block_t *before, block_t *after) { - return(sqrt( - pow((before->speed_x-after->speed_x), 2)+ - pow((before->speed_y-after->speed_y), 2))); -} +//inline float junction_jerk(block_t *before, block_t *after) { +// return sqrt( +// pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2)); +//} + -// Return the safe speed which is max_jerk/2, e.g. the -// speed under which you cannot exceed max_jerk no matter what you do. -float safe_speed(block_t *block) { - float safe_speed; - safe_speed = max_xy_jerk/2; - if(abs(block->speed_z) > max_z_jerk/2) safe_speed = max_z_jerk/2; - if (safe_speed > block->nominal_speed) safe_speed = block->nominal_speed; - return safe_speed; -} // The kernel called by planner_recalculate() when scanning the plan from last to first entry. void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if(!current) { - return; - } - - float entry_speed = current->nominal_speed; - float exit_factor; - float exit_speed; - if (next) { - exit_speed = next->entry_speed; - } - else { - exit_speed = safe_speed(current); - } - - // Calculate the entry_factor for the current block. - if (previous) { - // Reduce speed so that junction_jerk is within the maximum allowed - float jerk = junction_jerk(previous, current); - if((previous->steps_x == 0) && (previous->steps_y == 0)) { - entry_speed = safe_speed(current); - } - else if (jerk > max_xy_jerk) { - entry_speed = (max_xy_jerk/jerk) * entry_speed; - } - if(abs(previous->speed_z - current->speed_z) > max_z_jerk) { - entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed; - } - // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. - if (entry_speed > exit_speed) { - float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters); - if (max_entry_speed < entry_speed) { - entry_speed = max_entry_speed; + if(!current) { return; } + + if (next) { + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (current->entry_speed != current->max_entry_speed) { + + // If nominal length true, max junction speed is guaranteed to be reached. Only compute + // for max allowable speed if block is decelerating and nominal length is false. + if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { + current->entry_speed = min( current->max_entry_speed, + max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); + } else { + current->entry_speed = current->max_entry_speed; } - } - } - else { - entry_speed = safe_speed(current); - } - // Store result - current->entry_speed = entry_speed; + current->recalculate_flag = true; + + } + } // Skip last block. Already initialized and set for recalculation. } // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the reverse pass. void planner_reverse_pass() { - char block_index = block_buffer_head; - block_t *block[3] = { NULL, NULL, NULL }; - while(block_index != block_buffer_tail) { - block_index--; - if(block_index < 0) block_index = BLOCK_BUFFER_SIZE-1; - block[2]= block[1]; - block[1]= block[0]; - block[0] = &block_buffer[block_index]; - planner_reverse_pass_kernel(block[0], block[1], block[2]); - } - planner_reverse_pass_kernel(NULL, block[0], block[1]); + uint8_t block_index = block_buffer_head; + if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) { + block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1); + block_t *block[3] = { NULL, NULL, NULL }; + while(block_index != block_buffer_tail) { + block_index = prev_block_index(block_index); + block[2]= block[1]; + block[1]= block[0]; + block[0] = &block_buffer[block_index]; + planner_reverse_pass_kernel(block[0], block[1], block[2]); + } + } } + // The kernel called by planner_recalculate() when scanning the plan from first to last entry. void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { - if(!current) { - return; - } - if(previous) { - // If the previous block is an acceleration block, but it is not long enough to - // complete the full speed change within the block, we need to adjust out entry - // speed accordingly. Remember current->entry_factor equals the exit factor of - // the previous block. - if(previous->entry_speed < current->entry_speed) { - float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters); - if (max_entry_speed < current->entry_speed) { - current->entry_speed = max_entry_speed; + if(!previous) { return; } + + // If the previous block is an acceleration block, but it is not long enough to complete the + // full speed change within the block, we need to adjust the entry speed accordingly. Entry + // speeds have already been reset, maximized, and reverse planned by reverse planner. + // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. + if (!previous->nominal_length_flag) { + if (previous->entry_speed < current->entry_speed) { + double entry_speed = min( current->entry_speed, + max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); + + // Check for junction speed change + if (current->entry_speed != entry_speed) { + current->entry_speed = entry_speed; + current->recalculate_flag = true; } } } @@ -1900,16 +1973,15 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the forward pass. void planner_forward_pass() { - char block_index = block_buffer_tail; - block_t *block[3] = { - NULL, NULL, NULL }; + uint8_t block_index = block_buffer_tail; + block_t *block[3] = { NULL, NULL, NULL }; while(block_index != block_buffer_head) { block[0] = block[1]; block[1] = block[2]; block[2] = &block_buffer[block_index]; planner_forward_pass_kernel(block[0],block[1],block[2]); - block_index = (block_index+1) & BLOCK_BUFFER_MASK; + block_index = next_block_index(block_index); } planner_forward_pass_kernel(block[1], block[2], NULL); } @@ -1918,18 +1990,30 @@ void planner_forward_pass() { // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. void planner_recalculate_trapezoids() { - char block_index = block_buffer_tail; + int8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; + while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { - calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed); + // Recalculate if current block entry or exit junction speed has changed. + if (current->recalculate_flag || next->recalculate_flag) { + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, + next->entry_speed/current->nominal_speed); + current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + } } - block_index = (block_index+1) & BLOCK_BUFFER_MASK; + block_index = next_block_index( block_index ); + } + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + if(next != NULL) { + calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, + MINIMUM_PLANNER_SPEED/next->nominal_speed); + next->recalculate_flag = false; } - calculate_trapezoid_for_block(next, next->entry_speed, safe_speed(next)); } // Recalculates the motion plan according to the following algorithm: @@ -1959,16 +2043,22 @@ void plan_init() { block_buffer_head = 0; block_buffer_tail = 0; memset(position, 0, sizeof(position)); // clear position + previous_speed[0] = 0.0; + previous_speed[1] = 0.0; + previous_speed[2] = 0.0; + previous_speed[3] = 0.0; + previous_nominal_speed = 0.0; } -inline void plan_discard_current_block() { + +FORCE_INLINE void plan_discard_current_block() { if (block_buffer_head != block_buffer_tail) { block_buffer_tail = (block_buffer_tail + 1) & BLOCK_BUFFER_MASK; } } -inline block_t *plan_get_current_block() { +FORCE_INLINE block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } @@ -1977,6 +2067,16 @@ inline block_t *plan_get_current_block() { return(block); } +// Gets the current block. Returns NULL if buffer empty +FORCE_INLINE bool blocks_queued() +{ + if (block_buffer_head == block_buffer_tail) { + return false; + } + else + return true; +} + void check_axes_activity() { unsigned char x_active = 0; unsigned char y_active = 0; @@ -1985,14 +2085,14 @@ void check_axes_activity() { block_t *block; if(block_buffer_tail != block_buffer_head) { - char block_index = block_buffer_tail; + uint8_t block_index = block_buffer_tail; while(block_index != block_buffer_head) { block = &block_buffer[block_index]; if(block->steps_x != 0) x_active++; if(block->steps_y != 0) y_active++; if(block->steps_z != 0) z_active++; if(block->steps_e != 0) e_active++; - block_index = (block_index+1) & BLOCK_BUFFER_MASK; + block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } if((DISABLE_X) && (x_active == 0)) disable_x(); @@ -2001,21 +2101,15 @@ void check_axes_activity() { if((DISABLE_E) && (e_active == 0)) disable_e(); } + +float junction_deviation = 0.1; // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration // calculation the caller must also provide the physical length of the line in millimeters. -void plan_buffer_line(float x, float y, float z, float e, float feed_rate) { - - // The target position of the tool in absolute steps - // Calculate target position in absolute steps - long target[4]; - target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); - target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); - target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); - target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); - +void plan_buffer_line(float x, float y, float z, float e, float feed_rate) +{ // Calculate the buffer head after we push this byte - int next_buffer_head = (block_buffer_head + 1) & BLOCK_BUFFER_MASK; + int next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. @@ -2023,12 +2117,16 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) { manage_heater(); manage_inactivity(1); } - - //showString(PSTR("X:")); - //Serial.print(x); - //showString(PSTR(" Y:")); - //Serial.println(y); + // The target position of the tool in absolute steps + // Calculate target position in absolute steps + //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow + long target[4]; + target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); + target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); + target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); + target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); + // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; @@ -2043,11 +2141,16 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) { block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); // Bail if this is a zero-length block - if (block->step_event_count == 0) { - return; - }; - + if (block->step_event_count <=dropsegments) { return; }; + + // Compute direction bits for this block + block->direction_bits = 0; + if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); } + if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); } + if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); } + if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); } + #ifdef DELAY_ENABLE if(block->steps_x != 0) { @@ -2077,114 +2180,217 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) { if(block->steps_e != 0) enable_e(); #endif - float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; - float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; - float delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; - float delta_e_mm = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS]; - block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm) + square(delta_e_mm)); + float delta_mm[4]; + delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; + delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; + delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; + delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS]; + block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + + square(delta_mm[Z_AXIS]) + square(delta_mm[E_AXIS])); + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + + // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. + float inverse_second = feed_rate * inverse_millimeters; + + block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 + block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 + + + + + if (block->steps_e == 0) { + if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; + } + else { + if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; + } - unsigned long microseconds; - microseconds = lround((block->millimeters/feed_rate)*1000000); +#ifdef SLOWDOWN + // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill + int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); - // Calculate speed in mm/minute for each axis - float multiplier = 60.0*1000000.0/microseconds; - block->speed_z = delta_z_mm * multiplier; - block->speed_x = delta_x_mm * multiplier; - block->speed_y = delta_y_mm * multiplier; - block->speed_e = delta_e_mm * multiplier; + if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); +#endif + +/* + // segment time im micro seconds + long segment_time = lround(1000000.0/inverse_second); + if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) { + if (segment_time<minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. + segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/blockcount); + } + } + else { + if (segment_time<minsegmenttime) segment_time=minsegmenttime; + } + // END OF SLOW DOWN SECTION +*/ + + + // Calculate speed in mm/sec for each axis + float current_speed[4]; + for(int i=0; i < 4; i++) { + current_speed[i] = delta_mm[i] * inverse_second; + } // Limit speed per axis - float speed_factor = 1; - float tmp_speed_factor; - if(abs(block->speed_x) > max_feedrate[X_AXIS]) { - speed_factor = max_feedrate[X_AXIS] / abs(block->speed_x); - } - if(abs(block->speed_y) > max_feedrate[Y_AXIS]){ - tmp_speed_factor = max_feedrate[Y_AXIS] / abs(block->speed_y); - if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; - } - if(abs(block->speed_z) > max_feedrate[Z_AXIS]){ - tmp_speed_factor = max_feedrate[Z_AXIS] / abs(block->speed_z); - if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; - } - if(abs(block->speed_e) > max_feedrate[E_AXIS]){ - tmp_speed_factor = max_feedrate[E_AXIS] / abs(block->speed_e); - if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; - } - multiplier = multiplier * speed_factor; - block->speed_z = delta_z_mm * multiplier; - block->speed_x = delta_x_mm * multiplier; - block->speed_y = delta_y_mm * multiplier; - block->speed_e = delta_e_mm * multiplier; - block->nominal_speed = block->millimeters * multiplier; - block->nominal_rate = ceil(block->step_event_count * multiplier / 60); - - if(block->nominal_rate < 120) block->nominal_rate = 120; - block->entry_speed = safe_speed(block); - - // Compute the acceleration rate for the trapezoid generator. - float travel_per_step = block->millimeters/block->step_event_count; + float speed_factor = 1.0; //factor <=1 do decrease speed + for(int i=0; i < 4; i++) { + if(abs(current_speed[i]) > max_feedrate[i]) + speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + } + + // Correct the speed + if( speed_factor < 1.0) { +// Serial.print("speed factor : "); Serial.println(speed_factor); + for(int i=0; i < 4; i++) { + if(abs(current_speed[i]) > max_feedrate[i]) + speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + /* + if(speed_factor < 0.1) { + Serial.print("speed factor : "); Serial.println(speed_factor); + Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]); + } + */ + } + for(unsigned char i=0; i < 4; i++) { + current_speed[i] *= speed_factor; + } + block->nominal_speed *= speed_factor; + block->nominal_rate *= speed_factor; + } + + // Compute and limit the acceleration rate for the trapezoid generator. + float steps_per_mm = block->step_event_count/block->millimeters; if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) { - block->acceleration_st = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2 + block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else { - block->acceleration_st = ceil( (acceleration)/travel_per_step); // convert to: acceleration steps/sec^2 + block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis - if((block->acceleration_st * block->steps_x / block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; - if((block->acceleration_st * block->steps_y / block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; - if((block->acceleration_st * block->steps_e / block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; - if(((block->acceleration_st / block->step_event_count) * block->steps_z ) > axis_steps_per_sqr_second[Z_AXIS]) + if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; } - block->acceleration = block->acceleration_st * travel_per_step; - -#ifdef ADVANCE - // Calculate advance rate - if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { - block->advance_rate = 0; - block->advance = 0; + block->acceleration = block->acceleration_st / steps_per_mm; + block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); + +#if 0 // Use old jerk for now + // Compute path unit vector + double unit_vec[3]; + + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; + + // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Let a circle be tangent to both previous and current path line segments, where the junction + // deviation is defined as the distance from the junction to the closest edge of the circle, + // colinear with the circle center. The circular segment joining the two paths represents the + // path of centripetal acceleration. Solve for max velocity based on max acceleration about the + // radius of the circle, defined indirectly by junction deviation. This may be also viewed as + // path width or max_jerk in the previous grbl version. This approach does not actually deviate + // from path, but used as a robust way to compute cornering speeds, as it takes into account the + // nonlinearities of both the junction angle and junction velocity. + double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed + + // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. + if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { + // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) + // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. + double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; + + // Skip and use default max junction speed for 0 degree acute junction. + if (cos_theta < 0.95) { + vmax_junction = min(previous_nominal_speed,block->nominal_speed); + // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. + if (cos_theta > -0.95) { + // Compute maximum junction velocity based on maximum acceleration and junction deviation + double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + vmax_junction = min(vmax_junction, + sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + } + } } - else { - long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); - float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * - (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536; - block->advance = advance; - if(acc_dist == 0) { - block->advance_rate = 0; +#endif + // Start with a safe speed + float vmax_junction = max_xy_jerk/2; + if(abs(current_speed[Z_AXIS]) > max_z_jerk/2) + vmax_junction = max_z_jerk/2; + vmax_junction = min(vmax_junction, block->nominal_speed); + + if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { + float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { + vmax_junction = block->nominal_speed; + } + if (jerk > max_xy_jerk) { + vmax_junction *= (max_xy_jerk/jerk); } + if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { + vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])); + } + } + block->max_entry_speed = vmax_junction; + + // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. + double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + block->entry_speed = min(vmax_junction, v_allowable); + + // Initialize planner efficiency flags + // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. + // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then + // the current block and next block junction speeds are guaranteed to always be at their maximum + // junction speeds in deceleration and acceleration, respectively. This is due to how the current + // block nominal speed limits both the current and next maximum junction speeds. Hence, in both + // the reverse and forward planners, the corresponding block junction speed will always be at the + // the maximum junction speed and may always be ignored for any speed reduction checks. + if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } + else { block->nominal_length_flag = false; } + block->recalculate_flag = true; // Always calculate trapezoid for new block + + // Update previous path unit_vector and nominal speed + memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + previous_nominal_speed = block->nominal_speed; + + #ifdef ADVANCE + // Calculate advance rate + if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { + block->advance_rate = 0; + block->advance = 0; + } else { - block->advance_rate = advance / (float)acc_dist; + long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); + float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * + (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256; + block->advance = advance; + if(acc_dist == 0) { + block->advance_rate = 0; + } + else { + block->advance_rate = advance / (float)acc_dist; + } } - } -#endif // ADVANCE + #endif // ADVANCE - // compute a preliminary conservative acceleration trapezoid - float safespeed = safe_speed(block); - calculate_trapezoid_for_block(block, safespeed, safespeed); - // Compute direction bits for this block - block->direction_bits = 0; - if (target[X_AXIS] < position[X_AXIS]) { - block->direction_bits |= (1<<X_AXIS); - } - if (target[Y_AXIS] < position[Y_AXIS]) { - block->direction_bits |= (1<<Y_AXIS); - } - if (target[Z_AXIS] < position[Z_AXIS]) { - block->direction_bits |= (1<<Z_AXIS); - } - if (target[E_AXIS] < position[E_AXIS]) { - block->direction_bits |= (1<<E_AXIS); - } - // Move buffer head - block_buffer_head = next_buffer_head; - // Update position + calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, + MINIMUM_PLANNER_SPEED/block->nominal_speed); + + // Move buffer head + block_buffer_head = next_buffer_head; + + // Update position memcpy(position, target, sizeof(target)); // position[] = target[] planner_recalculate(); @@ -2199,7 +2405,13 @@ void plan_set_position(float x, float y, float z, float e) position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); - position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); + position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); + st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]); + previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. + previous_speed[0] = 0.0; + previous_speed[1] = 0.0; + previous_speed[2] = 0.0; + previous_speed[3] = 0.0; } #ifdef AUTOTEMP @@ -2323,6 +2535,12 @@ asm volatile ( \ #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A) #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A) +#ifdef ENDSTOPS_ONLY_FOR_HOMING + #define CHECK_ENDSTOPS if(check_endstops) +#else + #define CHECK_ENDSTOPS +#endif + static block_t *current_block; // A pointer to the block currently being traced // Variables used by The Stepper Driver Interrupt @@ -2337,9 +2555,27 @@ static short old_advance = 0; static short e_steps; static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. static long acceleration_time, deceleration_time; -static long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; +//static long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate; static unsigned short acc_step_rate; // needed for deccelaration start point +static char step_loops; +static unsigned short OCR1A_nominal; + +volatile long endstops_trigsteps[3]={0,0,0}; +static volatile bool endstop_x_hit=false; +static volatile bool endstop_y_hit=false; +static volatile bool endstop_z_hit=false; + +static bool old_x_min_endstop=false; +static bool old_x_max_endstop=false; +static bool old_y_min_endstop=false; +static bool old_y_max_endstop=false; +static bool old_z_min_endstop=false; +static bool old_z_max_endstop=false; +static bool check_endstops = true; + +volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0}; +volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1}; // __________________________ @@ -2361,16 +2597,36 @@ static unsigned short acc_step_rate; // needed for deccelaration start point void st_wake_up() { // TCNT1 = 0; + if(busy == false) ENABLE_STEPPER_DRIVER_INTERRUPT(); } -inline unsigned short calc_timer(unsigned short step_rate) +void enable_endstops(bool check) +{ + check_endstops = check; +} + +FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { unsigned short timer; + if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY; + + if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times + step_rate = (step_rate >> 2)&0x3fff; + step_loops = 4; + } + else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times + step_rate = (step_rate >> 1)&0x7fff; + step_loops = 2; + } + else { + step_loops = 1; + } + if(step_rate < 32) step_rate = 32; step_rate -= 32; // Correct for minimal speed - if(step_rate >= (8*256)) + if(step_rate >= (8*256)) // higher step rate { // higher step rate unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0]; unsigned char tmp_step_rate = (step_rate & 0x00ff); @@ -2385,48 +2641,36 @@ inline unsigned short calc_timer(unsigned short step_rate) timer = (unsigned short)pgm_read_word_near(table_address); timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3); } - - if(timer < 100) timer = 100; - + if(timer < 100) { timer = 100; }//(20kHz this should never happen) return timer; } // Initializes the trapezoid generator from the current block. Called whenever a new // block begins. -inline void trapezoid_generator_reset() +FORCE_INLINE void trapezoid_generator_reset() { - accelerate_until = current_block->accelerate_until; - decelerate_after = current_block->decelerate_after; - acceleration_rate = current_block->acceleration_rate; - initial_rate = current_block->initial_rate; - final_rate = current_block->final_rate; - nominal_rate = current_block->nominal_rate; - #ifdef ADVANCE advance = current_block->initial_advance; final_advance = current_block->final_advance; - advance_rate = current_block->advance_rate; + // Do E steps + advance steps + e_steps += ((advance >>8) - old_advance); + old_advance = advance >>8; #endif deceleration_time = 0; // step_rate to timer interval - acc_step_rate = initial_rate; + acc_step_rate = current_block->initial_rate; acceleration_time = calc_timer(acc_step_rate); OCR1A = acceleration_time; + OCR1A_nominal = calc_timer(current_block->nominal_rate); + } // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. ISR(TIMER1_COMPA_vect) { - if(busy){ /*Serial.println("BUSY")*/; - return; - } // The busy-flag is used to avoid reentering this interrupt - - busy = true; - sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) - // If there is no current block, attempt to pop one from the buffer if (current_block == NULL) { // Anything in the buffer? @@ -2438,10 +2682,12 @@ ISR(TIMER1_COMPA_vect) counter_z = counter_x; counter_e = counter_x; step_events_completed = 0; - e_steps = 0; +// #ifdef ADVANCE +// e_steps = 0; +// #endif } else { - DISABLE_STEPPER_DRIVER_INTERRUPT(); + OCR1A=2000; // 1kHz. } } @@ -2449,143 +2695,236 @@ ISR(TIMER1_COMPA_vect) // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt out_bits = current_block->direction_bits; -#ifdef ADVANCE - // Calculate E early. - counter_e += current_block->steps_e; - if (counter_e > 0) { - counter_e -= current_block->step_event_count; - if ((out_bits & (1<<E_AXIS)) != 0) { // - direction - CRITICAL_SECTION_START; - e_steps--; - CRITICAL_SECTION_END; - } - else { - CRITICAL_SECTION_START; - e_steps++; - CRITICAL_SECTION_END; - } - } - // Do E steps + advance steps - CRITICAL_SECTION_START; - e_steps += ((advance >> 16) - old_advance); - CRITICAL_SECTION_END; - old_advance = advance >> 16; -#endif //ADVANCE - // Set direction en check limit switches if ((out_bits & (1<<X_AXIS)) != 0) { // -direction WRITE(X_DIR_PIN, INVERT_X_DIR); - if(READ(X_MIN_PIN) != X_ENDSTOP_INVERT) { - step_events_completed = current_block->step_event_count; + count_direction[X_AXIS]=-1; + CHECK_ENDSTOPS + { + #if X_MIN_PIN > -1 + bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOP_INVERT); + if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) { + endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; + endstop_x_hit=true; + step_events_completed = current_block->step_event_count; + } + old_x_min_endstop = x_min_endstop; + #endif + } + } + else { // +direction + WRITE(X_DIR_PIN,!INVERT_X_DIR); + count_direction[X_AXIS]=1; + CHECK_ENDSTOPS + { + #if X_MAX_PIN > -1 + bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOP_INVERT); + if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){ + endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; + endstop_x_hit=true; + step_events_completed = current_block->step_event_count; + } + old_x_max_endstop = x_max_endstop; + #endif } } - else // +direction - WRITE(X_DIR_PIN,!INVERT_X_DIR); if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction WRITE(Y_DIR_PIN,INVERT_Y_DIR); - if(READ(Y_MIN_PIN) != Y_ENDSTOP_INVERT) { - step_events_completed = current_block->step_event_count; + count_direction[Y_AXIS]=-1; + CHECK_ENDSTOPS + { + #if Y_MIN_PIN > -1 + bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOP_INVERT); + if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) { + endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; + endstop_y_hit=true; + step_events_completed = current_block->step_event_count; + } + old_y_min_endstop = y_min_endstop; + #endif } } - else // +direction + else { // +direction WRITE(Y_DIR_PIN,!INVERT_Y_DIR); + count_direction[Y_AXIS]=1; + CHECK_ENDSTOPS + { + #if Y_MAX_PIN > -1 + bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOP_INVERT); + if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){ + endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; + endstop_y_hit=true; + step_events_completed = current_block->step_event_count; + } + old_y_max_endstop = y_max_endstop; + #endif + } + } if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction WRITE(Z_DIR_PIN,INVERT_Z_DIR); - if(READ(Z_MIN_PIN) != Z_ENDSTOP_INVERT) { - step_events_completed = current_block->step_event_count; + count_direction[Z_AXIS]=-1; + CHECK_ENDSTOPS + { + #if Z_MIN_PIN > -1 + bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOP_INVERT); + if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) { + endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; + endstop_z_hit=true; + step_events_completed = current_block->step_event_count; + } + old_z_min_endstop = z_min_endstop; + #endif } } - else // +direction - WRITE(Z_DIR_PIN,!INVERT_Z_DIR); - -#ifndef ADVANCE - if ((out_bits & (1<<E_AXIS)) != 0) // -direction - WRITE(E_DIR_PIN,INVERT_E_DIR); - else // +direction - WRITE(E_DIR_PIN,!INVERT_E_DIR); -#endif //!ADVANCE - - counter_x += current_block->steps_x; - if (counter_x > 0) { - WRITE(X_STEP_PIN, HIGH); - counter_x -= current_block->step_event_count; - WRITE(X_STEP_PIN, LOW); + else { // +direction + WRITE(Z_DIR_PIN,!INVERT_Z_DIR); + count_direction[Z_AXIS]=1; + CHECK_ENDSTOPS + { + #if Z_MAX_PIN > -1 + bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOP_INVERT); + if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) { + endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; + endstop_z_hit=true; + step_events_completed = current_block->step_event_count; + } + old_z_max_endstop = z_max_endstop; + #endif + } } - counter_y += current_block->steps_y; - if (counter_y > 0) { - WRITE(Y_STEP_PIN, HIGH); - counter_y -= current_block->step_event_count; - WRITE(Y_STEP_PIN, LOW); - } + #ifndef ADVANCE + if ((out_bits & (1<<E_AXIS)) != 0) { // -direction + WRITE(E_DIR_PIN,INVERT_E_DIR); + count_direction[E_AXIS]=-1; + } + else { // +direction + WRITE(E_DIR_PIN,!INVERT_E_DIR); + count_direction[E_AXIS]=-1; + } + #endif //!ADVANCE + - counter_z += current_block->steps_z; - if (counter_z > 0) { - WRITE(Z_STEP_PIN, HIGH); - counter_z -= current_block->step_event_count; - WRITE(Z_STEP_PIN, LOW); - } + + for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves) + //MSerial.checkRx(); // Check for serial chars. + + #ifdef ADVANCE + counter_e += current_block->steps_e; + if (counter_e > 0) { + counter_e -= current_block->step_event_count; + if ((out_bits & (1<<E_AXIS)) != 0) { // - direction + e_steps--; + } + else { + e_steps++; + } + } + #endif //ADVANCE + + counter_x += current_block->steps_x; + if (counter_x > 0) { + WRITE(X_STEP_PIN, HIGH); + counter_x -= current_block->step_event_count; + WRITE(X_STEP_PIN, LOW); + count_position[X_AXIS]+=count_direction[X_AXIS]; + } -#ifndef ADVANCE - counter_e += current_block->steps_e; - if (counter_e > 0) { - WRITE(E_STEP_PIN, HIGH); - counter_e -= current_block->step_event_count; - WRITE(E_STEP_PIN, LOW); - } -#endif //!ADVANCE + counter_y += current_block->steps_y; + if (counter_y > 0) { + WRITE(Y_STEP_PIN, HIGH); + counter_y -= current_block->step_event_count; + WRITE(Y_STEP_PIN, LOW); + count_position[Y_AXIS]+=count_direction[Y_AXIS]; + } + + counter_z += current_block->steps_z; + if (counter_z > 0) { + WRITE(Z_STEP_PIN, HIGH); + counter_z -= current_block->step_event_count; + WRITE(Z_STEP_PIN, LOW); + count_position[Z_AXIS]+=count_direction[Z_AXIS]; + } + #ifndef ADVANCE + counter_e += current_block->steps_e; + if (counter_e > 0) { + WRITE(E_STEP_PIN, HIGH); + counter_e -= current_block->step_event_count; + WRITE(E_STEP_PIN, LOW); + count_position[E_AXIS]+=count_direction[E_AXIS]; + } + #endif //!ADVANCE + step_events_completed += 1; + if(step_events_completed >= current_block->step_event_count) break; + } // Calculare new timer value unsigned short timer; unsigned short step_rate; - if (step_events_completed < accelerate_until) { - MultiU24X24toH16(acc_step_rate, acceleration_time, acceleration_rate); - acc_step_rate += initial_rate; + if (step_events_completed <= (unsigned long int)current_block->accelerate_until) { + + MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate); + acc_step_rate += current_block->initial_rate; // upper limit - if(acc_step_rate > nominal_rate) - acc_step_rate = nominal_rate; + if(acc_step_rate > current_block->nominal_rate) + acc_step_rate = current_block->nominal_rate; // step_rate to timer interval timer = calc_timer(acc_step_rate); - advance += advance_rate; - acceleration_time += timer; OCR1A = timer; + acceleration_time += timer; + #ifdef ADVANCE + for(int8_t i=0; i < step_loops; i++) { + advance += advance_rate; + } + //if(advance > current_block->advance) advance = current_block->advance; + // Do E steps + advance steps + e_steps += ((advance >>8) - old_advance); + old_advance = advance >>8; + + #endif } - else if (step_events_completed >= decelerate_after) { - MultiU24X24toH16(step_rate, deceleration_time, acceleration_rate); + else if (step_events_completed > (unsigned long int)current_block->decelerate_after) { + MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate); if(step_rate > acc_step_rate) { // Check step_rate stays positive - step_rate = final_rate; + step_rate = current_block->final_rate; } else { step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point. } // lower limit - if(step_rate < final_rate) - step_rate = final_rate; + if(step_rate < current_block->final_rate) + step_rate = current_block->final_rate; // step_rate to timer interval timer = calc_timer(step_rate); -#ifdef ADVANCE - advance -= advance_rate; - if(advance < final_advance) - advance = final_advance; -#endif //ADVANCE - deceleration_time += timer; OCR1A = timer; - } + deceleration_time += timer; + #ifdef ADVANCE + for(int8_t i=0; i < step_loops; i++) { + advance -= advance_rate; + } + if(advance < final_advance) advance = final_advance; + // Do E steps + advance steps + e_steps += ((advance >>8) - old_advance); + old_advance = advance >>8; + #endif //ADVANCE + } + else { + OCR1A = OCR1A_nominal; + } + // If current block is finished, reset pointer - step_events_completed += 1; if (step_events_completed >= current_block->step_event_count) { current_block = NULL; plan_discard_current_block(); } } - busy=false; } #ifdef ADVANCE @@ -2595,23 +2934,27 @@ unsigned char old_OCR0A; // Timer 0 is shared with millies ISR(TIMER0_COMPA_vect) { - // Critical section needed because Timer 1 interrupt has higher priority. - // The pin set functions are placed on trategic position to comply with the stepper driver timing. - WRITE(E_STEP_PIN, LOW); + old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz) + OCR0A = old_OCR0A; // Set E direction (Depends on E direction + advance) - if (e_steps < 0) { - WRITE(E_DIR_PIN,INVERT_E_DIR); - e_steps++; - WRITE(E_STEP_PIN, HIGH); - } - if (e_steps > 0) { - WRITE(E_DIR_PIN,!INVERT_E_DIR); - e_steps--; - WRITE(E_STEP_PIN, HIGH); + for(unsigned char i=0; i<4;i++) + { + if (e_steps != 0) + { + WRITE(E0_STEP_PIN, LOW); + if (e_steps < 0) { + WRITE(E0_DIR_PIN, INVERT_E0_DIR); + e_steps++; + WRITE(E0_STEP_PIN, HIGH); + } + else if (e_steps > 0) { + WRITE(E0_DIR_PIN, !INVERT_E0_DIR); + e_steps--; + WRITE(E0_STEP_PIN, HIGH); + } + } + } } - old_OCR0A += 25; // 10kHz interrupt - OCR0A = old_OCR0A; -} #endif // ADVANCE void st_init() @@ -2628,24 +2971,46 @@ void st_init() TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10); // 2MHz timer OCR1A = 0x4000; - DISABLE_STEPPER_DRIVER_INTERRUPT(); + TCNT1 = 0; + ENABLE_STEPPER_DRIVER_INTERRUPT(); #ifdef ADVANCE + #if defined(TCCR0A) && defined(WGM01) + TCCR0A &= ~(1<<WGM01); + TCCR0A &= ~(1<<WGM00); + #endif e_steps = 0; TIMSK0 |= (1<<OCIE0A); #endif //ADVANCE + + #ifdef ENDSTOPS_ONLY_FOR_HOMING + enable_endstops(false); + #else + enable_endstops(true); + #endif + sei(); } // Block until all buffered steps are executed void st_synchronize() { - while(plan_get_current_block()) { + while(blocks_queued()) { manage_heater(); manage_inactivity(1); } } +void st_set_position(const long &x, const long &y, const long &z, const long &e) +{ + CRITICAL_SECTION_START; + count_position[X_AXIS] = x; + count_position[Y_AXIS] = y; + count_position[Z_AXIS] = z; + count_position[E_AXIS] = e; + CRITICAL_SECTION_END; +} + #ifdef DEBUG void log_message(char* message) { |