diff options
| -rw-r--r-- | Firmware/Configuration.h | 28 | ||||
| -rw-r--r-- | Firmware/ConfigurationStore.cpp | 54 | ||||
| -rw-r--r-- | Firmware/Configuration_adv.h | 10 | ||||
| -rw-r--r-- | Firmware/Marlin.h | 33 | ||||
| -rw-r--r-- | Firmware/MarlinSerial.cpp | 12 | ||||
| -rw-r--r-- | Firmware/MarlinSerial.h | 31 | ||||
| -rw-r--r-- | Firmware/Marlin_main.cpp | 1432 | ||||
| -rw-r--r-- | Firmware/cardreader.cpp | 3 | ||||
| -rw-r--r-- | Firmware/mesh_bed_calibration.cpp | 720 | ||||
| -rw-r--r-- | Firmware/mesh_bed_calibration.h | 23 | ||||
| -rw-r--r-- | Firmware/mesh_bed_leveling.cpp | 53 | ||||
| -rw-r--r-- | Firmware/pins.h | 12 | ||||
| -rw-r--r-- | Firmware/planner.cpp | 101 | ||||
| -rw-r--r-- | Firmware/planner.h | 16 | ||||
| -rw-r--r-- | Firmware/stepper.cpp | 36 | ||||
| -rw-r--r-- | Firmware/ultralcd.cpp | 28 |
16 files changed, 1204 insertions, 1388 deletions
diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h index 76b2cbd44..e8c5d22c5 100644 --- a/Firmware/Configuration.h +++ b/Firmware/Configuration.h @@ -31,20 +31,6 @@ // Advanced settings can be found in Configuration_adv.h // BASIC SETTINGS: select your board type, temperature sensor type, axis scaling, and endstop configuration -//=========================================================================== -//============================= DELTA Printer =============================== -//=========================================================================== -// For a Delta printer replace the configuration files with the files in the -// example_configurations/delta directory. -// - -//=========================================================================== -//============================= SCARA Printer =============================== -//=========================================================================== -// For a Delta printer replace the configuration files with the files in the -// example_configurations/SCARA directory. -// - // User-specified version info of this build to display in [Pronterface, etc] terminal window during // startup. Implementation of an idea by Prof Braino to inform user that any changes made to this // build by the user have been successfully uploaded into firmware. @@ -396,9 +382,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of //Manual homing switch locations: // For deltabots this means top and center of the Cartesian print volume. -//#define MANUAL_Z_HOME_POS 402 // For delta: Distance between nozzle and print surface after homing. - - // Offset of the extruders (uncomment if using more than one and relying on firmware to position when changing). // The offset has to be X=0, Y=0 for the extruder 0 hotend (default extruder). @@ -634,9 +617,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of // SF send wrong arc g-codes when using Arc Point as fillet procedure //#define SF_ARC_FIX -// Support for the BariCUDA Paste Extruder. -//#define BARICUDA - //define BlinkM/CyzRgb Support //#define BLINKM @@ -654,14 +634,6 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of // //#define NUM_SERVOS 3 // Servo index starts with 0 for M280 command -// Servo Endstops -// -// This allows for servo actuated endstops, primary usage is for the Z Axis to eliminate calibration or bed height changes. -// Use M206 command to correct for switch height offset to actual nozzle height. Store that setting with M500. -// -//#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1 -//#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 70,0} // X,Y,Z Axis Extend and Retract angles - /**********************************************************************\ * Support for a filament diameter sensor * Also allows adjustment of diameter at print time (vs at slicing) diff --git a/Firmware/ConfigurationStore.cpp b/Firmware/ConfigurationStore.cpp index a301b8c86..097d6c0da 100644 --- a/Firmware/ConfigurationStore.cpp +++ b/Firmware/ConfigurationStore.cpp @@ -63,12 +63,6 @@ void Config_StoreSettings() EEPROM_WRITE_VAR(i,max_z_jerk); EEPROM_WRITE_VAR(i,max_e_jerk); EEPROM_WRITE_VAR(i,add_homing); - #ifdef DELTA - EEPROM_WRITE_VAR(i,endstop_adj); - EEPROM_WRITE_VAR(i,delta_radius); - EEPROM_WRITE_VAR(i,delta_diagonal_rod); - EEPROM_WRITE_VAR(i,delta_segments_per_second); - #endif #ifndef ULTIPANEL int plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP, plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP, plaPreheatFanSpeed = PLA_PREHEAT_FAN_SPEED; int absPreheatHotendTemp = ABS_PREHEAT_HOTEND_TEMP, absPreheatHPBTemp = ABS_PREHEAT_HPB_TEMP, absPreheatFanSpeed = ABS_PREHEAT_FAN_SPEED; @@ -99,9 +93,6 @@ void Config_StoreSettings() int lcd_contrast = 32; #endif EEPROM_WRITE_VAR(i,lcd_contrast); - #ifdef SCARA - EEPROM_WRITE_VAR(i,axis_scaling); // Add scaling for SCARA - #endif #ifdef FWRETRACT EEPROM_WRITE_VAR(i,autoretract_enabled); EEPROM_WRITE_VAR(i,retract_length); @@ -149,16 +140,6 @@ void Config_PrintSettings() SERIAL_ECHOLN(""); SERIAL_ECHO_START; -#ifdef SCARA -SERIAL_ECHOLNPGM("Scaling factors:"); - SERIAL_ECHO_START; - SERIAL_ECHOPAIR(" M365 X",axis_scaling[X_AXIS]); - SERIAL_ECHOPAIR(" Y",axis_scaling[Y_AXIS]); - SERIAL_ECHOPAIR(" Z",axis_scaling[Z_AXIS]); - SERIAL_ECHOLN(""); - - SERIAL_ECHO_START; -#endif SERIAL_ECHOLNPGM("Maximum feedrates (mm/s):"); SERIAL_ECHO_START; SERIAL_ECHOPAIR(" M203 X", max_feedrate[X_AXIS]); @@ -200,22 +181,6 @@ SERIAL_ECHOLNPGM("Scaling factors:"); SERIAL_ECHOPAIR(" Y" ,add_homing[Y_AXIS] ); SERIAL_ECHOPAIR(" Z" ,add_homing[Z_AXIS] ); SERIAL_ECHOLN(""); -#ifdef DELTA - SERIAL_ECHO_START; - SERIAL_ECHOLNPGM("Endstop adjustement (mm):"); - SERIAL_ECHO_START; - SERIAL_ECHOPAIR(" M666 X",endstop_adj[X_AXIS] ); - SERIAL_ECHOPAIR(" Y" ,endstop_adj[Y_AXIS] ); - SERIAL_ECHOPAIR(" Z" ,endstop_adj[Z_AXIS] ); - SERIAL_ECHOLN(""); - SERIAL_ECHO_START; - SERIAL_ECHOLNPGM("Delta settings: L=delta_diagonal_rod, R=delta_radius, S=delta_segments_per_second"); - SERIAL_ECHO_START; - SERIAL_ECHOPAIR(" M665 L",delta_diagonal_rod ); - SERIAL_ECHOPAIR(" R" ,delta_radius ); - SERIAL_ECHOPAIR(" S" ,delta_segments_per_second ); - SERIAL_ECHOLN(""); -#endif #ifdef PIDTEMP SERIAL_ECHO_START; SERIAL_ECHOLNPGM("PID settings:"); @@ -305,12 +270,6 @@ void Config_RetrieveSettings() EEPROM_READ_VAR(i,max_z_jerk); EEPROM_READ_VAR(i,max_e_jerk); EEPROM_READ_VAR(i,add_homing); - #ifdef DELTA - EEPROM_READ_VAR(i,endstop_adj); - EEPROM_READ_VAR(i,delta_radius); - EEPROM_READ_VAR(i,delta_diagonal_rod); - EEPROM_READ_VAR(i,delta_segments_per_second); - #endif #ifndef ULTIPANEL int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed; int absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed; @@ -337,9 +296,6 @@ void Config_RetrieveSettings() int lcd_contrast; #endif EEPROM_READ_VAR(i,lcd_contrast); - #ifdef SCARA - EEPROM_READ_VAR(i,axis_scaling); - #endif #ifdef FWRETRACT EEPROM_READ_VAR(i,autoretract_enabled); @@ -390,9 +346,6 @@ void Config_ResetDefault() axis_steps_per_unit[i]=tmp1[i]; max_feedrate[i]=tmp2[i]; max_acceleration_units_per_sq_second[i]=tmp3[i]; - #ifdef SCARA - axis_scaling[i]=1; - #endif } // steps per sq second need to be updated to agree with the units per sq second @@ -407,13 +360,6 @@ void Config_ResetDefault() max_z_jerk=DEFAULT_ZJERK; max_e_jerk=DEFAULT_EJERK; add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0; -#ifdef DELTA - endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0; - delta_radius= DELTA_RADIUS; - delta_diagonal_rod= DELTA_DIAGONAL_ROD; - delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND; - recalc_delta_settings(delta_radius, delta_diagonal_rod); -#endif #ifdef ULTIPANEL plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP; plaPreheatHPBTemp = PLA_PREHEAT_HPB_TEMP; diff --git a/Firmware/Configuration_adv.h b/Firmware/Configuration_adv.h index f5cdadd1b..1ce9032ad 100644 --- a/Firmware/Configuration_adv.h +++ b/Firmware/Configuration_adv.h @@ -324,12 +324,6 @@ #ifdef COREXY #error BABYSTEPPING not implemented for COREXY yet. #endif - - #ifdef DELTA - #ifdef BABYSTEP_XY - #error BABYSTEPPING only implemented for Z axis on deltabots. - #endif - #endif #endif // extruder advance constant (s2/mm3) @@ -435,10 +429,6 @@ const unsigned int dropsegments=5; //everything with less than this number of st //============================= Define Defines ============================ //=========================================================================== -#if defined (ENABLE_AUTO_BED_LEVELING) && defined (DELTA) - #error "Bed Auto Leveling is still not compatible with Delta Kinematics." -#endif - #if EXTRUDERS > 1 && defined TEMP_SENSOR_1_AS_REDUNDANT #error "You cannot use TEMP_SENSOR_1_AS_REDUNDANT if EXTRUDERS > 1" #endif diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h index 69b9a40c7..3cb858f15 100644 --- a/Firmware/Marlin.h +++ b/Firmware/Marlin.h @@ -211,22 +211,21 @@ void FlushSerialRequestResend(); void ClearToSend();
void get_coordinates();
-#ifdef DELTA
-void calculate_delta(float cartesian[3]);
-extern float delta[3];
-#endif
-#ifdef SCARA
-void calculate_delta(float cartesian[3]);
-void calculate_SCARA_forward_Transform(float f_scara[3]);
-#endif
void prepare_move();
void kill();
void Stop();
bool IsStopped();
-void enquecommand(const char *cmd); //put an ASCII command at the end of the current buffer.
-void enquecommand_P(const char *cmd); //put an ASCII command at the end of the current buffer, read from flash
+//put an ASCII command at the end of the current buffer.
+void enquecommand(const char *cmd, bool from_progmem = false);
+//put an ASCII command at the end of the current buffer, read from flash
+#define enquecommand_P(cmd) enquecommand(cmd, true)
+void enquecommand_front(const char *cmd, bool from_progmem = false);
+//put an ASCII command at the end of the current buffer, read from flash
+#define enquecommand_P(cmd) enquecommand(cmd, true)
+#define enquecommand_front_P(cmd) enquecommand_front(cmd, true)
+
void prepare_arc_move(char isclockwise);
void clamp_to_software_endstops(float target[3]);
@@ -252,25 +251,11 @@ extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional extern float current_position[NUM_AXIS] ;
extern float destination[NUM_AXIS] ;
extern float add_homing[3];
-#ifdef DELTA
-extern float endstop_adj[3];
-extern float delta_radius;
-extern float delta_diagonal_rod;
-extern float delta_segments_per_second;
-void recalc_delta_settings(float radius, float diagonal_rod);
-#endif
-#ifdef SCARA
-extern float axis_scaling[3]; // Build size scaling
-#endif
extern float min_pos[3];
extern float max_pos[3];
extern bool axis_known_position[3];
extern float zprobe_zoffset;
extern int fanSpeed;
-#ifdef BARICUDA
-extern int ValvePressure;
-extern int EtoPPressure;
-#endif
#ifdef FAN_SOFT_PWM
extern unsigned char fanSpeedSoftPwm;
diff --git a/Firmware/MarlinSerial.cpp b/Firmware/MarlinSerial.cpp index e4e737433..fba87cc86 100644 --- a/Firmware/MarlinSerial.cpp +++ b/Firmware/MarlinSerial.cpp @@ -53,8 +53,16 @@ FORCE_INLINE void store_char(unsigned char c) //SIGNAL(SIG_USART_RECV) SIGNAL(M_USARTx_RX_vect) { - unsigned char c = M_UDRx; - store_char(c); + // Test for a framing error. + if (M_UCSRxA & (1<<M_FEx)) { + // Characters received with the framing errors will be ignored. + // The temporary variable "c" was made volatile, so the compiler does not optimize this out. + volatile unsigned char c = M_UDRx; + } else { + // Read the input register. + unsigned char c = M_UDRx; + store_char(c); + } } #endif diff --git a/Firmware/MarlinSerial.h b/Firmware/MarlinSerial.h index 7ccdfd6a0..6776b5689 100644 --- a/Firmware/MarlinSerial.h +++ b/Firmware/MarlinSerial.h @@ -53,6 +53,7 @@ #define M_UBRRxH SERIAL_REGNAME(UBRR,SERIAL_PORT,H) #define M_UBRRxL SERIAL_REGNAME(UBRR,SERIAL_PORT,L) #define M_RXCx SERIAL_REGNAME(RXC,SERIAL_PORT,) +#define M_FEx SERIAL_REGNAME(FE,SERIAL_PORT,) #define M_USARTx_RX_vect SERIAL_REGNAME(USART,SERIAL_PORT,_RX_vect) #define M_U2Xx SERIAL_REGNAME(U2X,SERIAL_PORT,) @@ -111,19 +112,25 @@ class MarlinSerial //: public Stream FORCE_INLINE void checkRx(void) { - if((M_UCSRxA & (1<<M_RXCx)) != 0) { - unsigned char c = M_UDRx; - int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE; - - // if we should be storing the received character into the location - // just before the tail (meaning that the head would advance to the - // current location of the tail), we're about to overflow the buffer - // and so we don't write the character or advance the head. - if (i != rx_buffer.tail) { - rx_buffer.buffer[rx_buffer.head] = c; - rx_buffer.head = i; + if((M_UCSRxA & (1<<M_RXCx)) != 0) { + // Test for a framing error. + if (M_UCSRxA & (1<<M_FEx)) { + // Characters received with the framing errors will be ignored. + // The temporary variable "c" was made volatile, so the compiler does not optimize this out. + volatile unsigned char c = M_UDRx; + } else { + unsigned char c = M_UDRx; + int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE; + // if we should be storing the received character into the location + // just before the tail (meaning that the head would advance to the + // current location of the tail), we're about to overflow the buffer + // and so we don't write the character or advance the head. + if (i != rx_buffer.tail) { + rx_buffer.buffer[rx_buffer.head] = c; + rx_buffer.head = i; + } + } } - } } diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index be0f792ea..d5a9a40cb 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -204,23 +204,12 @@ // M509 - force language selection on next restart // M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) // M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal] -// M665 - set delta configurations -// M666 - set delta endstop adjustment // M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ] // M907 - Set digital trimpot motor current using axis codes. // M908 - Control digital trimpot directly. // M350 - Set microstepping mode. // M351 - Toggle MS1 MS2 pins directly. -// ************ SCARA Specific - This can change to suit future G-code regulations -// M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration) -// M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree) -// M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration) -// M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree) -// M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position) -// M365 - SCARA calibration: Scaling factor, X, Y, Z axis -//************* SCARA End *************** - // M928 - Start SD logging (M928 filename.g) - ended by M29 // M999 - Restart after being stopped by error @@ -298,9 +287,6 @@ float volumetric_multiplier[EXTRUDERS] = {1.0 }; float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 }; float add_homing[3]={0,0,0}; -#ifdef DELTA -float endstop_adj[3]={0,0,0}; -#endif float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS }; float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS }; @@ -322,14 +308,6 @@ float extruder_offset[NUM_EXTRUDER_OFFSETS][EXTRUDERS] = { #endif uint8_t active_extruder = 0; int fanSpeed=0; -#ifdef SERVO_ENDSTOPS - int servo_endstops[] = SERVO_ENDSTOPS; - int servo_endstop_angles[] = SERVO_ENDSTOP_ANGLES; -#endif -#ifdef BARICUDA -int ValvePressure=0; -int EtoPPressure=0; -#endif #ifdef FWRETRACT bool autoretract_enabled=false; @@ -367,27 +345,6 @@ int EtoPPressure=0; #endif #endif -#ifdef DELTA - float delta[3] = {0.0, 0.0, 0.0}; - #define SIN_60 0.8660254037844386 - #define COS_60 0.5 - // these are the default values, can be overriden with M665 - float delta_radius= DELTA_RADIUS; - float delta_tower1_x= -SIN_60*delta_radius; // front left tower - float delta_tower1_y= -COS_60*delta_radius; - float delta_tower2_x= SIN_60*delta_radius; // front right tower - float delta_tower2_y= -COS_60*delta_radius; - float delta_tower3_x= 0.0; // back middle tower - float delta_tower3_y= delta_radius; - float delta_diagonal_rod= DELTA_DIAGONAL_ROD; - float delta_diagonal_rod_2= sq(delta_diagonal_rod); - float delta_segments_per_second= DELTA_SEGMENTS_PER_SECOND; -#endif - -#ifdef SCARA // Build size scaling -float axis_scaling[3]={1,1,1}; // Build size scaling, default to 1 -#endif - bool cancel_heatup = false ; #ifdef FILAMENT_SENSOR @@ -411,9 +368,7 @@ const char echomagic[] PROGMEM = "echo:"; const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'}; float destination[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0}; -#ifndef DELTA static float delta[3] = {0.0, 0.0, 0.0}; -#endif // For tracing an arc static float offset[3] = {0.0, 0.0, 0.0}; @@ -425,14 +380,44 @@ static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0; // Also there is bool axis_relative_modes[] per axis flag. static bool relative_mode = false; -static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE]; -// Marking a line in the cmdbuffer. If false, the command is confirmed by sending an "OK" on the serial line. -static bool fromsd[BUFSIZE]; +// String circular buffer. Commands may be pushed to the buffer from both sides: +// Chained commands will be pushed to the front, interactive (from LCD menu) +// and printing commands (from serial line or from SD card) are pushed to the tail. +// First character of each entry indicates the type of the entry: +#define CMDBUFFER_CURRENT_TYPE_UNKNOWN 0 +// Command in cmdbuffer was sent over USB. +#define CMDBUFFER_CURRENT_TYPE_USB 1 +// Command in cmdbuffer was read from SDCARD. +#define CMDBUFFER_CURRENT_TYPE_SDCARD 2 +// Command in cmdbuffer was generated by the UI. +#define CMDBUFFER_CURRENT_TYPE_UI 3 +// Command in cmdbuffer was generated by another G-code. +#define CMDBUFFER_CURRENT_TYPE_CHAINED 4 + +// How much space to reserve for the chained commands +// of type CMDBUFFER_CURRENT_TYPE_CHAINED, +// which are pushed to the front of the queue? +// Maximum 5 commands of max length 20 + null terminator. +#define CMDBUFFER_RESERVE_FRONT (5*21) +// Reserve BUFSIZE lines of length MAX_CMD_SIZE plus CMDBUFFER_RESERVE_FRONT. +static char cmdbuffer[BUFSIZE * (MAX_CMD_SIZE + 1) + CMDBUFFER_RESERVE_FRONT]; +// Head of the circular buffer, where to read. static int bufindr = 0; +// Tail of the buffer, where to write. static int bufindw = 0; +// Number of lines in cmdbuffer. static int buflen = 0; -//static int i = 0; -static char serial_char; +// Flag for processing the current command inside the main Arduino loop(). +// If a new command was pushed to the front of a command buffer while +// processing another command, this replaces the command on the top. +// Therefore don't remove the command from the queue in the loop() function. +static bool cmdbuffer_front_already_processed = false; + +// Type of a command, which is to be executed right now. +#define CMDBUFFER_CURRENT_TYPE (cmdbuffer[bufindr]) +// String of a command, which is to be executed right now. +#define CMDBUFFER_CURRENT_STRING (cmdbuffer+bufindr+1) + static int serial_count = 0; static boolean comment_mode = false; static char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc @@ -505,37 +490,236 @@ void serial_echopair_P(const char *s_P, unsigned long v) } #endif //!SDSUPPORT +// Pop the currently processed command from the queue. +// It is expected, that there is at least one command in the queue. +void cmdqueue_pop_front() +{ + if (buflen > 0) { + SERIAL_ECHOPGM("Dequeing "); + SERIAL_ECHO(cmdbuffer+bufindr+1); + SERIAL_ECHOLNPGM(""); + SERIAL_ECHOPGM("Old indices: buflen "); + SERIAL_ECHO(buflen); + SERIAL_ECHOPGM(", bufindr "); + SERIAL_ECHO(bufindr); + SERIAL_ECHOPGM(", bufindw "); + SERIAL_ECHO(bufindw); + SERIAL_ECHOPGM(", serial_count "); + SERIAL_ECHO(serial_count); + SERIAL_ECHOPGM(", bufsize "); + SERIAL_ECHO(sizeof(cmdbuffer)); + SERIAL_ECHOLNPGM(""); + if (-- buflen == 0) { + // Empty buffer. + if (serial_count == 0) + // No serial communication is pending. Reset both pointers to zero. + bufindw = 0; + bufindr = bufindw; + } else { + // There is at least one ready line in the buffer. + // First skip the current command ID and iterate up to the end of the string. + for (++ bufindr; cmdbuffer[bufindr] != 0; ++ bufindr) ; + // Second, skip the end of string null character and iterate until a nonzero command ID is found. + for (++ bufindr; bufindr < sizeof(cmdbuffer) && cmdbuffer[bufindr] == 0; ++ bufindr) ; + // If the end of the buffer was empty, + if (bufindr == sizeof(cmdbuffer)) { + // skip to the start and find the nonzero command. + for (bufindr = 0; cmdbuffer[bufindr] == 0; ++ bufindr) ; + } + SERIAL_ECHOPGM("New indices: buflen "); + SERIAL_ECHO(buflen); + SERIAL_ECHOPGM(", bufindr "); + SERIAL_ECHO(bufindr); + SERIAL_ECHOPGM(", bufindw "); + SERIAL_ECHO(bufindw); + SERIAL_ECHOPGM(", serial_count "); + SERIAL_ECHO(serial_count); + SERIAL_ECHOPGM(" new command on the top: "); + SERIAL_ECHO(cmdbuffer+bufindr+1); + SERIAL_ECHOLNPGM(""); + } + } +} + +// How long a string could be pushed to the front of the command queue? +// If yes, adjust bufindr to the new position, where the new command could be enqued. +// len_asked does not contain the zero terminator size. +bool cmdqueue_could_enqueue_front(int len_asked) +{ + // MAX_CMD_SIZE has to accommodate the zero terminator. + if (len_asked >= MAX_CMD_SIZE) + return false; + // Remove the currently processed command from the queue. + if (! cmdbuffer_front_already_processed) { + cmdqueue_pop_front(); + cmdbuffer_front_already_processed = true; + } + if (bufindr == bufindw && buflen > 0) + // Full buffer. + return false; + // Adjust the end of the write buffer based on whether a partial line is in the receive buffer. + int endw = (serial_count > 0) ? (bufindw + MAX_CMD_SIZE + 1) : bufindw; + if (bufindw < bufindr) + // Simple case. There is a contiguous space between the write buffer and the read buffer. + return endw + len_asked + 2 < bufindr; + // Otherwise the free space is split between the start and end. + if (len_asked + 2 <= bufindr) { + // Could fit at the start. + bufindr -= len_asked + 2; + return true; + } + int bufindr_new = sizeof(cmdbuffer) - len_asked - 2; + if (endw <= bufindr_new) { + memset(cmdbuffer, 0, bufindr); + bufindr = bufindr_new; + return true; + } + return false; +} + +// Could one enqueue a command of lenthg len_asked into the buffer, +// while leaving CMDBUFFER_RESERVE_FRONT at the start? +// If yes, adjust bufindw to the new position, where the new command could be enqued. +// len_asked does not contain the zero terminator size. +bool cmdqueue_could_enqueue_back(int len_asked) +{ + // MAX_CMD_SIZE has to accommodate the zero terminator. + if (len_asked >= MAX_CMD_SIZE) + return false; + + if (bufindr == bufindw && buflen > 0) + // Full buffer. + return false; + + if (serial_count > 0) { + // If there is some data stored starting at bufindw, len_asked is certainly smaller than + // the allocated data buffer. Try to reserve a new buffer and to move the already received + // serial data. + // How much memory to reserve for the commands pushed to the front? + // End of the queue, when pushing to the end. + int endw = bufindw + len_asked + 2; + if (bufindw < bufindr) + // Simple case. There is a contiguous space between the write buffer and the read buffer. + return endw + CMDBUFFER_RESERVE_FRONT <= bufindr; + // Otherwise the free space is split between the start and end. + if (// Could one fit to the end, including the reserve? + endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) || + // Could one fit to the end, and the reserve to the start? + (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr)) + return true; + // Could one fit both to the start? + if (len_asked + 2 + CMDBUFFER_RESERVE_FRONT <= bufindr) { + // Mark the rest of the buffer as used. + memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw); + // and point to the start. + bufindw = 0; + return true; + } + } else { + // How much memory to reserve for the commands pushed to the front? + // End of the queue, when pushing to the end. + int endw = bufindw + len_asked + 2; + if (bufindw < bufindr) + // Simple case. There is a contiguous space between the write buffer and the read buffer. + return endw + CMDBUFFER_RESERVE_FRONT <= bufindr; + // Otherwise the free space is split between the start and end. + if (// Could one fit to the end, including the reserve? + endw + CMDBUFFER_RESERVE_FRONT <= sizeof(cmdbuffer) || + // Could one fit to the end, and the reserve to the start? + (endw <= sizeof(cmdbuffer) && CMDBUFFER_RESERVE_FRONT <= bufindr)) + return true; + // Could one fit both to the start? + if (len_asked + 2 + CMDBUFFER_RESERVE_FRONT <= bufindr) { + // Mark the rest of the buffer as used. + memset(cmdbuffer+bufindw, 0, sizeof(cmdbuffer)-bufindw); + // and point to the start. + bufindw = 0; + return true; + } + } + return false; +} + +void cmdqueue_dump_to_serial() +{ + SERIAL_ECHOLNPGM("Content of the buffer: "); + if (buflen == 0) { + SERIAL_ECHOLNPGM("The command buffer is empty."); + } else { + SERIAL_ECHOPGM("Number of entries: "); + SERIAL_ECHO(buflen); + SERIAL_ECHOLNPGM(""); + } + if (bufindr < bufindw) { + + } else { +// for (uint8_t i = 0; i < BUFSIZE; ++ i) +// SERIAL_ECHO(cmdbuffer[(i+bufindw)%BUFSIZE]); + } + SERIAL_ECHOLNPGM("End of the buffer."); +} + //adds an command to the main command buffer //thats really done in a non-safe way. //needs overworking someday -void enquecommand(const char *cmd) +// Currently the maximum length of a command piped through this function is around 20 characters +void enquecommand(const char *cmd, bool from_progmem) { - if(buflen < BUFSIZE) - { - //this is dangerous if a mixing of serial and this happens - strcpy(&(cmdbuffer[bufindw][0]),cmd); - SERIAL_ECHO_START; - SERIAL_ECHORPGM(MSG_Enqueing); - SERIAL_ECHO(cmdbuffer[bufindw]); - SERIAL_ECHOLNPGM("\""); - bufindw= (bufindw + 1)%BUFSIZE; - buflen += 1; - } + int len = from_progmem ? strlen_P(cmd) : strlen(cmd); + // Does cmd fit the queue while leaving sufficient space at the front for the chained commands? + // If it fits, it may move bufindw, so it points to a contiguous buffer, which fits cmd. + if (cmdqueue_could_enqueue_back(len)) { + // This is dangerous if a mixing of serial and this happens + // This may easily be tested: If serial_count > 0, we have a problem. + cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_UI; + if (from_progmem) + strcpy_P(cmdbuffer + bufindw + 1, cmd); + else + strcpy(cmdbuffer + bufindw + 1, cmd); + SERIAL_ECHO_START; + SERIAL_ECHORPGM(MSG_Enqueing); + SERIAL_ECHO(cmdbuffer + bufindw + 1); + SERIAL_ECHOLNPGM("\""); + bufindw += len + 2; + if (bufindw == sizeof(cmdbuffer)) + bufindw = 0; + ++ buflen; + } else { + SERIAL_ECHO_START; + SERIAL_ECHORPGM(MSG_Enqueing); + if (from_progmem) + SERIAL_PROTOCOLRPGM(cmd); + else + SERIAL_ECHO(cmd); + SERIAL_ECHOLNPGM("\" failed: Buffer full!"); + cmdqueue_dump_to_serial(); + } } -void enquecommand_P(const char *cmd) +void enquecommand_front(const char *cmd, bool from_progmem) { - if(buflen < BUFSIZE) - { - //this is dangerous if a mixing of serial and this happens - strcpy_P(&(cmdbuffer[bufindw][0]),cmd); - SERIAL_ECHO_START; - SERIAL_ECHORPGM(MSG_Enqueing); - SERIAL_ECHO(cmdbuffer[bufindw]); - SERIAL_ECHOLNPGM("\""); - bufindw= (bufindw + 1)%BUFSIZE; - buflen += 1; - } + int len = from_progmem ? strlen_P(cmd) : strlen(cmd); + // Does cmd fit the queue? This call shall move bufindr, so the command may be copied. + if (cmdqueue_could_enqueue_front(len)) { + cmdbuffer[bufindr] = CMDBUFFER_CURRENT_TYPE_UI; + if (from_progmem) + strcpy_P(cmdbuffer + bufindr + 1, cmd); + else + strcpy(cmdbuffer + bufindr + 1, cmd); + SERIAL_ECHO_START; + SERIAL_ECHOPGM("Enqueing to the front: \""); + SERIAL_ECHO(cmdbuffer + bufindr + 1); + SERIAL_ECHOLNPGM("\""); + } else { + SERIAL_ECHO_START; + SERIAL_ECHOPGM("Enqueing to the front: \""); + if (from_progmem) + SERIAL_PROTOCOLRPGM(cmd); + else + SERIAL_ECHO(cmd); + SERIAL_ECHOLNPGM("\" failed: Buffer full!"); + cmdqueue_dump_to_serial(); + } } void setup_killpin() @@ -546,16 +730,6 @@ void setup_killpin() #endif } -// Set home pin -void setup_homepin(void) -{ -#if defined(HOME_PIN) && HOME_PIN > -1 - SET_INPUT(HOME_PIN); - WRITE(HOME_PIN,HIGH); -#endif -} - - void setup_photpin() { #if defined(PHOTOGRAPH_PIN) && PHOTOGRAPH_PIN > -1 @@ -605,21 +779,6 @@ void servo_init() #if (NUM_SERVOS >= 5) #error "TODO: enter initalisation code for more servos" #endif - - // Set position of Servo Endstops that are defined - #ifdef SERVO_ENDSTOPS - for(int8_t i = 0; i < 3; i++) - { - if(servo_endstops[i] > -1) { - servos[servo_endstops[i]].write(servo_endstop_angles[i * 2 + 1]); - } - } - #endif - - #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - delay(PROBE_SERVO_DEACTIVATION_DELAY); - servos[servo_endstops[Z_AXIS]].detach(); - #endif } static void lcd_language_menu(); @@ -629,7 +788,9 @@ static void lcd_language_menu(); enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet }; #endif - +// "Setup" function is called by the Arduino framework on startup. +// Before startup, the Timers-functions (PWM)/Analog RW and HardwareSerial provided by the Arduino-code +// are initialized by the main() routine provided by the Arduino framework. void setup() { setup_killpin(); @@ -681,10 +842,6 @@ void setup() SERIAL_ECHO(freeMemory()); SERIAL_ECHORPGM(MSG_PLANNER_BUFFER_BYTES); SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE); - for(int8_t i = 0; i < BUFSIZE; i++) - { - fromsd[i] = false; - } // loads data from EEPROM if available else uses defaults (and resets step acceleration rate) Config_RetrieveSettings(); @@ -695,7 +852,9 @@ void setup() st_init(); // Initialize stepper, this enables interrupts! setup_photpin(); servo_init(); - + // Reset the machine correction matrix. + // It does not make sense to load the correction matrix until the machine is homed. + world2machine_reset(); lcd_init(); if(!READ(BTN_ENC) ){ @@ -723,10 +882,6 @@ void setup() #ifdef DIGIPOT_I2C digipot_i2c_init(); #endif -#ifdef Z_PROBE_SLED - pinMode(SERVO0_PIN, OUTPUT); - digitalWrite(SERVO0_PIN, LOW); // turn it off -#endif // Z_PROBE_SLED setup_homepin(); #if defined(Z_AXIS_ALWAYS_ON) @@ -734,8 +889,8 @@ void setup() #endif } -//unsigned char first_run_ever=1; -//void first_time_menu(); +// The loop() function is called in an endless loop by the Arduino framework from the default main() routine. +// Before loop(), the setup() function is called by the main() routine. void loop() { @@ -750,10 +905,8 @@ void loop() is_usb_printing = false; } + get_command(); - - if(buflen < (BUFSIZE-1)) - get_command(); #ifdef SDSUPPORT card.checkautostart(false); #endif @@ -762,33 +915,27 @@ void loop() #ifdef SDSUPPORT if(card.saving) { - if(strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL) - { - card.write_command(cmdbuffer[bufindr]); + // Saving a G-code file onto an SD-card is in progress. + // Saving starts with M28, saving until M29 is seen. + if(strstr_P(CMDBUFFER_CURRENT_STRING, PSTR("M29")) == NULL) { + card.write_command(CMDBUFFER_CURRENT_STRING); if(card.logging) - { process_commands(); - } else - { SERIAL_PROTOCOLLNRPGM(MSG_OK); - } - } - else - { + } else { card.closefile(); SERIAL_PROTOCOLLNRPGM(MSG_FILE_SAVED); } - } - else - { + } else { process_commands(); } #else process_commands(); #endif //SDSUPPORT - buflen = (buflen-1); - bufindr = (bufindr + 1)%BUFSIZE; + if (! cmdbuffer_front_already_processed) + cmdqueue_pop_front(); + cmdbuffer_front_already_processed = false; } //check heater every n milliseconds manage_heater(); @@ -799,8 +946,12 @@ void loop() void get_command() { - while( MYSERIAL.available() > 0 && buflen < BUFSIZE) { - serial_char = MYSERIAL.read(); + // Test and reserve space for the new command string. + if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1)) + return; + + while (MYSERIAL.available() > 0) { + char serial_char = MYSERIAL.read(); if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || @@ -810,15 +961,17 @@ void get_command() comment_mode = false; //for new command return; } - cmdbuffer[bufindw][serial_count] = 0; //terminate string + cmdbuffer[bufindw+serial_count+1] = 0; //terminate string if(!comment_mode){ comment_mode = false; //for new command - fromsd[bufindw] = false; - if(strchr(cmdbuffer[bufindw], 'N') != NULL) + if ((strchr_pointer = strchr(cmdbuffer+bufindw+1, 'N')) != NULL) { - strchr_pointer = strchr(cmdbuffer[bufindw], 'N'); - gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10)); - if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) { + // Line number met. When sending a G-code over a serial line, each line may be stamped with its index, + // and Marlin tests, whether the successive lines are stamped with an increasing line number ID. + gcode_N = (strtol(strchr_pointer+1, NULL, 10)); + if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer+bufindw+1, PSTR("M110")) == NULL) ) { + // M110 - set current line number. + // Line numbers not sent in succession. SERIAL_ERROR_START; SERIAL_ERRORRPGM(MSG_ERR_LINE_NO); SERIAL_ERRORLN(gcode_LastN); @@ -828,14 +981,13 @@ void get_command() return; } - if(strchr(cmdbuffer[bufindw], '*') != NULL) + if((strchr_pointer = strchr(cmdbuffer+bufindw+1, '*')) != NULL) { byte checksum = 0; - byte count = 0; - while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++]; - strchr_pointer = strchr(cmdbuffer[bufindw], '*'); - - if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) { + char *p = cmdbuffer+bufindw+1; + while (p != strchr_pointer) + checksum = checksum^(*p++); + if (int(strtol(strchr_pointer+1, NULL, 10)) != int(checksum)) { SERIAL_ERROR_START; SERIAL_ERRORRPGM(MSG_ERR_CHECKSUM_MISMATCH); SERIAL_ERRORLN(gcode_LastN); @@ -843,7 +995,8 @@ void get_command() serial_count = 0; return; } - //if no errors, continue parsing + // If no errors, remove the checksum and continue parsing. + *strchr_pointer = 0; } else { @@ -857,10 +1010,10 @@ void get_command() gcode_LastN = gcode_N; //if no errors, continue parsing - } + } // end of 'N' command else // if we don't receive 'N' but still see '*' { - if((strchr(cmdbuffer[bufindw], '*') != NULL)) + if((strchr(cmdbuffer+bufindw+1, '*') != NULL)) { SERIAL_ERROR_START; SERIAL_ERRORRPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM); @@ -868,51 +1021,54 @@ void get_command() serial_count = 0; return; } - } - if((strchr(cmdbuffer[bufindw], 'G') != NULL)){ - strchr_pointer = strchr(cmdbuffer[bufindw], 'G'); - - if (!IS_SD_PRINTING) - { - usb_printing_counter = 10; - is_usb_printing = true; - } - - switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))) - { - case 0: - case 1: - case 2: - case 3: - if (Stopped == true) - { - SERIAL_ERRORLNRPGM(MSG_ERR_STOPPED); - LCD_MESSAGERPGM(MSG_STOPPED); - } - break; - default: - break; - } - - } + } // end of '*' command + if ((strchr_pointer = strchr(cmdbuffer+bufindw+1, 'G')) != NULL) { + if (! IS_SD_PRINTING) { + usb_printing_counter = 10; + is_usb_printing = true; + } + if (Stopped == true) { + int gcode = strtol(strchr_pointer+1, NULL, 10); + if (gcode >= 0 && gcode <= 3) { + SERIAL_ERRORLNRPGM(MSG_ERR_STOPPED); + LCD_MESSAGERPGM(MSG_STOPPED); + } + } + } // end of 'G' command //If command was e-stop process now - if(strcmp(cmdbuffer[bufindw], "M112") == 0) + if(strcmp(cmdbuffer+bufindw+1, "M112") == 0) kill(); - bufindw = (bufindw + 1)%BUFSIZE; - buflen += 1; - } + // Store the current line into buffer, move to the next line. + cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_USB; + SERIAL_ECHO_START; + SERIAL_ECHOPGM("Storing a command line to buffer: "); + SERIAL_ECHO(cmdbuffer+bufindw+1); + SERIAL_ECHOLNPGM(""); + bufindw += strlen(cmdbuffer+bufindw+1) + 2; + if (bufindw == sizeof(cmdbuffer)) + bufindw = 0; + ++ buflen; + } // end of 'not comment mode' serial_count = 0; //clear buffer - } - else - { + // Don't call cmdqueue_could_enqueue_back if there are no characters waiting + // in the queue, as this function will reserve the memory. + if (MYSERIAL.available() == 0 || ! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1)) + return; + } // end of "end of line" processing + else { + // Not an "end of line" symbol. Store the new character into a buffer. if(serial_char == ';') comment_mode = true; - if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char; + if(!comment_mode) cmdbuffer[bufindw+1+serial_count++] = serial_char; } - } + } // end of serial line processing loop + + #ifdef SDSUPPORT if(!card.sdprinting || serial_count!=0){ + // If there is a half filled buffer from serial line, wait until return before + // continuing with the serial line. return; } @@ -923,9 +1079,10 @@ void get_command() static bool stop_buffering=false; if(buflen==0) stop_buffering=false; - while( !card.eof() && buflen < BUFSIZE && !stop_buffering) { + // Reads whole lines from the SD card. Never leaves a half-filled line in the cmdbuffer. + while( !card.eof() && !stop_buffering) { int16_t n=card.get(); - serial_char = (char)n; + char serial_char = (char)n; if(serial_char == '\n' || serial_char == '\r' || (serial_char == '#' && comment_mode == false) || @@ -957,46 +1114,34 @@ void get_command() comment_mode = false; //for new command return; //if empty line } - cmdbuffer[bufindw][serial_count] = 0; //terminate string -// if(!comment_mode){ - fromsd[bufindw] = true; - buflen += 1; - bufindw = (bufindw + 1)%BUFSIZE; -// } + cmdbuffer[bufindw+serial_count+1] = 0; //terminate string + cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_SDCARD; + ++ buflen; + bufindw += strlen(cmdbuffer+bufindw+1) + 2; + if (bufindw == sizeof(cmdbuffer)) + bufindw = 0; comment_mode = false; //for new command serial_count = 0; //clear buffer + // The following line will reserve buffer space if available. + if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1)) + return; } else { if(serial_char == ';') comment_mode = true; - if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char; + if(!comment_mode) cmdbuffer[bufindw+1+serial_count++] = serial_char; } } #endif //SDSUPPORT - -} - - -float code_value() -{ - return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); } -long code_value_long() -{ - return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); -} -int16_t code_value_short() { - return (int16_t)(strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); -} - -bool code_seen(char code) -{ - strchr_pointer = strchr(cmdbuffer[bufindr], code); - return (strchr_pointer != NULL); //Return True if a character was found -} +// Return True if a character was found +static inline bool code_seen(char code) { return (strchr_pointer = strchr(CMDBUFFER_CURRENT_STRING, code)) != NULL; } +static inline float code_value() { return strtod(strchr_pointer+1, NULL); } +static inline long code_value_long() { return strtol(strchr_pointer+1, NULL, 10); } +static inline int16_t code_value_short() { return int16_t(strtol(strchr_pointer+1, NULL, 10)); }; #define DEFINE_PGM_READ_ANY(type, reader) \ static inline type pgm_read_any(const type *p) \ @@ -1078,59 +1223,9 @@ static void axis_is_at_home(int axis) { } } #endif -#ifdef SCARA - float homeposition[3]; - char i; - - if (axis < 2) - { - - for (i=0; i<3; i++) - { - homeposition[i] = base_home_pos(i); - } - // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]); - // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]); - // Works out real Homeposition angles using inverse kinematics, - // and calculates homing offset using forward kinematics - calculate_delta(homeposition); - - // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]); - // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); - - for (i=0; i<2; i++) - { - delta[i] -= add_homing[i]; - } - - // SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(add_homing[X_AXIS]); - // SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(add_homing[Y_AXIS]); - // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]); - // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]); - - calculate_SCARA_forward_Transform(delta); - - // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]); - // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]); - - current_position[axis] = delta[axis]; - - // SCARA home positions are based on configuration since the actual limits are determined by the - // inverse kinematic transform. - min_pos[axis] = base_min_pos(axis); // + (delta[axis] - base_home_pos(axis)); - max_pos[axis] = base_max_pos(axis); // + (delta[axis] - base_home_pos(axis)); - } - else - { - current_position[axis] = base_home_pos(axis) + add_homing[axis]; - min_pos[axis] = base_min_pos(axis) + add_homing[axis]; - max_pos[axis] = base_max_pos(axis) + add_homing[axis]; - } -#else current_position[axis] = base_home_pos(axis) + add_homing[axis]; min_pos[axis] = base_min_pos(axis) + add_homing[axis]; max_pos[axis] = base_max_pos(axis) + add_homing[axis]; -#endif } @@ -1268,52 +1363,14 @@ static void do_blocking_move_relative(float offset_x, float offset_y, float offs } -static void engage_z_probe() { - // Engage Z Servo endstop if enabled - #ifdef SERVO_ENDSTOPS - if (servo_endstops[Z_AXIS] > -1) { -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - servos[servo_endstops[Z_AXIS]].attach(0); -#endif - servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]); -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - delay(PROBE_SERVO_DEACTIVATION_DELAY); - servos[servo_endstops[Z_AXIS]].detach(); -#endif - } - #endif -} - -static void retract_z_probe() { - // Retract Z Servo endstop if enabled - #ifdef SERVO_ENDSTOPS - if (servo_endstops[Z_AXIS] > -1) { -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - servos[servo_endstops[Z_AXIS]].attach(0); -#endif - servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]); -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - delay(PROBE_SERVO_DEACTIVATION_DELAY); - servos[servo_endstops[Z_AXIS]].detach(); -#endif - } - #endif -} - /// Probe bed height at position (x,y), returns the measured z value static float probe_pt(float x, float y, float z_before) { // move to right place do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before); do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]); -#ifndef Z_PROBE_SLED - engage_z_probe(); // Engage Z Servo endstop if available -#endif // Z_PROBE_SLED run_z_probe(); float measured_z = current_position[Z_AXIS]; -#ifndef Z_PROBE_SLED - retract_z_probe(); -#endif // Z_PROBE_SLED SERIAL_PROTOCOLRPGM(MSG_BED); SERIAL_PROTOCOLPGM(" x: "); @@ -1341,27 +1398,10 @@ static void homeaxis(int axis) { if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder); #endif - - current_position[axis] = 0; plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - -#ifndef Z_PROBE_SLED - // Engage Servo endstop if enabled - #ifdef SERVO_ENDSTOPS - #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - if (axis==Z_AXIS) { - engage_z_probe(); - } - else - #endif - if (servo_endstops[axis] > -1) { - servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]); - } - #endif -#endif // Z_PROBE_SLED destination[axis] = 1.5 * max_length(axis) * axis_home_dir; feedrate = homing_feedrate[axis]; plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); @@ -1374,43 +1414,16 @@ static void homeaxis(int axis) { st_synchronize(); destination[axis] = 2*home_retract_mm(axis) * axis_home_dir; -#ifdef DELTA - feedrate = homing_feedrate[axis]/10; -#else feedrate = homing_feedrate[axis]/2 ; -#endif plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); st_synchronize(); -#ifdef DELTA - // retrace by the amount specified in endstop_adj - if (endstop_adj[axis] * axis_home_dir < 0) { - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - destination[axis] = endstop_adj[axis]; - plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); - st_synchronize(); - } -#endif axis_is_at_home(axis); destination[axis] = current_position[axis]; feedrate = 0.0; endstops_hit_on_purpose(); axis_known_position[axis] = true; - - // Retract Servo endstop if enabled - #ifdef SERVO_ENDSTOPS - if (servo_endstops[axis] > -1) { - servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]); - } - #endif -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - #ifndef Z_PROBE_SLED - if (axis==Z_AXIS) retract_z_probe(); - #endif -#endif - } } -#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS) void refresh_cmd_timeout(void) { @@ -1435,12 +1448,7 @@ void refresh_cmd_timeout(void) retracted[active_extruder]=true; prepare_move(); current_position[Z_AXIS]-=retract_zlift; -#ifdef DELTA - calculate_delta(current_position); // change cartesian kinematic to delta kinematic; - plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); -#else plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); -#endif prepare_move(); feedrate = oldFeedrate; } else if(!retracting && retracted[active_extruder]) { @@ -1449,12 +1457,7 @@ void refresh_cmd_timeout(void) destination[Z_AXIS]=current_position[Z_AXIS]; destination[E_AXIS]=current_position[E_AXIS]; current_position[Z_AXIS]+=retract_zlift; -#ifdef DELTA - calculate_delta(current_position); // change cartesian kinematic to delta kinematic; - plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); -#else plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); -#endif //prepare_move(); if (swapretract) { current_position[E_AXIS]-=(retract_length_swap+retract_recover_length_swap)/volumetric_multiplier[active_extruder]; @@ -1471,42 +1474,6 @@ void refresh_cmd_timeout(void) } //retract #endif //FWRETRACT -#ifdef Z_PROBE_SLED -// -// Method to dock/undock a sled designed by Charles Bell. -// -// dock[in] If true, move to MAX_X and engage the electromagnet -// offset[in] The additional distance to move to adjust docking location -// -static void dock_sled(bool dock, int offset=0) { - int z_loc; - - if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) { - LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN); - SERIAL_ECHO_START; - SERIAL_ECHOLNRPGM(MSG_POSITION_UNKNOWN); - return; - } - - if (dock) { - do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, - current_position[Y_AXIS], - current_position[Z_AXIS]); - // turn off magnet - digitalWrite(SERVO0_PIN, LOW); - } else { - if (current_position[Z_AXIS] < (Z_RAISE_BEFORE_PROBING + 5)) - z_loc = Z_RAISE_BEFORE_PROBING; - else - z_loc = current_position[Z_AXIS]; - do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, - Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc); - // turn on magnet - digitalWrite(SERVO0_PIN, HIGH); - } -} -#endif - void process_commands() { #ifdef FILAMENT_RUNOUT_SUPPORT @@ -1747,7 +1714,6 @@ void process_commands() //ClearToSend(); } break; -#ifndef SCARA //disable arc support case 2: // G2 - CW ARC if(Stopped == false) { get_arc_coordinates(); @@ -1760,7 +1726,6 @@ void process_commands() prepare_arc_move(false); } break; -#endif case 4: // G4 dwell LCD_MESSAGERPGM(MSG_DWELL); codenum = 0; @@ -1805,6 +1770,10 @@ void process_commands() mbl.active = 0; #endif + // Reset world2machine_rotation_and_skew and world2machine_shift, therefore + // the planner will not perform any adjustments in the XY plane. + world2machine_reset(); + saved_feedrate = feedrate; saved_feedmultiply = feedmultiply; feedmultiply = 100; @@ -1817,47 +1786,16 @@ void process_commands() } feedrate = 0.0; -#ifdef DELTA - // A delta can only safely home all axis at the same time - // all axis have to home at the same time - - // Move all carriages up together until the first endstop is hit. - current_position[X_AXIS] = 0; - current_position[Y_AXIS] = 0; - current_position[Z_AXIS] = 0; - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - - destination[X_AXIS] = 3 * Z_MAX_LENGTH; - destination[Y_AXIS] = 3 * Z_MAX_LENGTH; - destination[Z_AXIS] = 3 * Z_MAX_LENGTH; - feedrate = 1.732 * homing_feedrate[X_AXIS]; - plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); - st_synchronize(); - endstops_hit_on_purpose(); - - current_position[X_AXIS] = destination[X_AXIS]; - current_position[Y_AXIS] = destination[Y_AXIS]; - current_position[Z_AXIS] = destination[Z_AXIS]; - - // take care of back off and rehome now we are all at the top - HOMEAXIS(X); - HOMEAXIS(Y); - HOMEAXIS(Z); - - calculate_delta(current_position); - plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); - -#else // NOT DELTA - home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))); #if Z_HOME_DIR > 0 // If homing away from BED do Z first if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) { - HOMEAXIS(Z); + homeaxis(Z_AXIS); } #endif #ifdef QUICK_HOME + // In the quick mode, if both x and y are to be homed, a diagonal move will be performed initially. if((home_all_axis)||( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS])) ) //first diagonal move { current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0; @@ -1894,11 +1832,9 @@ void process_commands() current_position[X_AXIS] = destination[X_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS]; - #ifndef SCARA current_position[Z_AXIS] = destination[Z_AXIS]; - #endif } - #endif + #endif /* QUICK_HOME */ if (home_all_axis) { @@ -1911,41 +1847,33 @@ void process_commands() int tmp_extruder = active_extruder; extruder_duplication_enabled = false; active_extruder = !active_extruder; - HOMEAXIS(X); + homeaxis(X_AXIS); inactive_extruder_x_pos = current_position[X_AXIS]; active_extruder = tmp_extruder; - HOMEAXIS(X); + homeaxis(X_AXIS); // reset state used by the different modes memcpy(raised_parked_position, current_position, sizeof(raised_parked_position)); delayed_move_time = 0; active_extruder_parked = true; #else - HOMEAXIS(X); + homeaxis(X_AXIS); #endif } if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) { - HOMEAXIS(Y); + homeaxis(Y_AXIS); } if(code_seen(axis_codes[X_AXIS])) { if(code_value_long() != 0) { - #ifdef SCARA - current_position[X_AXIS]=code_value(); - #else current_position[X_AXIS]=code_value()+add_homing[X_AXIS]; - #endif } } if(code_seen(axis_codes[Y_AXIS])) { if(code_value_long() != 0) { - #ifdef SCARA - current_position[Y_AXIS]=code_value(); - #else current_position[Y_AXIS]=code_value()+add_homing[Y_AXIS]; - #endif } } @@ -1961,11 +1889,15 @@ void process_commands() #ifdef MESH_BED_LEVELING // If Mesh bed leveling, moxve X&Y to safe position for home if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] )) { - HOMEAXIS(X); - HOMEAXIS(Y); + homeaxis(X_AXIS); + homeaxis(Y_AXIS); } // 1st mesh bed leveling measurement point, corrected. - mbl.get_meas_xy(0, 0, destination[X_AXIS], destination[Y_AXIS], false); + world2machine_initialize(); + current_position[X_AXIS] = world2machine_rotation_and_skew[0][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[0][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[0]; + current_position[Y_AXIS] = world2machine_rotation_and_skew[1][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[1][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[1]; + world2machine_reset(); +// mbl.get_meas_xy(0, 0, destination[X_AXIS], destination[Y_AXIS], false); // destination[X_AXIS] = MESH_MIN_X - X_PROBE_OFFSET_FROM_EXTRUDER; // destination[Y_AXIS] = MESH_MIN_Y - Y_PROBE_OFFSET_FROM_EXTRUDER; destination[Z_AXIS] = MESH_HOME_Z_SEARCH; // Set destination away from bed @@ -1977,10 +1909,10 @@ void process_commands() st_synchronize(); current_position[X_AXIS] = destination[X_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS]; - HOMEAXIS(Z); + homeaxis(Z_AXIS); _doMeshL = true; #else // MESH_BED_LEVELING - HOMEAXIS(Z); + homeaxis(Z_AXIS); #endif // MESH_BED_LEVELING } #else // defined(Z_SAFE_HOMING): Z Safe mode activated. @@ -1997,7 +1929,7 @@ void process_commands() current_position[X_AXIS] = destination[X_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS]; - HOMEAXIS(Z); + homeaxis(Z_AXIS); } // Let's see if X and Y are homed and probe is inside bed area. if(code_seen(axis_codes[Z_AXIS])) { @@ -2014,7 +1946,7 @@ void process_commands() plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); st_synchronize(); - HOMEAXIS(Z); + homeaxis(Z_AXIS); } else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) { LCD_MESSAGERPGM(MSG_POSITION_UNKNOWN); SERIAL_ECHO_START; @@ -2038,15 +1970,8 @@ void process_commands() current_position[Z_AXIS] += zprobe_zoffset; //Add Z_Probe offset (the distance is negative) } #endif - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); -#endif // else DELTA - -#ifdef SCARA - calculate_delta(current_position); - plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); -#endif // SCARA #ifdef ENDSTOPS_ONLY_FOR_HOMING enable_endstops(false); @@ -2065,6 +1990,11 @@ void process_commands() } #endif + // Load the machine correction matrix + world2machine_initialize(); + // and correct the current_position to match the transformed coordinate system. + world2machine_update_current(); + #ifdef MESH_BED_LEVELING if (code_seen('W')) { @@ -2075,7 +2005,9 @@ void process_commands() if ( _doMeshL) { st_synchronize(); - enquecommand_P((PSTR("G80"))); + // Push the commands to the front of the message queue in the reverse order! + // There shall be always enough space reserved for these commands. + enquecommand_front_P((PSTR("G80"))); } #endif @@ -2097,9 +2029,6 @@ void process_commands() break; // abort G29, since we don't know where we are } -#ifdef Z_PROBE_SLED - dock_sled(false); -#endif // Z_PROBE_SLED st_synchronize(); // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly //vector_3 corrected_position = plan_get_position_mm(); @@ -2225,15 +2154,11 @@ void process_commands() apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); -#ifdef Z_PROBE_SLED - dock_sled(true, -SLED_DOCKING_OFFSET); // correct for over travel. -#endif // Z_PROBE_SLED } break; #ifndef Z_PROBE_SLED case 30: // G30 Single Z Probe { - engage_z_probe(); // Engage Z Servo endstop if available st_synchronize(); // TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly setup_for_endstop_move(); @@ -2251,7 +2176,6 @@ void process_commands() SERIAL_PROTOCOLPGM("\n"); clean_up_after_endstop_move(); - retract_z_probe(); // Retract Z Servo endstop if available } break; #else @@ -2290,8 +2214,10 @@ void process_commands() // Firstly check if we know where we are if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS] ) ){ // We don't know where we are! HOME! - enquecommand_P((PSTR("G28 W0"))); - enquecommand_P((PSTR("G80"))); + // Push the commands to the front of the message queue in the reverse order! + // There shall be always enough space reserved for these commands. + enquecommand_front_P((PSTR("G80"))); + enquecommand_front_P((PSTR("G28 W0"))); break; } @@ -2302,7 +2228,9 @@ void process_commands() current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder); // The move to the first calibration point. - mbl.get_meas_xy(0, 0, current_position[X_AXIS], current_position[Y_AXIS], false); + current_position[X_AXIS] = pgm_read_float(bed_ref_points); + current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1); +// mbl.get_meas_xy(0, 0, current_position[X_AXIS], current_position[Y_AXIS], false); plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/30, active_extruder); // Wait until the move is finished. st_synchronize(); @@ -2316,7 +2244,7 @@ void process_commands() int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS]/60; int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS]/40; setup_for_endstop_move(); - while (!(mesh_point == ((MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) ))) { + while (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) { // Move Z to proper distance current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; @@ -2328,7 +2256,9 @@ void process_commands() iy = mesh_point / MESH_MEAS_NUM_X_POINTS; if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag - mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false); + current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point); + current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1); +// mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false); enable_endstops(false); plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder); st_synchronize(); @@ -2479,23 +2409,14 @@ void process_commands() plan_set_e_position(current_position[E_AXIS]); } else { -#ifdef SCARA - if (i == X_AXIS || i == Y_AXIS) { - current_position[i] = code_value(); - } - else { - current_position[i] = code_value()+add_homing[i]; - } -#else current_position[i] = code_value()+add_homing[i]; -#endif plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); } } } break; } - } + } // end if(code_seen('G')) else if(code_seen('M')) { @@ -2603,7 +2524,7 @@ void process_commands() case 28: //M28 - Start SD write starpos = (strchr(strchr_pointer + 4,'*')); if(starpos != NULL){ - char* npos = strchr(cmdbuffer[bufindr], 'N'); + char* npos = strchr(CMDBUFFER_CURRENT_STRING, 'N'); strchr_pointer = strchr(npos,' ') + 1; *(starpos) = '\0'; } @@ -2618,7 +2539,7 @@ void process_commands() card.closefile(); starpos = (strchr(strchr_pointer + 4,'*')); if(starpos != NULL){ - char* npos = strchr(cmdbuffer[bufindr], 'N'); + char* npos = strchr(CMDBUFFER_CURRENT_STRING, 'N'); strchr_pointer = strchr(npos,' ') + 1; *(starpos) = '\0'; } @@ -2663,7 +2584,7 @@ void process_commands() case 928: //M928 - Start SD write starpos = (strchr(strchr_pointer + 5,'*')); if(starpos != NULL){ - char* npos = strchr(cmdbuffer[bufindr], 'N'); + char* npos = strchr(CMDBUFFER_CURRENT_STRING, 'N'); strchr_pointer = strchr(npos,' ') + 1; *(starpos) = '\0'; } @@ -2715,69 +2636,44 @@ void process_commands() } break; - case 44: + case 45: reset_bed_offset_and_skew(); + world2machine_reset(); break; - case 45: // M45: mesh_bed_calibration - { - // Firstly check if we know where we are - if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ){ - // We don't know where we are! HOME! - enquecommand_P((PSTR("G28 X0 Y0"))); - enquecommand_P((PSTR("M45"))); - break; - } - - setup_for_endstop_move(); - find_bed_offset_and_skew(); -// improve_bed_offset_and_skew(); - clean_up_after_endstop_move(); - current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder); - /* - current_position[X_AXIS] = X_MIN_POS+0.2; - current_position[Y_AXIS] = Y_MIN_POS+0.2; - current_position[Z_AXIS] = Z_MIN_POS; - plan_buffer_line(current_position[X_AXIS], current_position[X_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder); - */ - st_synchronize(); - } - break; - case 46: // M46: mesh_bed_calibration with manual Z up { // Firstly check if we know where we are if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ){ // We don't know where we are! HOME! - enquecommand_P((PSTR("G28 X0 Y0 W0"))); // W0 tells G28 to not perform mesh bed leveling. - enquecommand_P((PSTR("M46"))); + // Push the commands to the front of the message queue in the reverse order! + // There shall be always enough space reserved for these commands. + enquecommand_front_P((PSTR("M46"))); + enquecommand_front_P((PSTR("G28 X Y"))); break; } lcd_update_enable(false); if (lcd_calibrate_z_end_stop_manual()) { - mbl.reset(); - setup_for_endstop_move(); - find_bed_offset_and_skew(); -// improve_bed_offset_and_skew(1); - clean_up_after_endstop_move(); - // Print head up. - current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; - plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder); - st_synchronize(); + mbl.reset(); + setup_for_endstop_move(); + find_bed_offset_and_skew(); + clean_up_after_endstop_move(); + // Print head up. + current_position[Z_AXIS] = MESH_HOME_Z_SEARCH; + plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder); + st_synchronize(); + // Push the commands to the front of the message queue in the reverse order! + // There shall be always enough space reserved for these commands. + enquecommand_front_P((PSTR("M47"))); + enquecommand_front_P((PSTR("G28 X Y"))); + } else { + // User canceled the operation. Give up. + lcd_update_enable(true); + lcd_implementation_clear(); + // lcd_return_to_status(); + lcd_update(); } - // lcd_update_enable(true); - //lcd_implementation_clear(); - //lcd_return_to_status(); - // lcd_update(); - // Mesh bed leveling. - // enquecommand_P((PSTR("G80"))); - // The iprovement. - - //enquecommand_P((PSTR("G80"))); - enquecommand_P((PSTR("G28 X0 Y0 W0"))); - enquecommand_P((PSTR("M47"))); } break; @@ -2786,8 +2682,10 @@ void process_commands() // Firstly check if we know where we are if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ) { // We don't know where we are! HOME! - enquecommand_P((PSTR("G28 X0 Y0 W0"))); // W0 tells G28 to not perform mesh bed leveling. - enquecommand_P((PSTR("M47"))); + // Push the commands to the front of the message queue in the reverse order! + // There shall be always enough space reserved for these commands. + enquecommand_front_P((PSTR("M47"))); + enquecommand_front_P((PSTR("G28 X Y"))); break; } lcd_update_enable(false); @@ -2801,19 +2699,22 @@ void process_commands() st_synchronize(); lcd_update_enable(true); lcd_update(); - if (success) + if (success) { // Mesh bed leveling. - enquecommand_P((PSTR("G80"))); + // Push the commands to the front of the message queue in the reverse order! + // There shall be always enough space reserved for these commands. + enquecommand_front_P((PSTR("G80"))); + } break; } - // case 47: - // lcd_diag_show_end_stops(); - // break; + case 48: + lcd_diag_show_end_stops(); + break; // M48 Z-Probe repeatability measurement function. // -// Usage: M48 <n #_samples> <X X_position_for_samples> <Y Y_position_for_samples> <V Verbose_Level> <Engage_probe_for_each_reading> <L legs_of_movement_prior_to_doing_probe> +// Usage: M48 <n #_samples> <X X_position_for_samples> <Y Y_position_for_samples> <V Verbose_Level> <L legs_of_movement_prior_to_doing_probe> // // This function assumes the bed has been homed. Specificaly, that a G28 command // as been issued prior to invoking the M48 Z-Probe repeatability measurement function. @@ -2838,7 +2739,7 @@ void process_commands() double mean=0.0; double sigma=0.0; double sample_set[50]; - int verbose_level=1, n=0, j, n_samples = 10, n_legs=0, engage_probe_for_each_reading=0 ; + int verbose_level=1, n=0, j, n_samples = 10, n_legs=0; double X_current, Y_current, Z_current; double X_probe_location, Y_probe_location, Z_start_location, ext_position; @@ -2869,9 +2770,6 @@ void process_commands() Z_start_location = st_get_position_mm(Z_AXIS) + Z_RAISE_BEFORE_PROBING; ext_position = st_get_position_mm(E_AXIS); - if (code_seen('E') || code_seen('e') ) - engage_probe_for_each_reading++; - if (code_seen('X') || code_seen('x') ) { X_probe_location = code_value() - X_PROBE_OFFSET_FROM_EXTRUDER; if (X_probe_location<X_MIN_POS || X_probe_location>X_MAX_POS ) { @@ -2934,8 +2832,6 @@ void process_commands() // Then retrace the right amount and use that in subsequent probes // - engage_z_probe(); - setup_for_endstop_move(); run_z_probe(); @@ -2949,9 +2845,6 @@ void process_commands() st_synchronize(); current_position[Z_AXIS] = Z_current = st_get_position_mm(Z_AXIS); - if (engage_probe_for_each_reading) - retract_z_probe(); - for( n=0; n<n_samples; n++) { do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location @@ -3003,11 +2896,6 @@ void process_commands() do_blocking_move_to( X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location } - if (engage_probe_for_each_reading) { - engage_z_probe(); - delay(1000); - } - setup_for_endstop_move(); run_z_probe(); @@ -3055,13 +2943,8 @@ void process_commands() current_position[E_AXIS], homing_feedrate[Z_AXIS]/60, active_extruder); st_synchronize(); - if (engage_probe_for_each_reading) { - retract_z_probe(); - delay(1000); - } } - retract_z_probe(); delay(1000); clean_up_after_endstop_move(); @@ -3321,37 +3204,6 @@ Sigma_Exit: fanSpeed = 0; break; #endif //FAN_PIN - #ifdef BARICUDA - // PWM for HEATER_1_PIN - #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 - case 126: //M126 valve open - if (code_seen('S')){ - ValvePressure=constrain(code_value(),0,255); - } - else { - ValvePressure=255; - } - break; - case 127: //M127 valve closed - ValvePressure = 0; - break; - #endif //HEATER_1_PIN - - // PWM for HEATER_2_PIN - #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 - case 128: //M128 valve open - if (code_seen('S')){ - EtoPPressure=constrain(code_value(),0,255); - } - else { - EtoPPressure=255; - } - break; - case 129: //M129 valve closed - EtoPPressure = 0; - break; - #endif //HEATER_2_PIN - #endif #if defined(PS_ON_PIN) && PS_ON_PIN > -1 case 80: // M80 - Turn on Power Supply @@ -3496,26 +3348,6 @@ Sigma_Exit: SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]); SERIAL_PROTOCOLLN(""); -#ifdef SCARA - SERIAL_PROTOCOLPGM("SCARA Theta:"); - SERIAL_PROTOCOL(delta[X_AXIS]); - SERIAL_PROTOCOLPGM(" Psi+Theta:"); - SERIAL_PROTOCOL(delta[Y_AXIS]); - SERIAL_PROTOCOLLN(""); - - SERIAL_PROTOCOLPGM("SCARA Cal - Theta:"); - SERIAL_PROTOCOL(delta[X_AXIS]+add_homing[X_AXIS]); - SERIAL_PROTOCOLPGM(" Psi+Theta (90):"); - SERIAL_PROTOCOL(delta[Y_AXIS]-delta[X_AXIS]-90+add_homing[Y_AXIS]); - SERIAL_PROTOCOLLN(""); - - SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:"); - SERIAL_PROTOCOL(delta[X_AXIS]/90*axis_steps_per_unit[X_AXIS]); - SERIAL_PROTOCOLPGM(" Psi+Theta:"); - SERIAL_PROTOCOL((delta[Y_AXIS]-delta[X_AXIS])/90*axis_steps_per_unit[Y_AXIS]); - SERIAL_PROTOCOLLN(""); - SERIAL_PROTOCOLLN(""); -#endif break; case 120: // M120 enable_endstops(false) ; @@ -3681,38 +3513,7 @@ Sigma_Exit: { if(code_seen(axis_codes[i])) add_homing[i] = code_value(); } - #ifdef SCARA - if(code_seen('T')) // Theta - { - add_homing[X_AXIS] = code_value() ; - } - if(code_seen('P')) // Psi - { - add_homing[Y_AXIS] = code_value() ; - } - #endif - break; - #ifdef DELTA - case 665: // M665 set delta configurations L<diagonal_rod> R<delta_radius> S<segments_per_sec> - if(code_seen('L')) { - delta_diagonal_rod= code_value(); - } - if(code_seen('R')) { - delta_radius= code_value(); - } - if(code_seen('S')) { - delta_segments_per_second= code_value(); - } - - recalc_delta_settings(delta_radius, delta_diagonal_rod); - break; - case 666: // M666 set delta endstop adjustemnt - for(int8_t i=0; i < 3; i++) - { - if(code_seen(axis_codes[i])) endstop_adj[i] = code_value(); - } break; - #endif #ifdef FWRETRACT case 207: //M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop] { @@ -3772,7 +3573,7 @@ Sigma_Exit: default: SERIAL_ECHO_START; SERIAL_ECHORPGM(MSG_UNKNOWN_COMMAND); - SERIAL_ECHO(cmdbuffer[bufindr]); + SERIAL_ECHO(CMDBUFFER_CURRENT_STRING); SERIAL_ECHOLNPGM("\""); } } @@ -4070,123 +3871,11 @@ Sigma_Exit: PID_autotune(temp, e, c); } break; - #ifdef SCARA - case 360: // M360 SCARA Theta pos1 - SERIAL_ECHOLN(" Cal: Theta 0 "); - //SoftEndsEnabled = false; // Ignore soft endstops during calibration - //SERIAL_ECHOLN(" Soft endstops disabled "); - if(Stopped == false) { - //get_coordinates(); // For X Y Z E F - delta[X_AXIS] = 0; - delta[Y_AXIS] = 120; - calculate_SCARA_forward_Transform(delta); - destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS]; - destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS]; - - prepare_move(); - //ClearToSend(); - return; - } - break; - - case 361: // SCARA Theta pos2 - SERIAL_ECHOLN(" Cal: Theta 90 "); - //SoftEndsEnabled = false; // Ignore soft endstops during calibration - //SERIAL_ECHOLN(" Soft endstops disabled "); - if(Stopped == false) { - //get_coordinates(); // For X Y Z E F - delta[X_AXIS] = 90; - delta[Y_AXIS] = 130; - calculate_SCARA_forward_Transform(delta); - destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS]; - destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS]; - - prepare_move(); - //ClearToSend(); - return; - } - break; - case 362: // SCARA Psi pos1 - SERIAL_ECHOLN(" Cal: Psi 0 "); - //SoftEndsEnabled = false; // Ignore soft endstops during calibration - //SERIAL_ECHOLN(" Soft endstops disabled "); - if(Stopped == false) { - //get_coordinates(); // For X Y Z E F - delta[X_AXIS] = 60; - delta[Y_AXIS] = 180; - calculate_SCARA_forward_Transform(delta); - destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS]; - destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS]; - - prepare_move(); - //ClearToSend(); - return; - } - break; - case 363: // SCARA Psi pos2 - SERIAL_ECHOLN(" Cal: Psi 90 "); - //SoftEndsEnabled = false; // Ignore soft endstops during calibration - //SERIAL_ECHOLN(" Soft endstops disabled "); - if(Stopped == false) { - //get_coordinates(); // For X Y Z E F - delta[X_AXIS] = 50; - delta[Y_AXIS] = 90; - calculate_SCARA_forward_Transform(delta); - destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS]; - destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS]; - - prepare_move(); - //ClearToSend(); - return; - } - break; - case 364: // SCARA Psi pos3 (90 deg to Theta) - SERIAL_ECHOLN(" Cal: Theta-Psi 90 "); - // SoftEndsEnabled = false; // Ignore soft endstops during calibration - //SERIAL_ECHOLN(" Soft endstops disabled "); - if(Stopped == false) { - //get_coordinates(); // For X Y Z E F - delta[X_AXIS] = 45; - delta[Y_AXIS] = 135; - calculate_SCARA_forward_Transform(delta); - destination[X_AXIS] = delta[X_AXIS]/axis_scaling[X_AXIS]; - destination[Y_AXIS] = delta[Y_AXIS]/axis_scaling[Y_AXIS]; - - prepare_move(); - //ClearToSend(); - return; - } - break; - case 365: // M364 Set SCARA scaling for X Y Z - for(int8_t i=0; i < 3; i++) - { - if(code_seen(axis_codes[i])) - { - - axis_scaling[i] = code_value(); - - } - } - break; - #endif case 400: // M400 finish all moves { st_synchronize(); } break; -#if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS) && not defined(Z_PROBE_SLED) - case 401: - { - engage_z_probe(); // Engage Z Servo endstop if available - } - break; - - case 402: - { - retract_z_probe(); // Retract Z Servo endstop if enabled - } - break; -#endif #ifdef FILAMENT_SENSOR case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or display nominal filament width @@ -4654,7 +4343,7 @@ case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp FlushSerialRequestResend(); break; } - } + } // end if(code_seen('M')) (end of M codes) else if(code_seen('T')) { @@ -4739,16 +4428,7 @@ case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp // Set the new active extruder and position active_extruder = tmp_extruder; #endif //else DUAL_X_CARRIAGE -#ifdef DELTA - - calculate_delta(current_position); // change cartesian kinematic to delta kinematic; - //sent position to plan_set_position(); - plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],current_position[E_AXIS]); - -#else plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - -#endif // Move to the old position if 'F' was in the parameters if(make_move && Stopped == false) { prepare_move(); @@ -4759,13 +4439,13 @@ case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp SERIAL_ECHO(MSG_ACTIVE_EXTRUDER); SERIAL_PROTOCOLLN((int)active_extruder); } - } + } // end if(code_seen('T')) (end of T codes) else { SERIAL_ECHO_START; SERIAL_ECHORPGM(MSG_UNKNOWN_COMMAND); - SERIAL_ECHO(cmdbuffer[bufindr]); + SERIAL_ECHO(CMDBUFFER_CURRENT_STRING); SERIAL_ECHOLNPGM("\""); } @@ -4781,14 +4461,13 @@ void FlushSerialRequestResend() ClearToSend(); } +// Confirm the execution of a command, if sent from a serial line. +// Execution of a command from a SD card will not be confirmed. void ClearToSend() { - previous_millis_cmd = millis(); - #ifdef SDSUPPORT - if(fromsd[bufindr]) - return; - #endif //SDSUPPORT - SERIAL_PROTOCOLLNRPGM(MSG_OK); + previous_millis_cmd = millis(); + if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_USB) + SERIAL_PROTOCOLLNRPGM(MSG_OK); } void get_coordinates() @@ -4855,45 +4534,6 @@ void clamp_to_software_endstops(float target[3]) } } -#ifdef DELTA -void recalc_delta_settings(float radius, float diagonal_rod) -{ - delta_tower1_x= -SIN_60*radius; // front left tower - delta_tower1_y= -COS_60*radius; - delta_tower2_x= SIN_60*radius; // front right tower - delta_tower2_y= -COS_60*radius; - delta_tower3_x= 0.0; // back middle tower - delta_tower3_y= radius; - delta_diagonal_rod_2= sq(diagonal_rod); -} - -void calculate_delta(float cartesian[3]) -{ - delta[X_AXIS] = sqrt(delta_diagonal_rod_2 - - sq(delta_tower1_x-cartesian[X_AXIS]) - - sq(delta_tower1_y-cartesian[Y_AXIS]) - ) + cartesian[Z_AXIS]; - delta[Y_AXIS] = sqrt(delta_diagonal_rod_2 - - sq(delta_tower2_x-cartesian[X_AXIS]) - - sq(delta_tower2_y-cartesian[Y_AXIS]) - ) + cartesian[Z_AXIS]; - delta[Z_AXIS] = sqrt(delta_diagonal_rod_2 - - sq(delta_tower3_x-cartesian[X_AXIS]) - - sq(delta_tower3_y-cartesian[Y_AXIS]) - ) + cartesian[Z_AXIS]; - /* - SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]); - - SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]); - */ -} -#endif - - #ifdef MESH_BED_LEVELING void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder) { float dx = x - current_position[X_AXIS]; @@ -4929,73 +4569,7 @@ void prepare_move() { clamp_to_software_endstops(destination); previous_millis_cmd = millis(); - - #ifdef SCARA //for now same as delta-code - -float difference[NUM_AXIS]; -for (int8_t i=0; i < NUM_AXIS; i++) { - difference[i] = destination[i] - current_position[i]; -} - -float cartesian_mm = sqrt( sq(difference[X_AXIS]) + - sq(difference[Y_AXIS]) + - sq(difference[Z_AXIS])); -if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); } -if (cartesian_mm < 0.000001) { return; } -float seconds = 6000 * cartesian_mm / feedrate / feedmultiply; -int steps = max(1, int(scara_segments_per_second * seconds)); - //SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm); - //SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds); - //SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps); -for (int s = 1; s <= steps; s++) { - float fraction = float(s) / float(steps); - for(int8_t i=0; i < NUM_AXIS; i++) { - destination[i] = current_position[i] + difference[i] * fraction; - } - - - calculate_delta(destination); - //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]); - //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]); - //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]); - //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]); - //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); - //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]); - - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], - destination[E_AXIS], feedrate*feedmultiply/60/100.0, - active_extruder); -} -#endif // SCARA - -#ifdef DELTA - float difference[NUM_AXIS]; - for (int8_t i=0; i < NUM_AXIS; i++) { - difference[i] = destination[i] - current_position[i]; - } - float cartesian_mm = sqrt(sq(difference[X_AXIS]) + - sq(difference[Y_AXIS]) + - sq(difference[Z_AXIS])); - if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); } - if (cartesian_mm < 0.000001) { return; } - float seconds = 6000 * cartesian_mm / feedrate / feedmultiply; - int steps = max(1, int(delta_segments_per_second * seconds)); - // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm); - // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds); - // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps); - for (int s = 1; s <= steps; s++) { - float fraction = float(s) / float(steps); - for(int8_t i=0; i < NUM_AXIS; i++) { - destination[i] = current_position[i] + difference[i] * fraction; - } - calculate_delta(destination); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], - destination[E_AXIS], feedrate*feedmultiply/60/100.0, - active_extruder); - } - -#endif // DELTA - + #ifdef DUAL_X_CARRIAGE if (active_extruder_parked) { @@ -5037,7 +4611,6 @@ for (int s = 1; s <= steps; s++) { } #endif //DUAL_X_CARRIAGE -#if ! (defined DELTA || defined SCARA) // Do not use feedmultiply for E or Z only moves if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) { #ifdef MESH_BED_LEVELING @@ -5053,7 +4626,6 @@ for (int s = 1; s <= steps; s++) { plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder); #endif } -#endif // !(DELTA || SCARA) for(int8_t i=0; i < NUM_AXIS; i++) { current_position[i] = destination[i]; @@ -5122,84 +4694,6 @@ void controllerFan() } #endif -#ifdef SCARA -void calculate_SCARA_forward_Transform(float f_scara[3]) -{ - // Perform forward kinematics, and place results in delta[3] - // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014 - - float x_sin, x_cos, y_sin, y_cos; - - //SERIAL_ECHOPGM("f_delta x="); SERIAL_ECHO(f_scara[X_AXIS]); - //SERIAL_ECHOPGM(" y="); SERIAL_ECHO(f_scara[Y_AXIS]); - - x_sin = sin(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1; - x_cos = cos(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1; - y_sin = sin(f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2; - y_cos = cos(f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2; - - // SERIAL_ECHOPGM(" x_sin="); SERIAL_ECHO(x_sin); - // SERIAL_ECHOPGM(" x_cos="); SERIAL_ECHO(x_cos); - // SERIAL_ECHOPGM(" y_sin="); SERIAL_ECHO(y_sin); - // SERIAL_ECHOPGM(" y_cos="); SERIAL_ECHOLN(y_cos); - - delta[X_AXIS] = x_cos + y_cos + SCARA_offset_x; //theta - delta[Y_AXIS] = x_sin + y_sin + SCARA_offset_y; //theta+phi - - //SERIAL_ECHOPGM(" delta[X_AXIS]="); SERIAL_ECHO(delta[X_AXIS]); - //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]); -} - -void calculate_delta(float cartesian[3]){ - //reverse kinematics. - // Perform reversed kinematics, and place results in delta[3] - // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014 - - float SCARA_pos[2]; - static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; - - SCARA_pos[X_AXIS] = cartesian[X_AXIS] * axis_scaling[X_AXIS] - SCARA_offset_x; //Translate SCARA to standard X Y - SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset_y; // With scaling factor. - - #if (Linkage_1 == Linkage_2) - SCARA_C2 = ( ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) ) / (2 * (float)L1_2) ) - 1; - #else - SCARA_C2 = ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2 ) / 45000; - #endif - - SCARA_S2 = sqrt( 1 - sq(SCARA_C2) ); - - SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2; - SCARA_K2 = Linkage_2 * SCARA_S2; - - SCARA_theta = ( atan2(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2(SCARA_K1, SCARA_K2) ) * -1; - SCARA_psi = atan2(SCARA_S2,SCARA_C2); - - delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - theta is support arm angle - delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG; // - equal to sub arm angle (inverted motor) - delta[Z_AXIS] = cartesian[Z_AXIS]; - - /* - SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]); - - SERIAL_ECHOPGM("scara x="); SERIAL_ECHO(SCARA_pos[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHOLN(SCARA_pos[Y_AXIS]); - - SERIAL_ECHOPGM("delta x="); SERIAL_ECHO(delta[X_AXIS]); - SERIAL_ECHOPGM(" y="); SERIAL_ECHO(delta[Y_AXIS]); - SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(delta[Z_AXIS]); - - SERIAL_ECHOPGM("C2="); SERIAL_ECHO(SCARA_C2); - SERIAL_ECHOPGM(" S2="); SERIAL_ECHO(SCARA_S2); - SERIAL_ECHOPGM(" Theta="); SERIAL_ECHO(SCARA_theta); - SERIAL_ECHOPGM(" Psi="); SERIAL_ECHOLN(SCARA_psi); - SERIAL_ECHOLN(" ");*/ -} - -#endif - #ifdef TEMP_STAT_LEDS static bool blue_led = false; static bool red_led = false; @@ -5240,12 +4734,6 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s static int killCount = 0; // make the inactivity button a bit less responsive const int KILL_DELAY = 10000; #endif - -#if defined(HOME_PIN) && HOME_PIN > -1 - static int homeDebounceCount = 0; // poor man's debouncing count - const int HOME_DEBOUNCE_DELAY = 10000; -#endif - if(buflen < (BUFSIZE-1)) get_command(); @@ -5296,28 +4784,6 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s kill(); } #endif - -#if defined(HOME_PIN) && HOME_PIN > -1 - // Check to see if we have to home, use poor man's debouncer - // --------------------------------------------------------- - if ( 0 == READ(HOME_PIN) ) - { - if (homeDebounceCount == 0) - { - enquecommand_P((PSTR("G28"))); - homeDebounceCount++; - LCD_ALERTMESSAGERPGM(MSG_AUTO_HOME); - } - else if (homeDebounceCount < HOME_DEBOUNCE_DELAY) - { - homeDebounceCount++; - } - else - { - homeDebounceCount = 0; - } - } -#endif #if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1 controllerFan(); //Check if fan should be turned on to cool stepper drivers down diff --git a/Firmware/cardreader.cpp b/Firmware/cardreader.cpp index f0a9e9d66..f64f47022 100644 --- a/Firmware/cardreader.cpp +++ b/Firmware/cardreader.cpp @@ -539,9 +539,10 @@ void CardReader::checkautostart(bool force) if(strncmp((char*)p.name,autoname,5)==0) { char cmd[30]; - + // M23: Select SD file sprintf_P(cmd, PSTR("M23 %s"), autoname); enquecommand(cmd); + // M24: Start/resume SD print enquecommand_P(PSTR("M24")); found=true; } diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index a22776da1..840d8dbd5 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -9,6 +9,420 @@ extern float home_retract_mm_ext(int axis);
+float world2machine_rotation_and_skew[2][2];
+float world2machine_shift[2];
+
+#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
+#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
+
+// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
+// The points are ordered in a zig-zag fashion to speed up the calibration.
+const float bed_ref_points[] PROGMEM = {
+ 13.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
+ 115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
+ 216.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
+
+ 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+ 115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+ 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+
+ 13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+ 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+ 216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
+};
+
+// Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
+// The points are the following: center front, center right, center rear, center left.
+const float bed_ref_points_4[] PROGMEM = {
+ 115.f - BED_ZERO_REF_X, 6.4f - BED_ZERO_REF_Y,
+ 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
+ 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
+ 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
+};
+
+static inline float sqr(float x) { return x * x; }
+
+bool calculate_machine_skew_and_offset_LS(
+ // Matrix of maximum 9 2D points (18 floats)
+ const float *measured_pts,
+ uint8_t npts,
+ const float *true_pts,
+ // Resulting correction matrix.
+ float *vec_x,
+ float *vec_y,
+ float *cntr
+ // Temporary values, 49-18-(2*3)=25 floats
+// , float *temp
+ )
+{
+ SERIAL_ECHOPGM("X vector, initial: ");
+ MYSERIAL.print(vec_x[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_x[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("Y vector, initial: ");
+ MYSERIAL.print(vec_y[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_y[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("center, initial: ");
+ MYSERIAL.print(cntr[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(cntr[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ for (uint8_t i = 0; i < npts; ++ i) {
+ SERIAL_ECHOPGM("point #");
+ MYSERIAL.print(int(i));
+ SERIAL_ECHOPGM(" measured: (");
+ MYSERIAL.print(measured_pts[i*2], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(measured_pts[i*2+1], 5);
+ SERIAL_ECHOPGM("); target: (");
+ MYSERIAL.print(pgm_read_float(true_pts+i*2 ), 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(pgm_read_float(true_pts+i*2+1), 5);
+ SERIAL_ECHOPGM("), error: ");
+ MYSERIAL.print(sqrt(
+ sqr(pgm_read_float(true_pts+i*2 ) - measured_pts[i*2 ]) +
+ sqr(pgm_read_float(true_pts+i*2+1) - measured_pts[i*2+1])), 5);
+ SERIAL_ECHOLNPGM("");
+ }
+ delay_keep_alive(100);
+
+
+ {
+ // Create covariance matrix for A, collect the right hand side b.
+ float A[3][3] = { 0.f };
+ float b[3] = { 0.f };
+ float acc;
+ for (uint8_t r = 0; r < 3; ++ r) {
+ for (uint8_t c = 0; c < 3; ++ c) {
+ acc = 0;
+ for (uint8_t i = 0; i < npts; ++ i) {
+ float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
+ float b = (c == 2) ? 1.f : measured_pts[2 * i + c];
+ acc += a * b;
+ }
+ A[r][c] = acc;
+ }
+ acc = 0.f;
+ for (uint8_t i = 0; i < npts; ++ i) {
+ float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
+ float b = pgm_read_float(true_pts+i*2);
+ acc += a * b;
+ }
+ b[r] = acc;
+ }
+ // Solve the linear equation for ax, bx, cx.
+ float x[3] = { 0.f };
+ for (uint8_t iter = 0; iter < 100; ++ iter) {
+ x[0] = (b[0] - A[0][1] * x[1] - A[0][2] * x[2]) / A[0][0];
+ x[1] = (b[1] - A[1][0] * x[0] - A[1][2] * x[2]) / A[1][1];
+ x[2] = (b[2] - A[2][0] * x[0] - A[2][1] * x[1]) / A[2][2];
+ }
+ // Store the result to the output variables.
+ vec_x[0] = x[0];
+ vec_y[0] = x[1];
+ cntr[0] = x[2];
+
+ // Recalculate A and b for the y values.
+ // Note the weighting of the first row of values.
+// const float weight_1st_row = 0.5f;
+ const float weight_1st_row = 0.2f;
+ for (uint8_t r = 0; r < 3; ++ r) {
+ for (uint8_t c = 0; c < 3; ++ c) {
+ acc = 0;
+ for (uint8_t i = 0; i < npts; ++ i) {
+ float w = (i < 3) ? weight_1st_row : 1.f;
+ float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
+ float b = (c == 2) ? 1.f : measured_pts[2 * i + c];
+ acc += a * b * w;
+ }
+ A[r][c] = acc;
+ }
+ acc = 0.f;
+ for (uint8_t i = 0; i < npts; ++ i) {
+ float w = (i < 3) ? weight_1st_row : 1.f;
+ float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
+ float b = pgm_read_float(true_pts+i*2+1);
+ acc += w * a * b;
+ }
+ b[r] = acc;
+ }
+ // Solve the linear equation for ay, by, cy.
+ x[0] = 0.f, x[1] = 0.f; x[2] = 0.f;
+ for (uint8_t iter = 0; iter < 100; ++ iter) {
+ x[0] = (b[0] - A[0][1] * x[1] - A[0][2] * x[2]) / A[0][0];
+ x[1] = (b[1] - A[1][0] * x[0] - A[1][2] * x[2]) / A[1][1];
+ x[2] = (b[2] - A[2][0] * x[0] - A[2][1] * x[1]) / A[2][2];
+ }
+ // Store the result to the output variables.
+ vec_x[1] = x[0];
+ vec_y[1] = x[1];
+ cntr[1] = x[2];
+ }
+
+ SERIAL_ECHOLNPGM("Error after correction: ");
+ for (uint8_t i = 0; i < npts; ++ i) {
+ float x = vec_x[0] * measured_pts[i*2] + vec_y[0] * measured_pts[i*2+1] + cntr[0];
+ float y = vec_x[1] * measured_pts[i*2] + vec_y[1] * measured_pts[i*2+1] + cntr[1];
+ SERIAL_ECHOPGM("point #");
+ MYSERIAL.print(int(i));
+ SERIAL_ECHOPGM(" measured: (");
+ MYSERIAL.print(measured_pts[i*2], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(measured_pts[i*2+1], 5);
+ SERIAL_ECHOPGM("); corrected: (");
+ MYSERIAL.print(x, 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(y, 5);
+ SERIAL_ECHOPGM("); target: (");
+ MYSERIAL.print(pgm_read_float(true_pts+i*2 ), 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(pgm_read_float(true_pts+i*2+1), 5);
+ SERIAL_ECHOPGM("), error: ");
+ MYSERIAL.print(sqrt(sqr(pgm_read_float(true_pts+i*2)-x)+sqr(pgm_read_float(true_pts+i*2+1)-y)));
+ SERIAL_ECHOLNPGM("");
+ }
+
+ SERIAL_ECHOPGM("X vector new, inverted: ");
+ MYSERIAL.print(vec_x[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_x[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("Y vector new, inverted: ");
+ MYSERIAL.print(vec_y[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_y[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("center new, inverted: ");
+ MYSERIAL.print(cntr[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(cntr[1], 5);
+ SERIAL_ECHOLNPGM("");
+ delay_keep_alive(100);
+
+#if 0
+ // Normalize the vectors. We expect, that the machine axes may be skewed a bit, but the distances are correct.
+ // l shall be very close to 1 already.
+ float l = sqrt(vec_x[0]*vec_x[0] + vec_x[1] * vec_x[1]);
+ vec_x[0] /= l;
+ vec_x[1] /= l;
+ SERIAL_ECHOPGM("Length of the X vector: ");
+ MYSERIAL.print(l, 5);
+ SERIAL_ECHOLNPGM("");
+ l = sqrt(vec_y[0]*vec_y[0] + vec_y[1] * vec_y[1]);
+ vec_y[0] /= l;
+ vec_y[1] /= l;
+ SERIAL_ECHOPGM("Length of the Y vector: ");
+ MYSERIAL.print(l, 5);
+ SERIAL_ECHOLNPGM("");
+
+ // Recalculate the center using the adjusted vec_x/vec_y
+ {
+ cntr[0] = 0.f;
+ cntr[1] = 0.f;
+ for (uint8_t i = 0; i < npts; ++ i) {
+ cntr[0] += measured_pts[2 * i ] - pgm_read_float(true_pts+i*2) * vec_x[0] - pgm_read_float(true_pts+i*2+1) * vec_y[0];
+ cntr[1] += measured_pts[2 * i + 1] - pgm_read_float(true_pts+i*2) * vec_x[1] - pgm_read_float(true_pts+i*2+1) * vec_y[1];
+ }
+ cntr[0] /= float(npts);
+ cntr[1] /= float(npts);
+ }
+
+ SERIAL_ECHOPGM("X vector new, inverted, normalized: ");
+ MYSERIAL.print(vec_x[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_x[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("Y vector new, inverted, normalized: ");
+ MYSERIAL.print(vec_y[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_y[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("center new, inverted, normalized: ");
+ MYSERIAL.print(cntr[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(cntr[1], 5);
+ SERIAL_ECHOLNPGM("");
+#endif
+
+ // Invert the transformation matrix made of vec_x, vec_y and cntr.
+ {
+ float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
+ float Ainv[2][2] = {
+ { vec_y[1] / d, - vec_y[0] / d },
+ { - vec_x[1] / d, vec_x[0] / d }
+ };
+ float cntrInv[2] = {
+ - Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
+ - Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
+ };
+ vec_x[0] = Ainv[0][0];
+ vec_x[1] = Ainv[1][0];
+ vec_y[0] = Ainv[0][1];
+ vec_y[1] = Ainv[1][1];
+ cntr[0] = cntrInv[0];
+ cntr[1] = cntrInv[1];
+ }
+
+ SERIAL_ECHOPGM("X vector, adjusted: ");
+ MYSERIAL.print(vec_x[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_x[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("Y vector, adjusted: ");
+ MYSERIAL.print(vec_y[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(vec_y[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOPGM("center, adjusted: ");
+ MYSERIAL.print(cntr[0], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(cntr[1], 5);
+ SERIAL_ECHOLNPGM("");
+
+ SERIAL_ECHOLNPGM("Difference after correction: ");
+ for (uint8_t i = 0; i < npts; ++ i) {
+ float x = vec_x[0] * pgm_read_float(true_pts+i*2) + vec_y[0] * pgm_read_float(true_pts+i*2+1) + cntr[0];
+ float y = vec_x[1] * pgm_read_float(true_pts+i*2) + vec_y[1] * pgm_read_float(true_pts+i*2+1) + cntr[1];
+ SERIAL_ECHOPGM("point #");
+ MYSERIAL.print(int(i));
+ SERIAL_ECHOPGM("measured: (");
+ MYSERIAL.print(measured_pts[i*2], 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(measured_pts[i*2+1], 5);
+ SERIAL_ECHOPGM("); measured-corrected: (");
+ MYSERIAL.print(x, 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(y, 5);
+ SERIAL_ECHOPGM("); target: (");
+ MYSERIAL.print(pgm_read_float(true_pts+i*2 ), 5);
+ SERIAL_ECHOPGM(", ");
+ MYSERIAL.print(pgm_read_float(true_pts+i*2+1), 5);
+ SERIAL_ECHOPGM("), error: ");
+ MYSERIAL.print(sqrt(sqr(measured_pts[i*2]-x)+sqr(measured_pts[i*2+1]-y)));
+ SERIAL_ECHOLNPGM("");
+ }
+ delay_keep_alive(100);
+
+ return true;
+}
+
+void reset_bed_offset_and_skew()
+{
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
+ eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
+}
+
+void world2machine_reset()
+{
+ // Identity transformation.
+ world2machine_rotation_and_skew[0][0] = 1.f;
+ world2machine_rotation_and_skew[0][1] = 0.f;
+ world2machine_rotation_and_skew[1][0] = 0.f;
+ world2machine_rotation_and_skew[1][1] = 1.f;
+ // Zero shift.
+ world2machine_shift[0] = 0.f;
+ world2machine_shift[1] = 0.f;
+}
+
+static inline bool vec_undef(const float v[2])
+{
+ const uint32_t *vx = (const uint32_t*)v;
+ return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
+}
+
+void world2machine_initialize()
+{
+ float cntr[2] = {
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
+ };
+ float vec_x[2] = {
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
+ };
+ float vec_y[2] = {
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
+ eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
+ };
+
+ bool reset = false;
+ if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y))
+ reset = true;
+ else {
+ // Length of the vec_x shall be close to unity.
+ float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
+ if (l < 0.9 || l > 1.1) {
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
+ reset = true;
+ }
+ // Length of the vec_y shall be close to unity.
+ l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
+ if (l < 0.9 || l > 1.1) {
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
+ reset = true;
+ }
+ // Correction of the zero point shall be reasonably small.
+ l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
+ if (l > 15.f) {
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
+ reset = true;
+ }
+ // vec_x and vec_y shall be nearly perpendicular.
+ l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
+ if (fabs(l) > 0.1f) {
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
+ reset = true;
+ }
+ }
+
+ if (reset) {
+ SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
+ reset_bed_offset_and_skew();
+ world2machine_reset();
+ } else {
+ world2machine_rotation_and_skew[0][0] = vec_x[0];
+ world2machine_rotation_and_skew[1][0] = vec_x[1];
+ world2machine_rotation_and_skew[0][1] = vec_y[0];
+ world2machine_rotation_and_skew[1][1] = vec_y[1];
+ world2machine_shift[0] = cntr[0];
+ world2machine_shift[1] = cntr[1];
+ }
+}
+
+// When switching from absolute to corrected coordinates,
+// this will get the absolute coordinates from the servos,
+// applies the inverse world2machine transformation
+// and stores the result into current_position[x,y].
+void world2machine_update_current()
+{
+ // Invert the transformation matrix made of vec_x, vec_y and cntr.
+ float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
+ float Ainv[2][2] = {
+ { world2machine_rotation_and_skew[1][1] / d, - world2machine_rotation_and_skew[0][1] / d },
+ { - world2machine_rotation_and_skew[1][0] / d, world2machine_rotation_and_skew[0][0] / d }
+ };
+ float x = current_position[X_AXIS] - world2machine_shift[0];
+ float y = current_position[Y_AXIS] - world2machine_shift[1];
+ current_position[X_AXIS] = Ainv[0][0] * x + Ainv[0][1] * y;
+ current_position[Y_AXIS] = Ainv[1][0] * x + Ainv[1][1] * y;
+}
+
static inline void go_xyz(float x, float y, float z, float fr)
{
plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
@@ -35,6 +449,12 @@ static inline void update_current_position_xyz() plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
+static inline void update_current_position_z()
+{
+ current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+ plan_set_z_position(current_position[Z_AXIS]);
+}
+
// At the current position, find the Z stop.
inline void find_bed_induction_sensor_point_z()
{
@@ -45,7 +465,7 @@ inline void find_bed_induction_sensor_point_z() current_position[Z_AXIS] = -10;
go_to_current(homing_feedrate[Z_AXIS]/60);
// we have to let the planner know where we are right now as it is not where we said to go.
- update_current_position_xyz();
+ update_current_position_z();
// move up the retract distance
current_position[Z_AXIS] += home_retract_mm_ext(Z_AXIS);
@@ -55,7 +475,7 @@ inline void find_bed_induction_sensor_point_z() current_position[Z_AXIS] -= home_retract_mm_ext(Z_AXIS) * 2;
go_to_current(homing_feedrate[Z_AXIS]/(4*60));
// we have to let the planner know where we are right now as it is not where we said to go.
- update_current_position_xyz();
+ update_current_position_z();
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
@@ -317,6 +737,7 @@ inline bool improve_bed_induction_sensor_point() enable_endstops(true);
go_xy(center_old_x, center_old_y, feedrate);
update_current_position_xyz();
+// if (! endstop_z_hit_on_purpose()) return false;
center_x += current_position[X_AXIS];
center_y += current_position[Y_AXIS];
}
@@ -360,16 +781,20 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y) enable_z_endstop(true);
go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
- if (! endstop_z_hit_on_purpose())
- return false;
+ if (! endstop_z_hit_on_purpose()) {
+ current_position[X_AXIS] = center_old_x;
+ goto canceled;
+ }
a = current_position[X_AXIS];
enable_z_endstop(false);
go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
enable_z_endstop(true);
go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
- if (! endstop_z_hit_on_purpose())
- return false;
+ if (! endstop_z_hit_on_purpose()) {
+ current_position[X_AXIS] = center_old_x;
+ goto canceled;
+ }
b = current_position[X_AXIS];
// Go to the center.
@@ -392,20 +817,23 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y) if (lift_z_on_min_y) {
// The first row of points are very close to the end stop.
// Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis.
- go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+5.f, homing_feedrate[Z_AXIS] / 60.f);
+ go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
// and go back.
go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
}
if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
// Already triggering before we started the move.
// Shift the trigger point slightly outwards.
- a = current_position[Y_AXIS] - 1.5f;
+ // a = current_position[Y_AXIS] - 1.5f;
+ a = current_position[Y_AXIS];
} else {
enable_z_endstop(true);
go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
- if (! endstop_z_hit_on_purpose())
- return false;
+ if (! endstop_z_hit_on_purpose()) {
+ current_position[Y_AXIS] = center_old_y;
+ goto canceled;
+ }
a = current_position[Y_AXIS];
}
enable_z_endstop(false);
@@ -413,8 +841,10 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y) enable_z_endstop(true);
go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
update_current_position_xyz();
- if (! endstop_z_hit_on_purpose())
- return false;
+ if (! endstop_z_hit_on_purpose()) {
+ current_position[Y_AXIS] = center_old_y;
+ goto canceled;
+ }
b = current_position[Y_AXIS];
// Go to the center.
@@ -424,6 +854,12 @@ inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y) }
return true;
+
+canceled:
+ // Go back to the center.
+ enable_z_endstop(false);
+ go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+ return false;
}
#define MESH_BED_CALIBRATION_SHOW_LCD
@@ -433,11 +869,14 @@ bool find_bed_offset_and_skew() // Reusing the z_values memory for the measurement cache.
// 7x7=49 floats, good for 16 (x,y,z) vectors.
float *pts = &mbl.z_values[0][0];
- float *vec_x = pts + 3 * 4;
- float *vec_y = vec_x + 3;
- float *cntr = vec_y + 3;
+ float *vec_x = pts + 2 * 4;
+ float *vec_y = vec_x + 2;
+ float *cntr = vec_y + 2;
memset(pts, 0, sizeof(float) * 7 * 7);
+ // Let the planner use the uncorrected coordinates.
+ world2machine_reset();
+
#ifdef MESH_BED_CALIBRATION_SHOW_LCD
lcd_implementation_clear();
lcd_print_at_PGM(0, 0, MSG_FIND_BED_OFFSET_AND_SKEW_LINE1);
@@ -451,33 +890,25 @@ bool find_bed_offset_and_skew() lcd_implementation_print_at(0, 2, k+1);
lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE3);
#endif /* MESH_BED_CALIBRATION_SHOW_LCD */
-
- int i, j;
- switch (k) {
- case 0: i = 1; j = 0; break;
- case 1: i = 2; j = 1; break;
- case 2: i = 1; j = 2; break;
- case 3: i = 0; j = 1; break;
- }
- float *pt = pts + k * 3;
+ float *pt = pts + k * 2;
// Go up to z_initial.
go_to_current(homing_feedrate[Z_AXIS] / 60.f);
// Go to the measurement point position.
- mbl.get_meas_xy(i, j, current_position[X_AXIS], current_position[Y_AXIS], true); // use default, uncorrected coordinates
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points_4+k*2);
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4+k*2+1);
go_to_current(homing_feedrate[X_AXIS] / 60.f);
if (! find_bed_induction_sensor_point_xy())
return false;
find_bed_induction_sensor_point_z();
pt[0] = current_position[X_AXIS];
pt[1] = current_position[Y_AXIS];
- pt[2] = current_position[Z_AXIS];
// Start searching for the other points at 3mm above the last point.
current_position[Z_AXIS] += 3.f;
cntr[0] += pt[0];
cntr[1] += pt[1];
- cntr[2] += pt[2];
}
+#if 0
// Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
{
float len;
@@ -486,8 +917,8 @@ bool find_bed_offset_and_skew() cntr[1] *= 1.f/4.f;
cntr[2] *= 1.f/4.f;
// Average the X vector.
- vec_x[0] = (pts[3 * 1 + 0] - pts[3 * 3 + 0]) / 2.f;
- vec_x[1] = (pts[3 * 1 + 1] - pts[3 * 3 + 1]) / 2.f;
+ vec_x[0] = (pts[2 * 1 + 0] - pts[2 * 3 + 0]) / 2.f;
+ vec_x[1] = (pts[2 * 1 + 1] - pts[2 * 3 + 1]) / 2.f;
len = sqrt(vec_x[0]*vec_x[0] + vec_x[1]*vec_x[1]);
if (0) {
// if (len < MEAS_NUM_X_DIST) {
@@ -500,8 +931,8 @@ bool find_bed_offset_and_skew() // Verify the maximum skew?
}
// Average the Y vector.
- vec_y[0] = (pts[3 * 2 + 0] - pts[3 * 0 + 0]) / 2.f;
- vec_y[1] = (pts[3 * 2 + 1] - pts[3 * 0 + 1]) / 2.f;
+ vec_y[0] = (pts[2 * 2 + 0] - pts[2 * 0 + 0]) / 2.f;
+ vec_y[1] = (pts[2 * 2 + 1] - pts[2 * 0 + 1]) / 2.f;
len = sqrt(vec_y[0]*vec_y[0] + vec_y[1]*vec_y[1]);
if (0) {
// if (len < MEAS_NUM_Y_DIST) {
@@ -539,6 +970,25 @@ bool find_bed_offset_and_skew() SERIAL_ECHOLN("");
#endif
}
+#endif
+
+ calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr);
+ world2machine_rotation_and_skew[0][0] = vec_x[0];
+ world2machine_rotation_and_skew[1][0] = vec_x[1];
+ world2machine_rotation_and_skew[0][1] = vec_y[0];
+ world2machine_rotation_and_skew[1][1] = vec_y[1];
+ world2machine_shift[0] = cntr[0];
+ world2machine_shift[1] = cntr[1];
+#if 1
+ // Fearlessly store the calibration values into the eeprom.
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
+#endif
+
return true;
}
@@ -552,6 +1002,16 @@ bool improve_bed_offset_and_skew(int8_t method) float *cntr = vec_y + 2;
memset(pts, 0, sizeof(float) * 7 * 7);
+ // Cache the current correction matrix.
+ vec_x[0] = world2machine_rotation_and_skew[0][0];
+ vec_x[1] = world2machine_rotation_and_skew[1][0];
+ vec_y[0] = world2machine_rotation_and_skew[0][1];
+ vec_y[1] = world2machine_rotation_and_skew[1][1];
+ cntr[0] = world2machine_shift[0];
+ cntr[1] = world2machine_shift[1];
+ // and reset the correction matrix, so the planner will not do anything.
+ world2machine_reset();
+
bool endstops_enabled = enable_endstops(false);
bool endstop_z_enabled = enable_z_endstop(false);
@@ -562,9 +1022,6 @@ bool improve_bed_offset_and_skew(int8_t method) // Collect a matrix of 9x9 points.
for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
- int ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
- int iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
- if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
// Print the decrasing ID of the measurement point.
#ifdef MESH_BED_CALIBRATION_SHOW_LCD
lcd_print_at_PGM(0, 1, MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
@@ -579,7 +1036,11 @@ bool improve_bed_offset_and_skew(int8_t method) go_to_current(homing_feedrate[Z_AXIS]/60);
// Go to the measurement point.
// Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
- mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
+ current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
+ current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
+ // The calibration points are very close to the min Y.
+ if (current_position[Y_AXIS] < Y_MIN_POS)
+ current_position[Y_AXIS] = Y_MIN_POS;
go_to_current(homing_feedrate[X_AXIS]/60);
// Find its Z position by running the normal vertical search.
// delay_keep_alive(3000);
@@ -587,34 +1048,53 @@ bool improve_bed_offset_and_skew(int8_t method) // delay_keep_alive(3000);
// Improve the point position by searching its center in a current plane.
int8_t n_errors = 3;
- for (int8_t iter = 0; iter < 4; ++ iter) {
+ for (int8_t iter = 0; iter < 8; ) {
bool found = false;
switch (method) {
case 0: found = improve_bed_induction_sensor_point(); break;
- case 1: found = improve_bed_induction_sensor_point2(iy == 0); break;
+ case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3); break;
default: break;
}
- if (! found) {
- if (n_errors -- == 0) {
- // Give up.
- goto canceled;
- } else {
- // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
- current_position[Z_AXIS] -= 0.025f;
- enable_endstops(false);
- enable_z_endstop(false);
- go_to_current(homing_feedrate[Z_AXIS]);
+ if (found) {
+ if (iter > 3) {
+ // Average the last 4 measurements.
+ pts[mesh_point*2 ] += current_position[X_AXIS];
+ pts[mesh_point*2+1] += current_position[Y_AXIS];
}
+ ++ iter;
+ } else if (n_errors -- == 0) {
+ // Give up.
+ goto canceled;
+ } else {
+ // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
+ current_position[Z_AXIS] -= 0.025f;
+ enable_endstops(false);
+ enable_z_endstop(false);
+ go_to_current(homing_feedrate[Z_AXIS]);
}
}
-// delay_keep_alive(3000);
- float *pt = pts + 2 * (ix + iy * 3);
- pt[0] = current_position[X_AXIS];
- pt[1] = current_position[Y_AXIS];
- cntr[0] += pt[0];
- cntr[1] += pt[1];
+ delay_keep_alive(3000);
+ }
+
+ // Average the last 4 measurements.
+ for (int8_t i = 0; i < 18; ++ i)
+ pts[i] *= (1.f/4.f);
+
+// Test the positions. Are the positions reproducible?
+#if 1
+ enable_endstops(false);
+ enable_z_endstop(false);
+ for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+ // Go to the measurement point.
+ // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+ current_position[X_AXIS] = pts[mesh_point*2];
+ current_position[Y_AXIS] = pts[mesh_point*2+1];
+ go_to_current(homing_feedrate[X_AXIS]/60);
+ delay_keep_alive(3000);
}
+#endif
+#if 0
// Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
// Average the center point.
cntr[0] *= 1.f/9.f;
@@ -625,7 +1105,6 @@ bool improve_bed_offset_and_skew(int8_t method) // Average the Y vector.
vec_y[0] = (pts[2 * 6 + 0] - pts[2 * 0 + 0] + pts[2 * 7 + 0] - pts[2 * 1 + 0] + pts[2 * 8 + 0] - pts[2 * 2 + 0]) / 6.f;
vec_y[1] = (pts[2 * 6 + 1] - pts[2 * 0 + 1] + pts[2 * 7 + 1] - pts[2 * 1 + 1] + pts[2 * 8 + 1] - pts[2 * 2 + 1]) / 6.f;
-
#if 1
// Fearlessly store the calibration values into the eeprom.
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
@@ -635,6 +1114,39 @@ bool improve_bed_offset_and_skew(int8_t method) eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
#endif
+#else
+ calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr);
+ world2machine_rotation_and_skew[0][0] = vec_x[0];
+ world2machine_rotation_and_skew[1][0] = vec_x[1];
+ world2machine_rotation_and_skew[0][1] = vec_y[0];
+ world2machine_rotation_and_skew[1][1] = vec_y[1];
+ world2machine_shift[0] = cntr[0];
+ world2machine_shift[1] = cntr[1];
+#if 1
+ // Fearlessly store the calibration values into the eeprom.
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
+ eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
+#endif
+#endif
+
+// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
+#if 1
+ enable_endstops(false);
+ enable_z_endstop(false);
+ delay_keep_alive(3000);
+ for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+ // Go to the measurement point.
+ // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+ current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
+ current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
+ go_to_current(homing_feedrate[X_AXIS]/60);
+ delay_keep_alive(3000);
+ }
+#endif
#if 0
// and let us know the result.
@@ -659,105 +1171,9 @@ bool improve_bed_offset_and_skew(int8_t method) return true;
canceled:
+ // Store the identity matrix to EEPROM.
+ reset_bed_offset_and_skew();
enable_endstops(endstops_enabled);
enable_z_endstop(endstop_z_enabled);
return false;
}
-
-void reset_bed_offset_and_skew()
-{
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
- eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
-}
-
-#if 0
-static const float[9][2] PROGMEM bed_points = {
-};
-
-bool calculate_machine_skew_and_offset_LS(
- // Matrix of 9 2D points (18 floats)
- float *pts,
- // Resulting correction matrix.
- float *vec_x,
- float *vec_y,
- float *cntr,
- // Temporary values, 49-18-(2*3)=25 floats
- float *temp
-{
- {
- // Create covariance matrix for A, collect the right hand side b.
- float A[3][3] = { 0.f };
- float b[3] = { 0.f };
- float acc;
- for (uint8_t r = 0; r < 3; ++ r) {
- for (uint8_t c = 0; c < 3; ++ c) {
- acc = 0;
- for (uint8_t i = 0; i < 9; ++ i) {
- float a = (r == 2) ? 1.f : pts[2 * i + r];
- float b = (c == 2) ? 1.f : pts[2 * i + c];
- acc += a * b;
- }
- A[r][c] = acc;
- }
- acc = 0.f;
- for (uint8_t i = 0; i < 9; ++ i) {
- float a = (r == 2) ? 1.f : pts[2 * i + r];
- float b = pgm_read_float(&coeff2[i][0]);
- acc += a * b;
- }
- b[r] = acc;
- }
- // Solve the linear equation for ax, bx, cx.
- float x[3] = { 0.f };
- for (uint8_t iter = 0; iter < 100; ++ iter) {
- x[0] = (b[0] - A[1] * x[1] - A[2] * x[2]) / A[0];
- x[1] = (b[1] - A[0] * x[0] - A[2] * x[2]) / A[1];
- x[2] = (b[2] - A[0] * x[0] - A[1] * x[1]) / A[2];
- }
- // Store the result to the output variables.
- vec_x[0] = x[0];
- vec_y[0] = x[1];
- cntr[0] = x[2];
-
- // Recalculate b for the y values.
- for (uint8_t r = 0; r < 3; ++ r) {
- acc = 0.f;
- for (uint8_t i = 0; i < 9; ++ i) {
- float a = (r == 2) ? 1.f : pts[2 * i + r];
- float b = pgm_read_float(&coeff2[i][1]);
- acc += a * b;
- }
- b[r] = acc;
- }
- // Solve the linear equation for ay, by, cy.
- x[0] = 0.f, x[1] = 0.f; x[2] = 0.f;
- for (uint8_t iter = 0; iter < 100; ++ iter) {
- x[0] = (b[0] - A[1] * x[1] - A[2] * x[2]) / A[0];
- x[1] = (b[1] - A[0] * x[0] - A[2] * x[2]) / A[1];
- x[2] = (b[2] - A[0] * x[0] - A[1] * x[1]) / A[2];
- }
- // Store the result to the output variables.
- vec_x[1] = x[0];
- vec_y[1] = x[1];
- cntr[1] = x[2];
- }
-
- // Normalize the vectors. We expect, that the machine axes may be skewed a bit, but the distances are correct.
- // l shall be very close to 1 already.
- float l = sqrt(vec_x[0]*vec_x[0] + vec_x[1] * vec_x[1]);
- vec_x[0] /= l;
- vec_x[1] /= l;
- l = sqrt(vec_y[0]*vec_y[0] + vec_y[1] * vec_y[1]);
- vec_y[0] /= l;
- vec_y[1] /= l;
-
-
-
- // Invert the transformation matrix made of vec_x, vec_y and cntr.
-
-}
-#endif
\ No newline at end of file diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h index 08dc358d1..2d53dba2c 100644 --- a/Firmware/mesh_bed_calibration.h +++ b/Firmware/mesh_bed_calibration.h @@ -1,6 +1,29 @@ #ifndef MESH_BED_CALIBRATION_H
#define MESH_BED_CALIBRATION_H
+// Exact positions of the print head above the bed reference points, in the world coordinates.
+// The world coordinates match the machine coordinates only in case, when the machine
+// is built properly, the end stops are at the correct positions and the axes are perpendicular.
+extern const float bed_ref_points[] PROGMEM;
+
+// 2x2 transformation matrix from the world coordinates to the machine coordinates.
+// Corrects for the rotation and skew of the machine axes.
+// Used by the planner's plan_buffer_line() and plan_set_position().
+extern float world2machine_rotation_and_skew[2][2];
+// Shift of the machine zero point, in the machine coordinates.
+extern float world2machine_shift[2];
+
+// Resets the transformation to identity.
+extern void world2machine_reset();
+// Loads the transformation from the EEPROM, if available.
+extern void world2machine_initialize();
+
+// When switching from absolute to corrected coordinates,
+// this will apply an inverse world2machine transformation
+// to current_position[x,y].
+extern void world2machine_update_current();
+
+
extern void find_bed_induction_sensor_point_z();
extern bool find_bed_induction_sensor_point_xy();
diff --git a/Firmware/mesh_bed_leveling.cpp b/Firmware/mesh_bed_leveling.cpp index b0717ce3a..506d3746a 100644 --- a/Firmware/mesh_bed_leveling.cpp +++ b/Firmware/mesh_bed_leveling.cpp @@ -1,4 +1,5 @@ #include "mesh_bed_leveling.h" +#include "mesh_bed_calibration.h" #include "Configuration.h" #ifdef MESH_BED_LEVELING @@ -22,6 +23,7 @@ static inline bool vec_undef(const float v[2]) void mesh_bed_leveling::get_meas_xy(int ix, int iy, float &x, float &y, bool use_default) { +#if 0 float cntr[2] = { eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)), eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4)) @@ -83,10 +85,18 @@ void mesh_bed_leveling::get_meas_xy(int ix, int iy, float &x, float &y, bool use SERIAL_ECHOLN(""); #endif } +#else + // Default, uncorrected positions of the calibration points. + // This coordinate will be corrected by the planner. + x = pgm_read_float(bed_ref_points + 2 * (iy * 3 + ix)); + y = pgm_read_float(bed_ref_points + 2 * (iy * 3 + ix) + 1); +#endif } #if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1 // Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS + +// #define MBL_BILINEAR void mesh_bed_leveling::upsample_3x3() { int idx0 = 0; @@ -106,9 +116,16 @@ void mesh_bed_leveling::upsample_3x3() if (i == idx1) continue; float x = get_x(i); - z_values[j][i] = z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) + - z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) + - z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1)); + #ifdef MBL_BILINEAR + z_values[j][i] = (x < x1) ? + ((z_values[j][idx0] * (x - x0) + z_values[j][idx1] * (x1 - x)) / (x1 - x0)) : + ((z_values[j][idx1] * (x - x1) + z_values[j][idx2] * (x2 - x)) / (x2 - x1)); + #else + z_values[j][i] = + z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) + + z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) + + z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1)); + #endif } } } @@ -126,12 +143,36 @@ void mesh_bed_leveling::upsample_3x3() if (j == idx1) continue; float y = get_y(j); - z_values[j][i] = z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) + - z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) + - z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1)); + #ifdef MBL_BILINEAR + z_values[j][i] = (y < y1) ? + ((z_values[idx0][i] * (y - y0) + z_values[idx1][i] * (y1 - y)) / (y1 - y0)) : + ((z_values[idx1][i] * (y - y1) + z_values[idx2][i] * (y2 - y)) / (y2 - y1)); + #else + z_values[j][i] = + z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) + + z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) + + z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1)); + #endif + } + } + } + +/* + // Relax the non-measured points. + const float weight = 0.2f; + for (uint8_t iter = 0; iter < 20; ++ iter) { + for (int8_t j = 1; j < 6; ++ j) { + for (int8_t i = 1; i < 6; ++ i) { + if (i == 3 || j == 3) + continue; + if ((i % 3) == 0 && (j % 3) == 0) + continue; + float avg = 0.25f * (z_values[j][i-1]+z_values[j][i+1]+z_values[j-1][i]+z_values[j+1][i]); + z_values[j][i] = (1.f-weight)*z_values[j][i] + weight*avg; } } } +*/ } #endif diff --git a/Firmware/pins.h b/Firmware/pins.h index be5deacef..8482166c3 100644 --- a/Firmware/pins.h +++ b/Firmware/pins.h @@ -104,11 +104,7 @@ #define KILL_PIN -1 //80 with Smart Controller LCD #define SUICIDE_PIN -1 //PIN that has to be turned on right after start, to keep power flowing. #define SDPOWER -1 - #ifdef BARICUDA - #define HEATER_2_PIN 6 - #else - #define HEATER_2_PIN -1 - #endif + #define HEATER_2_PIN -1 #ifdef MINI_RAMBO #define ELECTRONICS "RAMBo13a" @@ -282,11 +278,7 @@ #define KILL_PIN -1 //80 with Smart Controller LCD #define SUICIDE_PIN -1 //PIN that has to be turned on right after start, to keep power flowing. #define SDPOWER -1 - #ifdef BARICUDA - #define HEATER_2_PIN 6 - #else - #define HEATER_2_PIN -1 - #endif + #define HEATER_2_PIN -1 #define HEATER_0_PIN 3 #define HEATER_BED_PIN 4 diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 3441ee0a1..623959c43 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -60,6 +60,7 @@ #ifdef MESH_BED_LEVELING #include "mesh_bed_leveling.h" +#include "mesh_bed_calibration.h" #endif //=========================================================================== @@ -458,20 +459,12 @@ void check_axes_activity() unsigned char z_active = 0; unsigned char e_active = 0; unsigned char tail_fan_speed = fanSpeed; - #ifdef BARICUDA - unsigned char tail_valve_pressure = ValvePressure; - unsigned char tail_e_to_p_pressure = EtoPPressure; - #endif block_t *block; if(block_buffer_tail != block_buffer_head) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; - #ifdef BARICUDA - tail_valve_pressure = block_buffer[block_index].valve_pressure; - tail_e_to_p_pressure = block_buffer[block_index].e_to_p_pressure; - #endif while(block_index != block_buffer_head) { block = &block_buffer[block_index]; @@ -515,16 +508,6 @@ void check_axes_activity() #ifdef AUTOTEMP getHighESpeed(); #endif - -#ifdef BARICUDA - #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 - analogWrite(HEATER_1_PIN,tail_valve_pressure); - #endif - - #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 - analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); - #endif -#endif } @@ -532,11 +515,7 @@ 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. -#ifdef ENABLE_AUTO_BED_LEVELING void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder) -#else -void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder) -#endif //ENABLE_AUTO_BED_LEVELING { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); @@ -554,6 +533,53 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif // ENABLE_AUTO_BED_LEVELING + // Apply the machine correction matrix. + { + #if 0 + SERIAL_ECHOPGM("Planner, current position - servos: "); + MYSERIAL.print(st_get_position_mm(X_AXIS), 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(st_get_position_mm(Y_AXIS), 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(st_get_position_mm(Z_AXIS), 5); + SERIAL_ECHOLNPGM(""); + + SERIAL_ECHOPGM("Planner, target position, initial: "); + MYSERIAL.print(x, 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(y, 5); + SERIAL_ECHOLNPGM(""); + + SERIAL_ECHOPGM("Planner, world2machine: "); + MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5); + SERIAL_ECHOLNPGM(""); + SERIAL_ECHOPGM("Planner, offset: "); + MYSERIAL.print(world2machine_shift[0], 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(world2machine_shift[1], 5); + SERIAL_ECHOLNPGM(""); + #endif + + float tmpx = x; + float tmpy = y; + x = world2machine_rotation_and_skew[0][0] * tmpx + world2machine_rotation_and_skew[0][1] * tmpy + world2machine_shift[0]; + y = world2machine_rotation_and_skew[1][0] * tmpx + world2machine_rotation_and_skew[1][1] * tmpy + world2machine_shift[1]; + + #if 0 + SERIAL_ECHOPGM("Planner, target position, corrected: "); + MYSERIAL.print(x, 5); + SERIAL_ECHOPGM(", "); + MYSERIAL.print(y, 5); + SERIAL_ECHOLNPGM(""); + #endif + } + // 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 @@ -622,10 +648,6 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi } block->fan_speed = fanSpeed; - #ifdef BARICUDA - block->valve_pressure = ValvePressure; - block->e_to_p_pressure = EtoPPressure; - #endif // Compute direction bits for this block block->direction_bits = 0; @@ -1061,16 +1083,20 @@ vector_3 plan_get_position() { } #endif // ENABLE_AUTO_BED_LEVELING -#ifdef ENABLE_AUTO_BED_LEVELING void plan_set_position(float x, float y, float z, const float &e) { - apply_rotation_xyz(plan_bed_level_matrix, x, y, z); -#else -void plan_set_position(const float &x, const float &y, const float &z, const float &e) -{ +#ifdef ENABLE_AUTO_BED_LEVELING + apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif // ENABLE_AUTO_BED_LEVELING - - + + // Apply the machine correction matrix. + { + float tmpx = x; + float tmpy = y; + x = world2machine_rotation_and_skew[0][0] * tmpx + world2machine_rotation_and_skew[0][1] * tmpy + world2machine_shift[0]; + y = world2machine_rotation_and_skew[1][0] * tmpx + world2machine_rotation_and_skew[1][1] * tmpy + world2machine_shift[1]; + } + position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); #ifdef MESH_BED_LEVELING @@ -1091,6 +1117,13 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo previous_speed[3] = 0.0; } +// Only useful in the bed leveling routine, when the mesh bed leveling is off. +void plan_set_z_position(float z) +{ + position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); + st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]); +} + void plan_set_e_position(const float &e) { position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); @@ -1116,4 +1149,4 @@ void reset_acceleration_rates() { axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i]; } -}
+} diff --git a/Firmware/planner.h b/Firmware/planner.h index b375f9286..ea1dc0825 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -64,10 +64,6 @@ typedef struct { unsigned long final_rate; // The minimal rate at exit unsigned long acceleration_st; // acceleration steps/sec^2 unsigned long fan_speed; - #ifdef BARICUDA - unsigned long valve_pressure; - unsigned long e_to_p_pressure; - #endif volatile char busy; } block_t; @@ -88,16 +84,18 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate // Get the position applying the bed level matrix if enabled vector_3 plan_get_position(); #else -void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder); +void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder); +//void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder); #endif // ENABLE_AUTO_BED_LEVELING // Set position. Used for G92 instructions. -#ifdef ENABLE_AUTO_BED_LEVELING +//#ifdef ENABLE_AUTO_BED_LEVELING void plan_set_position(float x, float y, float z, const float &e); -#else -void plan_set_position(const float &x, const float &y, const float &z, const float &e); -#endif // ENABLE_AUTO_BED_LEVELING +//#else +//void plan_set_position(const float &x, const float &y, const float &z, const float &e); +//#endif // ENABLE_AUTO_BED_LEVELING +void plan_set_z_position(const float z); void plan_set_e_position(const float &e); diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index faf0426fd..d46cd62cc 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -1197,7 +1197,6 @@ void babystep(const uint8_t axis,const bool direction) } break; -#ifndef DELTA case Z_AXIS: { enable_z(); @@ -1229,41 +1228,6 @@ void babystep(const uint8_t axis,const bool direction) } break; -#else //DELTA - case Z_AXIS: - { - enable_x(); - enable_y(); - enable_z(); - uint8_t old_x_dir_pin= READ(X_DIR_PIN); - uint8_t old_y_dir_pin= READ(Y_DIR_PIN); - uint8_t old_z_dir_pin= READ(Z_DIR_PIN); - //setup new step - WRITE(X_DIR_PIN,(INVERT_X_DIR)^direction^BABYSTEP_INVERT_Z); - WRITE(Y_DIR_PIN,(INVERT_Y_DIR)^direction^BABYSTEP_INVERT_Z); - WRITE(Z_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z); - - //perform step - WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); - WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN); - WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN); - - //wait a tiny bit - { - float x=1./float(axis+1); //absolutely useless - } - WRITE(X_STEP_PIN, INVERT_X_STEP_PIN); - WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN); - WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN); - - //get old pin state back. - WRITE(X_DIR_PIN,old_x_dir_pin); - WRITE(Y_DIR_PIN,old_y_dir_pin); - WRITE(Z_DIR_PIN,old_z_dir_pin); - - } - break; -#endif default: break; } diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp index 1953e2a11..ae6319689 100644 --- a/Firmware/ultralcd.cpp +++ b/Firmware/ultralcd.cpp @@ -125,10 +125,8 @@ static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visua /* Different types of actions that can be used in menu items. */ static void menu_action_back(menuFunc_t data); static void menu_action_submenu(menuFunc_t data); -static void menu_action_gcode(const char* pgcode); static void menu_action_function(menuFunc_t data); static void menu_action_setlang(unsigned char lang); -static void menu_action_sdfile(const char* filename, char* longFilename); static void menu_action_sddirectory(const char* filename, char* longFilename); static void menu_action_setting_edit_bool(const char* pstr, bool* ptr); static void menu_action_setting_edit_int3(const char* pstr, int* ptr, int minValue, int maxValue); @@ -1021,12 +1019,7 @@ static void _lcd_move(const char *name, int axis, int min, int max) { if (min_software_endstops && current_position[axis] < min) current_position[axis] = min; if (max_software_endstops && current_position[axis] > max) current_position[axis] = max; encoderPosition = 0; -#ifdef DELTA - calculate_delta(current_position); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis] / 60, active_extruder); -#else plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[axis] / 60, active_extruder); -#endif lcdDrawUpdate = 1; } if (lcdDrawUpdate) lcd_implementation_drawedit(name, ftostr31(current_position[axis])); @@ -1040,12 +1033,7 @@ static void lcd_move_e() { current_position[E_AXIS] += float((int)encoderPosition) * move_menu_scale; encoderPosition = 0; -#ifdef DELTA - calculate_delta(current_position); - plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS] / 60, active_extruder); -#else plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[E_AXIS] / 60, active_extruder); -#endif lcdDrawUpdate = 1; } if (lcdDrawUpdate) @@ -1543,7 +1531,7 @@ void lcd_mesh_calibration() void lcd_mesh_calibration_reset() { - enquecommand_P(PSTR("M44")); + enquecommand_P(PSTR("M45")); lcd_return_to_status(); } @@ -2520,26 +2508,12 @@ static void menu_action_back(menuFunc_t data) { static void menu_action_submenu(menuFunc_t data) { lcd_goto_menu(data); } -static void menu_action_gcode(const char* pgcode) { - enquecommand_P(pgcode); -} static void menu_action_setlang(unsigned char lang) { lcd_set_lang(lang); } static void menu_action_function(menuFunc_t data) { (*data)(); } -static void menu_action_sdfile(const char* filename, char* longFilename) -{ - char cmd[30]; - char* c; - sprintf_P(cmd, PSTR("M23 %s"), filename); - for (c = &cmd[4]; *c; c++) - *c = tolower(*c); - enquecommand(cmd); - enquecommand_P(PSTR("M24")); - lcd_return_to_status(); -} static void menu_action_sddirectory(const char* filename, char* longFilename) { card.chdir(filename); |