Fix Dondolo

MarlinKimbra/Marlin_main.cpp

@@ -6950,22 +6950,18 @@ inline void gcode_T(uint8_t tmp_extruder) {
             active_driver = 0;
             ECHO_LMV(DB, MSG_ACTIVE_COLOR, (int)active_extruder);
           #elif ENABLED(DONDOLO)
-            active_extruder = target_extruder;
-            active_driver = 0;
-            if (active_extruder == 0) {
             st_synchronize();
             servo[DONDOLO_SERVO_INDEX].attach(0);
+            if (target_extruder == 0) {
               servo[DONDOLO_SERVO_INDEX].write(DONDOLO_SERVOPOS_E0);
-              delay (DONDOLO_SERVO_DELAY);
-              servo[DONDOLO_SERVO_INDEX].detach();
             }
-            else if (active_extruder == 1) {
-              st_synchronize();
-              servo[DONDOLO_SERVO_INDEX].attach(0);
+            else if (target_extruder == 1) {
               servo[DONDOLO_SERVO_INDEX].write(DONDOLO_SERVOPOS_E1);
+            }
             delay(DONDOLO_SERVO_DELAY);
             servo[DONDOLO_SERVO_INDEX].detach();
-            }
+            active_extruder = target_extruder;
+            active_driver = 0;
             set_stepper_direction(true);
             ECHO_LMV(DB, MSG_ACTIVE_DRIVER, active_driver);
             ECHO_LMV(DB, MSG_ACTIVE_EXTRUDER, active_extruder);

MarlinKimbra/stepper.cpp

@@ -576,25 +576,23 @@ void set_stepper_direction(bool onlye) {
 
   #if DISABLED(ADVANCE) && ENABLED(DONDOLO)
     if (TEST(out_bits, E_AXIS)) {
-      if (active_extruder == 0) {
-        REV_E_DIR();
-        count_direction[E_AXIS] = -1;
+      switch(active_extruder) {
+        case 0:
+          REV_E_DIR(); break;
+        case 1:
+          NORM_E_DIR(); break;
       }
-      else {
-        NORM_E_DIR();
       count_direction[E_AXIS] = -1;
     }
-    }
     else {
-      if (active_extruder == 0) {
-        NORM_E_DIR();
-        count_direction[E_AXIS] = 1;
+      switch(active_extruder) {
+        case 0:
+          NORM_E_DIR(); break;
+        case 1:
+          REV_E_DIR(); break;
       }
-      else {
-        REV_E_DIR();
       count_direction[E_AXIS] = 1;
     }
-    }
   #elif DISABLED(ADVANCE)
     if (TEST(out_bits, E_AXIS)) {
       REV_E_DIR();