Merge pull request #943 from PavelSindler/snmm_v2

M600 for snmm_v2: initial version

Firmware/Marlin.h

@@ -468,4 +468,7 @@ void proc_commands();
 
 bool mmu_get_reponse();
 void mmu_not_responding();
-void mmu_load_to_nozzle();
+void mmu_load_to_nozzle();
+void M600_load_filament(bool fsensor_enabled);
+void mmu_M600_load_filament();
+void M600_load_filament_movements();

Firmware/Marlin_main.cpp

@@ -6269,7 +6269,6 @@ Sigma_Exit:
 #endif //PAT9125
 
 		st_synchronize();
-		float target[4];
 		float lastpos[4];
 
         if (farm_mode)
@@ -6285,10 +6284,6 @@ Sigma_Exit:
 
 		float HotendTempBckp = degTargetHotend(active_extruder);
 		int fanSpeedBckp = fanSpeed;
-        target[X_AXIS]=current_position[X_AXIS];
-        target[Y_AXIS]=current_position[Y_AXIS];
-        target[Z_AXIS]=current_position[Z_AXIS];
-        target[E_AXIS]=current_position[E_AXIS];
         lastpos[X_AXIS]=current_position[X_AXIS];
         lastpos[Y_AXIS]=current_position[Y_AXIS];
         lastpos[Z_AXIS]=current_position[Z_AXIS];
@@ -6297,27 +6292,27 @@ Sigma_Exit:
         //Restract extruder
         if(code_seen('E'))
         {
-          target[E_AXIS]+= code_value();
+          current_position[E_AXIS]+= code_value();
         }
         else
         {
           #ifdef FILAMENTCHANGE_FIRSTRETRACT
-            target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT ;
+            current_position[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT ;
           #endif
         }
-        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
 
         //Lift Z
         if(code_seen('Z'))
         {
-          target[Z_AXIS]+= code_value();
+          current_position[Z_AXIS]+= code_value();
         }
         else
         {
           #ifdef FILAMENTCHANGE_ZADD
-            target[Z_AXIS]+= FILAMENTCHANGE_ZADD ;
-            if(target[Z_AXIS] < 10){
-              target[Z_AXIS]+= 10 ;
+            current_position[Z_AXIS]+= FILAMENTCHANGE_ZADD ;
+            if(current_position[Z_AXIS] < 10){
+              current_position[Z_AXIS]+= 10 ;
               TooLowZ = 1;
             }else{
               TooLowZ = 0;
@@ -6326,30 +6321,30 @@ Sigma_Exit:
      
           
         }
-        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_ZFEED, active_extruder);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_ZFEED, active_extruder);
 
         //Move XY to side
         if(code_seen('X'))
         {
-          target[X_AXIS]+= code_value();
+          current_position[X_AXIS]+= code_value();
         }
         else
         {
           #ifdef FILAMENTCHANGE_XPOS
-            target[X_AXIS]= FILAMENTCHANGE_XPOS ;
+            current_position[X_AXIS]= FILAMENTCHANGE_XPOS ;
           #endif
         }
         if(code_seen('Y'))
         {
-          target[Y_AXIS]= code_value();
+          current_position[Y_AXIS]= code_value();
         }
         else
         {
           #ifdef FILAMENTCHANGE_YPOS
-            target[Y_AXIS]= FILAMENTCHANGE_YPOS ;
+            current_position[Y_AXIS]= FILAMENTCHANGE_YPOS ;
           #endif
         }
-        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_XYFEED, active_extruder);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_XYFEED, active_extruder);
 		st_synchronize();
 		KEEPALIVE_STATE(PAUSED_FOR_USER);
 
@@ -6365,14 +6360,6 @@ Sigma_Exit:
 			manage_heater();
 			manage_inactivity(true);
 
-			/*#ifdef SNMM
-			target[E_AXIS] += 0.002;
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 500, active_extruder);
-
-			#endif // SNMM*/
-
-			//if (cnt == 0)
-			{
 #if BEEPER > 0
 				if (counterBeep == 500) {
 					counterBeep = 0;
@@ -6386,9 +6373,7 @@ Sigma_Exit:
 				}
 				
 				counterBeep++;
-#else
 #endif
-			}
 			
 			switch (wait_for_user_state) {
 			case 0: 
@@ -6446,7 +6431,7 @@ Sigma_Exit:
 
 			if (code_seen('L'))
 			{
-				target[E_AXIS] += code_value();
+				current_position[E_AXIS] += code_value();
 			}
 			else
 			{
@@ -6454,32 +6439,32 @@ Sigma_Exit:
 
 #else
 #ifdef FILAMENTCHANGE_FINALRETRACT
-				target[E_AXIS] += FILAMENTCHANGE_FINALRETRACT;
+				current_position[E_AXIS] += FILAMENTCHANGE_FINALRETRACT;
 #endif
 #endif // SNMM
 			}
 
 #ifdef SNMM
-			target[E_AXIS] += 12;
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 3500, active_extruder);
-			target[E_AXIS] += 6;
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 5000, active_extruder);
-			target[E_AXIS] += (FIL_LOAD_LENGTH * -1);
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 5000, active_extruder);
+			current_position[E_AXIS] += 12;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3500, active_extruder);
+			current_position[E_AXIS] += 6;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 5000, active_extruder);
+			current_position[E_AXIS] += (FIL_LOAD_LENGTH * -1);
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 5000, active_extruder);
 			st_synchronize();
-			target[E_AXIS] += (FIL_COOLING);
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 50, active_extruder);
-			target[E_AXIS] += (FIL_COOLING*-1);
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 50, active_extruder);
-			target[E_AXIS] += (bowden_length[snmm_extruder] * -1);
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 3000, active_extruder);
+			current_position[E_AXIS] += (FIL_COOLING);
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 50, active_extruder);
+			current_position[E_AXIS] += (FIL_COOLING*-1);
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 50, active_extruder);
+			current_position[E_AXIS] += (bowden_length[snmm_extruder] * -1);
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000, active_extruder);
 			st_synchronize();
 
 #else
-			//		plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
-			//plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 3500 / 60, active_extruder);
+			//		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
+			//plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3500 / 60, active_extruder);
             
-            target[E_AXIS] -= FILAMENTCHANGE_FINALRETRACT;
+            current_position[E_AXIS] -= FILAMENTCHANGE_FINALRETRACT;
             st_synchronize();
 #ifdef TMC2130
             uint8_t tmc2130_current_r_bckp = tmc2130_current_r[E_AXIS];
@@ -6492,14 +6477,14 @@ Sigma_Exit:
 
 #endif //TMC2130
 
-            target[E_AXIS] -= 45;
-            plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 5200 / 60, active_extruder);
+            current_position[E_AXIS] -= 45;
+            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 5200 / 60, active_extruder);
             st_synchronize();
-            target[E_AXIS] -= 15;
-            plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 1000 / 60, active_extruder);
+            current_position[E_AXIS] -= 15;
+            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 1000 / 60, active_extruder);
             st_synchronize();
-            target[E_AXIS] -= 20;
-            plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 1000 / 60, active_extruder);
+            current_position[E_AXIS] -= 20;
+            plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 1000 / 60, active_extruder);
             st_synchronize();
 
 #ifdef TMC2130            
@@ -6550,89 +6535,16 @@ Sigma_Exit:
 			//lcd_return_to_status();
 			lcd_update_enable(true);
 		
-        //Wait for user to insert filament
-        lcd_wait_interact();
-		//load_filament_time = millis();
-		KEEPALIVE_STATE(PAUSED_FOR_USER);
-
-#ifdef PAT9125
-		if (filament_autoload_enabled && (old_fsensor_enabled || !fsensor_watch_runout)) fsensor_autoload_check_start();
-#endif //PAT9125
-        while(!lcd_clicked())
-		{
-          manage_heater();
-          manage_inactivity(true);
-#ifdef PAT9125
-		  if (filament_autoload_enabled && (old_fsensor_enabled || !fsensor_watch_runout) && fsensor_check_autoload())
-		  {
-			tone(BEEPER, 1000);
-			delay_keep_alive(50);
-			noTone(BEEPER);
-			  break;
-		  }
-#endif //PAT9125
-/*#ifdef SNMM
-		  target[E_AXIS] += 0.002;
-		  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 500, active_extruder);
-
-#endif // SNMM*/
-
-        }
-#ifdef PAT9125
-		if (filament_autoload_enabled && (old_fsensor_enabled || !fsensor_watch_runout)) fsensor_autoload_check_stop();
-#endif //PAT9125
-		//WRITE(BEEPER, LOW);
-		KEEPALIVE_STATE(IN_HANDLER);
-
-
-#ifdef SNMM
-		display_loading();
-		KEEPALIVE_STATE(PAUSED_FOR_USER);
-		do {
-			target[E_AXIS] += 0.002;
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 500, active_extruder);
-			delay_keep_alive(2);
-		} while (!lcd_clicked());
-		KEEPALIVE_STATE(IN_HANDLER);
-		/*if (millis() - load_filament_time > 2) {
-			load_filament_time = millis();
-			target[E_AXIS] += 0.001;
-			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 1000, active_extruder);
-		}*/
-
-        //Filament inserted     
-		//Feed the filament to the end of nozzle quickly   		
-		st_synchronize();
-		target[E_AXIS] += bowden_length[snmm_extruder];
-		plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 3000, active_extruder);
-		target[E_AXIS] += FIL_LOAD_LENGTH - 60;
-		plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 1400, active_extruder);
-		target[E_AXIS] += 40;
-		plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 400, active_extruder);
-		target[E_AXIS] += 10;
-		plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 50, active_extruder);
-		//Extrude some filament
-        target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_EXFEED, active_extruder); 
-#else
-		target[E_AXIS] += FILAMENTCHANGE_FIRSTFEED;
-		plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_EFEED, active_extruder);
-		//Extrude some filament
-        target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-        plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_EXFEED, active_extruder); 
-
-#endif // SNMM
-        
+        //Wait for user to insert filament and load filament to nozzle
+#ifdef SNMM_V2
+		mmu_M600_load_filament();
+#else		
+		M600_load_filament(old_fsensor_enabled);
+#endif    
 
-        
-        //Wait for user to check the state
+		//Wait for user to check the state
         lcd_change_fil_state = 0;
-        lcd_loading_filament();
-
-		tone(BEEPER, 500);
-		delay_keep_alive(50);
-		noTone(BEEPER);
-
+		
 		while ((lcd_change_fil_state == 0)||(lcd_change_fil_state != 1)){
           lcd_change_fil_state = 0;
 		  KEEPALIVE_STATE(PAUSED_FOR_USER);
@@ -6642,39 +6554,17 @@ Sigma_Exit:
             
              // Filament failed to load so load it again
              case 2:
-#ifdef SNMM
-				 display_loading();
-				 do {
-					 target[E_AXIS] += 0.002;
-					 plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 500, active_extruder);
-					 delay_keep_alive(2);
-				 } while (!lcd_clicked());
-
-				 st_synchronize();
-				 target[E_AXIS] += bowden_length[snmm_extruder];
-				 plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 3000, active_extruder);
-				 target[E_AXIS] += FIL_LOAD_LENGTH - 60;
-				 plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 1400, active_extruder);
-				 target[E_AXIS] += 40;
-				 plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 400, active_extruder);
-				 target[E_AXIS] += 10;
-				 plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 50, active_extruder);
-
+#ifdef SNMM_V2
+                     mmu_M600_load_filament(); //change to "wrong filament loaded" option?
 #else
-                     target[E_AXIS]+= FILAMENTCHANGE_FIRSTFEED ;
-                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_EFEED, active_extruder); 
-#endif                
-                     target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_EXFEED, active_extruder); 
-
-                     lcd_loading_filament();
-
+                     M600_load_filament_movements();
+#endif
                      break;
 
              // Filament loaded properly but color is not clear
              case 3:
-                     target[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
-                     plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 2, active_extruder); 
+                     current_position[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
+                     plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 2, active_extruder); 
                      lcd_loading_color();
                      break;
                  
@@ -6689,34 +6579,37 @@ Sigma_Exit:
         
 
       //Not let's go back to print
-		fanSpeed = fanSpeedBckp;
+	  fanSpeed = fanSpeedBckp;
 
       //Feed a little of filament to stabilize pressure
-      target[E_AXIS]+= FILAMENTCHANGE_RECFEED;
-      plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_EXFEED, active_extruder);
+      current_position[E_AXIS]+= FILAMENTCHANGE_RECFEED;
+      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_EXFEED, active_extruder);
         
       //Retract
-      target[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT;
-      plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
+      current_position[E_AXIS]+= FILAMENTCHANGE_FIRSTRETRACT;
+      plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
         
 
         
-      //plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 70, active_extruder); //should do nothing
+      //plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 70, active_extruder); //should do nothing
       
       //Move XY back
-      plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_XYFEED, active_extruder);
+      plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_XYFEED, active_extruder);
       
       //Move Z back
-      plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_ZFEED, active_extruder);
+      plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_ZFEED, active_extruder);
         
         
-      target[E_AXIS]= target[E_AXIS] - FILAMENTCHANGE_FIRSTRETRACT;
+      current_position[E_AXIS]= current_position[E_AXIS] - FILAMENTCHANGE_FIRSTRETRACT;
         
       //Unretract       
-      plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
+      plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
         
       //Set E position to original  
       plan_set_e_position(lastpos[E_AXIS]);
+
+	  memcpy(current_position, lastpos, sizeof(lastpos));
+	  memcpy(destination, current_position, sizeof(current_position));
        
       //Recover feed rate 
       feedmultiply=feedmultiplyBckp;
@@ -7157,29 +7050,7 @@ Sigma_Exit:
 
 #ifdef SNMM_V2
 		  printf_P(PSTR("T code: %d \n"), tmp_extruder);
-          switch (tmp_extruder) 
-          {
-          case 1:
-              
-              fprintf_P(uart2io, PSTR("T1\n"));
-              break;
-          case 2:
-              
-              fprintf_P(uart2io, PSTR("T2\n"));
-              break;
-          case 3:
-              
-              fprintf_P(uart2io, PSTR("T3\n"));
-              break;
-          case 4:
-              
-              fprintf_P(uart2io, PSTR("T4\n"));
-              break;
-          default:
-              
-              fprintf_P(uart2io, PSTR("T0\n"));
-              break;
-          }
+		  fprintf_P(uart2io, PSTR("T%d\n"), tmp_extruder);
 
 		  bool response = mmu_get_reponse();
 		  if (!response) mmu_not_responding();
@@ -9204,7 +9075,7 @@ void mmu_not_responding() {
 }
 
 void mmu_load_to_nozzle() {
-	bool saved_e_relative_mode = axis_relative_modes[E_AXIS];
+	/*bool saved_e_relative_mode = axis_relative_modes[E_AXIS];
 	if (!saved_e_relative_mode) {
 		enquecommand_front_P(PSTR("M82")); // set extruder to relative mode
 	}
@@ -9212,9 +9083,122 @@ void mmu_load_to_nozzle() {
 		enquecommand_front_P((PSTR("G1 E14.4000 F871")));
 		enquecommand_front_P((PSTR("G1 E36.0000 F1393")));
 		enquecommand_front_P((PSTR("G1 E14.4000 F871")));			  
-		if (!saved_e_relative_mode) {
-		  enquecommand_front_P(PSTR("M83")); // set extruder to relative mode
-		}
+	if (!saved_e_relative_mode) {
+	    enquecommand_front_P(PSTR("M83")); // set extruder to relative mode
+	}*/
+	st_synchronize();
+	
+	bool saved_e_relative_mode = axis_relative_modes[E_AXIS];
+	if (!saved_e_relative_mode) axis_relative_modes[E_AXIS] = true;
+	current_position[E_AXIS] += 7.2f;
+    float feedrate = 562;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate / 60, active_extruder);
+    st_synchronize();
+	current_position[E_AXIS] += 14.4f;
+	feedrate = 871;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate / 60, active_extruder);
+    st_synchronize();
+	current_position[E_AXIS] += 36.0f;
+	feedrate = 1393;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate / 60, active_extruder);
+    st_synchronize();
+	current_position[E_AXIS] += 14.4f;
+	feedrate = 871;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate / 60, active_extruder);
+    st_synchronize();
+	if (!saved_e_relative_mode) axis_relative_modes[E_AXIS] = false;
+}
+
+void mmu_switch_extruder(uint8_t extruder) {
+
+}
+
+void mmu_M600_load_filament() {
+#ifdef SNMM_V2
+		  bool response = false;
+		  tmp_extruder = choose_extruder_menu();
+		  lcd_update_enable(false);
+		  lcd_clear();
+		  lcd_set_cursor(0, 1); lcd_puts_P(_T(MSG_LOADING_FILAMENT));
+		  lcd_print(" ");
+		  lcd_print(snmm_extruder + 1);
+		  snmm_filaments_used |= (1 << tmp_extruder); //for stop print
+		  printf_P(PSTR("T code: %d \n"), tmp_extruder);
+		  fprintf_P(uart2io, PSTR("T%d\n"), tmp_extruder);
+		  response = mmu_get_reponse();
+		  if (!response) mmu_not_responding();
+
+    	  snmm_extruder = tmp_extruder; //filament change is finished
+
+		  mmu_load_to_nozzle();
+#endif
+}
+
+void M600_load_filament_movements() {
+#ifdef SNMM
+				 display_loading();
+				 do {
+					 current_position[E_AXIS] += 0.002;
+					 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 500, active_extruder);
+					 delay_keep_alive(2);
+				 } while (!lcd_clicked());
+
+				 st_synchronize();
+				 current_position[E_AXIS] += bowden_length[snmm_extruder];
+				 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000, active_extruder);
+				 current_position[E_AXIS] += FIL_LOAD_LENGTH - 60;
+				 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 1400, active_extruder);
+				 current_position[E_AXIS] += 40;
+				 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 400, active_extruder);
+				 current_position[E_AXIS] += 10;
+				 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 50, active_extruder);
+
+#else
+                     current_position[E_AXIS]+= FILAMENTCHANGE_FIRSTFEED ;
+                     plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_EFEED, active_extruder); 
+#endif                
+                     current_position[E_AXIS]+= FILAMENTCHANGE_FINALFEED ;
+                     plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], FILAMENTCHANGE_EXFEED, active_extruder); 
+
+                     lcd_loading_filament();
+}
+
+void M600_load_filament(bool fsensor_enabled) {
+
+		lcd_wait_interact();
+
+		//load_filament_time = millis();
+		KEEPALIVE_STATE(PAUSED_FOR_USER);
+
+#ifdef PAT9125
+		if (filament_autoload_enabled && (fsensor_enabled || fsensor_M600)) fsensor_autoload_check_start();
+#endif //PAT9125
+        while(!lcd_clicked())
+		{
+          manage_heater();
+          manage_inactivity(true);
+#ifdef PAT9125
+		  if (filament_autoload_enabled && (fsensor_enabled || fsensor_M600) && fsensor_check_autoload())
+		  {
+			tone(BEEPER, 1000);
+			delay_keep_alive(50);
+			noTone(BEEPER);
+			  break;
+		  }
+#endif //PAT9125
+
+        }
+#ifdef PAT9125
+		if (filament_autoload_enabled && (fsensor_enabled || fsensor_M600)) fsensor_autoload_check_stop();
+#endif //PAT9125
+		KEEPALIVE_STATE(IN_HANDLER);
+
+		M600_load_filament_movements();
+
+		tone(BEEPER, 500);
+		delay_keep_alive(50);
+		noTone(BEEPER);
+
 }
 
 #define FIL_LOAD_LENGTH 60

Firmware/ultralcd.cpp

@@ -1463,6 +1463,9 @@ void lcd_commands()
 			enquecommand_P(PSTR("M190 S" STRINGIFY(PLA_PREHEAT_HPB_TEMP)));
 			enquecommand_P(PSTR("M109 S" STRINGIFY(PLA_PREHEAT_HOTEND_TEMP)));
 			enquecommand_P(_T(MSG_M117_V2_CALIBRATION));
+#ifdef SNMM_V2
+			enquecommand_P(PSTR("T?"));
+#endif //SNMM_V2
 			enquecommand_P(PSTR("G28"));
 			enquecommand_P(PSTR("G92 E0.0"));
 			lcd_commands_step = 8;
@@ -1653,8 +1656,12 @@ void lcd_commands()
 		if (lcd_commands_step == 2 && !blocks_queued() && cmd_buffer_empty())
 		{
 			lcd_timeoutToStatus = millis() + LCD_TIMEOUT_TO_STATUS;
+			enquecommand_P(PSTR("M107")); //turn off printer fan			
+			#ifdef SNMM_V2
+			enquecommand_P(PSTR("M702 C"));
+			#else //SNMM_V2
 			enquecommand_P(PSTR("G1 E-0.07500 F2100.00000"));
-			enquecommand_P(PSTR("M107")); //turn off printer fan
+			#endif //SNMM_V2
 			enquecommand_P(PSTR("M104 S0")); // turn off temperature
 			enquecommand_P(PSTR("M140 S0")); // turn off heatbed
 			enquecommand_P(PSTR("G1 Z10 F1300.000"));
@@ -1726,7 +1733,7 @@ void lcd_commands()
 			enquecommand_P(PSTR("G1 X50 Y" STRINGIFY(Y_MAX_POS) " E0 F7000"));
 			#endif
 			lcd_ignore_click(false);
-			#ifdef SNMM
+			#if defined (SNMM) || defined (SNMM_V2)
 			lcd_commands_step = 8;
 			#else
 			lcd_commands_step = 3;
@@ -1747,7 +1754,7 @@ void lcd_commands()
 			lcd_setstatuspgm(_T(MSG_PRINT_ABORTED));
 			cancel_heatup = true;
 			setTargetBed(0);
-			#ifndef SNMM
+			#if !(defined (SNMM) || defined (SNMM_V2))
 			setTargetHotend(0, 0);	//heating when changing filament for multicolor
 			setTargetHotend(0, 1);
 			setTargetHotend(0, 2);
@@ -1758,12 +1765,16 @@ void lcd_commands()
 			lcd_commands_step = 5;
 		}
 		if (lcd_commands_step == 7 && !blocks_queued()) {
+			#ifdef SNMM_V2
+			enquecommand_P(PSTR("M702 C")); //current
+			#else //SNMM_V2
 			switch(snmm_stop_print_menu()) {
 				case 0: enquecommand_P(PSTR("M702")); break;//all 
 				case 1: enquecommand_P(PSTR("M702 U")); break; //used
 				case 2: enquecommand_P(PSTR("M702 C")); break; //current
 				default: enquecommand_P(PSTR("M702")); break;
 			}
+			#endif //SNMM_V2
 			lcd_commands_step = 3;
 		}
 		if (lcd_commands_step == 8 && !blocks_queued()) { //step 8 is here for delay (going to next step after execution of all gcodes from step 4)
@@ -4324,6 +4335,9 @@ void lcd_toshiba_flash_air_compatibility_toggle()
 }
 
 void lcd_v2_calibration() {
+#ifdef SNMM_V2
+	lcd_commands_type = LCD_COMMAND_V2_CAL;
+#else //SNMM_V2
 	bool loaded = lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Is PLA filament loaded?"), false, true);////MSG_PLA_FILAMENT_LOADED c=20 r=2
 	if (loaded) {
 		lcd_commands_type = LCD_COMMAND_V2_CAL;
@@ -4340,6 +4354,7 @@ void lcd_v2_calibration() {
 			}
 		}
 	}
+#endif //SNMM_V2
 	lcd_return_to_status();
 	lcd_update_enable(true);
 }