diff --git a/README.md b/README.md
index 6b8c5a8..881bd96 100644
--- a/README.md
+++ b/README.md
@@ -310,7 +310,9 @@ VIN sensing
 <br/>
 
 # Revision History
-### Version 2.1.1
+### Version 2.2.0
+- Added 2 functions startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, ...).
+- ESP32 core 3.x support.
 - Improved examples, especially follower examples.
 
 ### Version 2.1.0
diff --git a/examples/Basic/RobotCarConfigurations.h b/examples/Basic/RobotCarConfigurations.h
index fc07c5d..ba7f32d 100644
--- a/examples/Basic/RobotCarConfigurations.h
+++ b/examples/Basic/RobotCarConfigurations.h
@@ -317,7 +317,7 @@
  * BASIC CONFIGURATION for ESP32-Cam car
  * Car controlled by an ESP32-Cam module
  */
-#if defined(ESP32) // a temporarily hack
+#if defined(ESP32) && !defined(CAR_IS_ESP32_CAM_BASED) // a temporarily hack
 #define CAR_IS_ESP32_CAM_BASED
 #endif
 #if defined(CAR_IS_ESP32_CAM_BASED)
diff --git a/examples/IRDispatcherControl/Distance.hpp b/examples/IRDispatcherControl/Distance.hpp
index 5cbcee2..50e2109 100644
--- a/examples/IRDispatcherControl/Distance.hpp
+++ b/examples/IRDispatcherControl/Distance.hpp
@@ -33,12 +33,16 @@
 #include "HCSR04.hpp" // include sources
 #include "RobotCarConfigurations.h" // helps the pretty printer / Ctrl F
 
-#if defined(CAR_HAS_SERVO) && defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE // saves 70 bytes program space
+#if defined(CAR_HAS_DISTANCE_SERVO)
+#  if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 #include "LightweightServo.hpp"
-#endif
+#    if defined(_LIGHTWEIGHT_SERVO_HPP)
+LightweightServo DistanceServo;    // The pan servo instance for distance sensor
+#    else // LightweightServo is not applicable for this CPU
+#undef USE_LIGHTWEIGHT_SERVO_LIBRARY
+#    endif
+#  endif // defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 
-#if defined(CAR_HAS_DISTANCE_SERVO)
 #  if !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 Servo DistanceServo;    // The pan servo instance for distance sensor
 #  endif
@@ -87,7 +91,7 @@ ForwardDistancesInfoStruct sForwardDistancesInfo;
 void initDistance() {
     getDistanceModesFromPins();
 
-#if defined(CAR_HAS_DISTANCE_SERVO) && !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
+#if defined(CAR_HAS_DISTANCE_SERVO)
     DistanceServo.attach(DISTANCE_SERVO_PIN);
 #endif
 
@@ -492,11 +496,8 @@ void DistanceServoWriteAndWaitForStop(uint8_t aTargetDegrees, bool doWaitForStop
     // The servo is top down and therefore inverted
     aTargetDegrees = 180 - aTargetDegrees;
 #endif
-#if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-    write10(aTargetDegrees);
-#else
     DistanceServo.write(aTargetDegrees);
-#endif
+
 
     /*
      * Delay until stopped
diff --git a/examples/IRDispatcherControl/RobotCarIRCommands.hpp b/examples/IRDispatcherControl/RobotCarIRCommands.hpp
index 0331900..a41968b 100644
--- a/examples/IRDispatcherControl/RobotCarIRCommands.hpp
+++ b/examples/IRDispatcherControl/RobotCarIRCommands.hpp
@@ -80,19 +80,19 @@ void doReset() {
 void goForward() {
     if (IRDispatcher.IRReceivedData.isRepeat) {
         // if repeat was pressed, we enable a "fast" stop
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
                 DIRECTION_FORWARD);
     } else {
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
                 DIRECTION_FORWARD);
     }
 }
 void goBackward() {
     if (IRDispatcher.IRReceivedData.isRepeat) {
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
                 DIRECTION_BACKWARD);
     } else {
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
                 DIRECTION_BACKWARD);
     }
 }
diff --git a/examples/RobotCarBasic/Distance.hpp b/examples/RobotCarBasic/Distance.hpp
index 5cbcee2..50e2109 100644
--- a/examples/RobotCarBasic/Distance.hpp
+++ b/examples/RobotCarBasic/Distance.hpp
@@ -33,12 +33,16 @@
 #include "HCSR04.hpp" // include sources
 #include "RobotCarConfigurations.h" // helps the pretty printer / Ctrl F
 
-#if defined(CAR_HAS_SERVO) && defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE // saves 70 bytes program space
+#if defined(CAR_HAS_DISTANCE_SERVO)
+#  if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 #include "LightweightServo.hpp"
-#endif
+#    if defined(_LIGHTWEIGHT_SERVO_HPP)
+LightweightServo DistanceServo;    // The pan servo instance for distance sensor
+#    else // LightweightServo is not applicable for this CPU
+#undef USE_LIGHTWEIGHT_SERVO_LIBRARY
+#    endif
+#  endif // defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 
-#if defined(CAR_HAS_DISTANCE_SERVO)
 #  if !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 Servo DistanceServo;    // The pan servo instance for distance sensor
 #  endif
@@ -87,7 +91,7 @@ ForwardDistancesInfoStruct sForwardDistancesInfo;
 void initDistance() {
     getDistanceModesFromPins();
 
-#if defined(CAR_HAS_DISTANCE_SERVO) && !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
+#if defined(CAR_HAS_DISTANCE_SERVO)
     DistanceServo.attach(DISTANCE_SERVO_PIN);
 #endif
 
@@ -492,11 +496,8 @@ void DistanceServoWriteAndWaitForStop(uint8_t aTargetDegrees, bool doWaitForStop
     // The servo is top down and therefore inverted
     aTargetDegrees = 180 - aTargetDegrees;
 #endif
-#if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-    write10(aTargetDegrees);
-#else
     DistanceServo.write(aTargetDegrees);
-#endif
+
 
     /*
      * Delay until stopped
diff --git a/examples/RobotCarBlueDisplay/Distance.hpp b/examples/RobotCarBlueDisplay/Distance.hpp
index 5cbcee2..50e2109 100644
--- a/examples/RobotCarBlueDisplay/Distance.hpp
+++ b/examples/RobotCarBlueDisplay/Distance.hpp
@@ -33,12 +33,16 @@
 #include "HCSR04.hpp" // include sources
 #include "RobotCarConfigurations.h" // helps the pretty printer / Ctrl F
 
-#if defined(CAR_HAS_SERVO) && defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE // saves 70 bytes program space
+#if defined(CAR_HAS_DISTANCE_SERVO)
+#  if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 #include "LightweightServo.hpp"
-#endif
+#    if defined(_LIGHTWEIGHT_SERVO_HPP)
+LightweightServo DistanceServo;    // The pan servo instance for distance sensor
+#    else // LightweightServo is not applicable for this CPU
+#undef USE_LIGHTWEIGHT_SERVO_LIBRARY
+#    endif
+#  endif // defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 
-#if defined(CAR_HAS_DISTANCE_SERVO)
 #  if !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 Servo DistanceServo;    // The pan servo instance for distance sensor
 #  endif
@@ -87,7 +91,7 @@ ForwardDistancesInfoStruct sForwardDistancesInfo;
 void initDistance() {
     getDistanceModesFromPins();
 
-#if defined(CAR_HAS_DISTANCE_SERVO) && !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
+#if defined(CAR_HAS_DISTANCE_SERVO)
     DistanceServo.attach(DISTANCE_SERVO_PIN);
 #endif
 
@@ -492,11 +496,8 @@ void DistanceServoWriteAndWaitForStop(uint8_t aTargetDegrees, bool doWaitForStop
     // The servo is top down and therefore inverted
     aTargetDegrees = 180 - aTargetDegrees;
 #endif
-#if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-    write10(aTargetDegrees);
-#else
     DistanceServo.write(aTargetDegrees);
-#endif
+
 
     /*
      * Delay until stopped
diff --git a/examples/RobotCarBlueDisplay/LightweightServo.h b/examples/RobotCarBlueDisplay/LightweightServo.h
index c4b1231..115646d 100644
--- a/examples/RobotCarBlueDisplay/LightweightServo.h
+++ b/examples/RobotCarBlueDisplay/LightweightServo.h
@@ -41,12 +41,12 @@
 #define ISR_COUNT_FOR_2_5_MILLIS (F_CPU / (8 * 400)) // 5000 For 400 Hz, 2.5 ms using a prescaler of 8.
 
 #if defined(__AVR_ATmega2560__)
-#define LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN   46
-#define LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN   45
-#define LEIGHTWEIGHT_SERVO_CHANNEL_C_PIN   44
+#define LIGHTWEIGHT_SERVO_CHANNEL_A_PIN   46
+#define LIGHTWEIGHT_SERVO_CHANNEL_B_PIN   45
+#define LIGHTWEIGHT_SERVO_CHANNEL_C_PIN   44
 #elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__)
-#define LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN    9
-#define LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN   10
+#define LIGHTWEIGHT_SERVO_CHANNEL_A_PIN    9
+#define LIGHTWEIGHT_SERVO_CHANNEL_B_PIN   10
 #endif
 
 void initLightweightServoPins(); // currently only pin 44 = OC5C/PL5, 45 = OC5B/PL4 + 46 = OC5A/PL3 on 2560 and pin 9 = OC1A + 10 = OC1B on 328
@@ -54,7 +54,8 @@ void checkAndInitLightweightServoPin(uint8_t aPin);
 void deinitLightweightServoPin(uint8_t aPin); // Set pin to input and disable non-inverting Compare Output mode
 
 int writeLightweightServoPin(int aDegree, uint8_t aPin, bool aUpdateFast = false);
-void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast = false);
+// The last parameter requires 8 byte more than DISABLE_SERVO_TIMER_AUTO_INITIALIZE, if false, but saves around 60 bytes anyway
+void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast = false, bool aDoAutoInit = true);
 
 void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int a180DegreeValue);
 void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds);
@@ -89,6 +90,22 @@ int writeLightweightServo(int aDegree, bool aUsePin9, bool aUpdateFast = false);
 void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aUpdateFast = false);
 #endif // Old and fast ATmega328 functions
 
+class LightweightServo {
+public:
+    uint8_t attach(int aPin);
+    uint8_t attach(int aPin, int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree);
+    void detach();
+    void write(int aTargetDegreeOrMicrosecond);
+    void writeMicroseconds(int aTargetMicrosecond); // Write pulse width in microseconds
+    /*
+     * Variables to enable adjustment for different servo types
+     * 544 and 2400 are values compatible with standard arduino values
+     */
+    int MicrosecondsForServo0Degree = 544;
+    int MicrosecondsForServo180Degree = 2400;
+    uint8_t LightweightServoPin;
+};
+
 #endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__)
 
 /*
diff --git a/examples/RobotCarBlueDisplay/LightweightServo.hpp b/examples/RobotCarBlueDisplay/LightweightServo.hpp
index 1362790..b9ca849 100644
--- a/examples/RobotCarBlueDisplay/LightweightServo.hpp
+++ b/examples/RobotCarBlueDisplay/LightweightServo.hpp
@@ -1,7 +1,10 @@
 /*
  *  LightweightServo.hpp
  *
- *  Lightweight Servo implementation only for pin 9 and 10 using only timer1 hardware and no interrupts or other overhead.
+ *  Lightweight Servo implementation timer hardware and no interrupts or other overhead.
+ *  Supported pins:
+ *  ATmega328: pin 44 = OC5C/PL5, 45 = OC5B/PL4 and 46 = OC5A/PL3 using only timer5 hardware
+ *  ATmega2560 pin 9 = OC1A and 10 = OC1B on 328 using only timer1 hardware
  *  Provides auto initialization.
  *  300 bytes code size / 4 bytes RAM including auto initialization compared to 700 / 48 bytes for Arduino Servo library.
  *  8 bytes for each call to setLightweightServoPulse...
@@ -26,12 +29,12 @@
  *
  */
 
-#ifndef _LIGHTWEIGHT_SERVO_HPP
-#define _LIGHTWEIGHT_SERVO_HPP
-
 #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined (__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__)
 #include "LightweightServo.h"
 
+#ifndef _LIGHTWEIGHT_SERVO_HPP
+#define _LIGHTWEIGHT_SERVO_HPP
+
 #if defined(DEBUG)
 #define LOCAL_DEBUG
 #else
@@ -213,7 +216,9 @@ void writeMicroseconds10Direct(int aMicroseconds) {
  * Attention - both pins are set to OUTPUT here!
  * 32 bytes code size
  * Assume, that PRR1 PRTIM5 bit is low, which enables timer 5
- * Currently only pin 44 = OC5C/PL5, 45 = OC5B/PL4 + 46 = OC5A/PL3 on 2560 and pin 9 = OC1A + 10 = OC1B on 328
+ *  Supported pins:
+ *  ATmega328: pin 44 = OC5C/PL5, 45 = OC5B/PL4 and 46 = OC5A/PL3 using only timer5 hardware
+ *  ATmega2560 pin 9 = OC1A and 10 = OC1B on 328 using only timer1 hardware
  */
 void initLightweightServoPins() {
 #if defined(__AVR_ATmega2560__)
@@ -246,17 +251,17 @@ void checkAndInitLightweightServoPin(uint8_t aPin) {
     // Mask all other bits to zero!
 #if defined(__AVR_ATmega2560__)
     uint8_t tNewTCCR5A = TCCR5A & (_BV(COM5A1) | _BV(COM5B1) | _BV(COM5C1) | _BV(WGM51));
-    if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN && !(TCCR5A & (_BV(COM5A1)))) {
+    if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN && !(TCCR5A & (_BV(COM5A1)))) {
         OCR5A = UINT16_MAX;
         DDRL |= (_BV(DDL3));
         tNewTCCR5A |= (_BV(COM5A1)) | _BV(WGM51); // FastPWM Mode mode TOP (20 ms) determined by ICR
         tPinWasNotInitialized = true;
-    } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR5A & (_BV(COM5B1)))) {
+    } else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR5A & (_BV(COM5B1)))) {
         OCR5B = UINT16_MAX;
         DDRL |= (_BV(DDL4));
         tNewTCCR5A |= (_BV(COM5B1)) | _BV(WGM51);
         tPinWasNotInitialized = true;
-    } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_C_PIN && !(TCCR5A & (_BV(COM5C1)))) {
+    } else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_C_PIN && !(TCCR5A & (_BV(COM5C1)))) {
         OCR5C = UINT16_MAX;
         DDRL |= (_BV(DDL5));
         tNewTCCR5A |= (_BV(COM5C1)) | _BV(WGM51);
@@ -264,12 +269,12 @@ void checkAndInitLightweightServoPin(uint8_t aPin) {
     }
 #else
     uint8_t tNewTCCR1A = TCCR1A & (_BV(COM1A1) | _BV(COM1B1)); // keep existing COM1A1 and COM1B1 settings
-    if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN && !(TCCR1A & (_BV(COM1A1)))) {
+    if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN && !(TCCR1A & (_BV(COM1A1)))) {
         OCR1A = UINT16_MAX;
         DDRB |= _BV(DDB1);                          // set OC1A = PortB1 -> PIN 9 to output direction
         tNewTCCR1A |= (_BV(COM1A1)) | _BV(WGM11);       // FastPWM Mode mode TOP (20 ms) determined by ICR1
         tPinWasNotInitialized = true;
-    } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR1A & (_BV(COM1B1)))) {
+    } else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN && !(TCCR1A & (_BV(COM1B1)))) {
         OCR1B = UINT16_MAX;
         DDRB |= _BV(DDB2);                          // set OC1B = PortB2 -> PIN 10 to output direction
         tNewTCCR1A |= (_BV(COM1B1)) | _BV(WGM11);
@@ -305,13 +310,14 @@ void checkAndInitLightweightServoPin(uint8_t aPin) {
 
 /*
  * Set pin to input and disable non-inverting Compare Output mode
+ * Init state is reflected by COMXX1 register bit
  */
 void deinitLightweightServoPin(uint8_t aPin) {
 #if defined(__AVR_ATmega2560__)
-    if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
+    if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
         DDRL &= ~(_BV(DDL3));
         TCCR5A &= ~(_BV(COM5A1));
-    } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN) {
+    } else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN) {
         DDRL &= ~(_BV(DDL4));
         TCCR5A &= ~(_BV(COM5B1));
     } else {
@@ -319,10 +325,10 @@ void deinitLightweightServoPin(uint8_t aPin) {
         TCCR5A &= ~(_BV(COM5C1));
     }
 #else
-    if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
+    if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
         DDRB &= ~(_BV(DDB1));           // set OC1A = PortB1 -> PIN 9 to input direction
         TCCR1A &= ~(_BV(COM1A1));       // disable non-inverting Compare Output mode OC1A
-    } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN) {
+    } else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN) {
         DDRB &= ~(_BV(DDB2));           // set OC1B = PortB2 -> PIN 10 to input direction
         TCCR1A &= ~(_BV(COM1B1));       // disable non-inverting Compare Output mode OC1B
     }
@@ -343,7 +349,7 @@ int writeLightweightServoPin(int aDegree, uint8_t aPin, bool aUpdateFast) {
     return aDegree;
 }
 
-void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast) {
+void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool aUpdateFast, bool aDoAutoInit) {
 #if defined(LOCAL_DEBUG)
     Serial.print(F(" Micros=")); // trailing space required if called by _writeMicrosecondsOrUnits()
     Serial.print(aMicroseconds);
@@ -351,29 +357,31 @@ void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool
     Serial.println(aPin);
 #endif
 #if !defined(DISABLE_SERVO_TIMER_AUTO_INITIALIZE)
-    checkAndInitLightweightServoPin(aPin);
+    if (aDoAutoInit) {
+        checkAndInitLightweightServoPin(aPin);
+    }
 #endif
-    // since the resolution is 1/2 of microsecond for 16 MHz CPU clock and prescaler of 8
+// since the resolution is 1/2 of microsecond for 16 MHz CPU clock and prescaler of 8
 #if (F_CPU == 16000000L)
     aMicroseconds *= 2;
 #elif (F_CPU < 8000000L) // for 8 MHz resolution is exactly 1 microsecond :-)
-    aMicroseconds /= (8000000L / F_CPU);
+aMicroseconds /= (8000000L / F_CPU);
 #endif
 #if defined(__AVR_ATmega2560__)
-    if (aUpdateFast) {
-        uint16_t tTimerCount = TCNT5;
-        if (tTimerCount > ISR_COUNT_FOR_2_5_MILLIS) {
-            // more than 2.5 ms since last pulse -> start a new one
-            TCNT5 = ICR5 - 1;
-        }
-    }
-    if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
-        OCR5A = aMicroseconds;
-    } else if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_B_PIN) {
-        OCR5B = aMicroseconds;
-    } else {
-        OCR5C = aMicroseconds;
+if (aUpdateFast) {
+    uint16_t tTimerCount = TCNT5;
+    if (tTimerCount > ISR_COUNT_FOR_2_5_MILLIS) {
+        // more than 2.5 ms since last pulse -> start a new one
+        TCNT5 = ICR5 - 1;
     }
+}
+if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
+    OCR5A = aMicroseconds;
+} else if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_B_PIN) {
+    OCR5B = aMicroseconds;
+} else {
+    OCR5C = aMicroseconds;
+}
 #else
     if (aUpdateFast) {
         uint16_t tTimerCount = TCNT1;
@@ -382,7 +390,7 @@ void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool
             TCNT1 = ICR1 - 1;
         }
     }
-    if (aPin == LEIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
+    if (aPin == LIGHTWEIGHT_SERVO_CHANNEL_A_PIN) {
         OCR1A = aMicroseconds;
     } else {
         OCR1B = aMicroseconds;
@@ -395,7 +403,11 @@ void writeMicrosecondsLightweightServoPin(int aMicroseconds, uint8_t aPin, bool
  * No parameter checking is done here!
  */
 void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds) {
+#if defined(__AVR_ATmega2560__)
+    ICR5 = aRefreshPeriodMicroseconds * 2;
+#else
     ICR1 = aRefreshPeriodMicroseconds * 2;
+#endif
 }
 /*
  * Set the mapping pulse width values for 0 and 180 degree
@@ -416,8 +428,46 @@ int MicrosecondsToDegreeLightweightServo(int aMicroseconds) {
     return map(aMicroseconds, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree, 0, 180);
 }
 
+/*
+ * LightweightServo class functions
+ */
+uint8_t LightweightServo::attach(int aPin) {
+    LightweightServoPin = aPin;
+    checkAndInitLightweightServoPin(aPin);
+    return aPin;
+}
+
+/*
+ * do not use parameters aMicrosecondsForServo0Degree and aMicrosecondsForServo180Degree
+ */
+uint8_t LightweightServo::attach(int aPin, int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree) {
+    LightweightServoPin = aPin;
+    MicrosecondsForServo0Degree = aMicrosecondsForServo0Degree;
+    MicrosecondsForServo180Degree = aMicrosecondsForServo180Degree;
+    checkAndInitLightweightServoPin(aPin);
+    return aPin;
+}
+
+void LightweightServo::detach() {
+    deinitLightweightServoPin(LightweightServoPin);
+}
+
+void LightweightServo::write(int aTargetDegreeOrMicrosecond) {
+    if (aTargetDegreeOrMicrosecond <= 180) {
+        aTargetDegreeOrMicrosecond = (map(aTargetDegreeOrMicrosecond, 0, 180, MicrosecondsForServo0Degree,
+                MicrosecondsForServo180Degree));
+    }
+    // The last false parameter requires 8 byte more than DISABLE_SERVO_TIMER_AUTO_INITIALIZE, but saves around 60 bytes anyway
+    writeMicrosecondsLightweightServoPin(aTargetDegreeOrMicrosecond, LightweightServoPin, false, false);
+}
+
+void LightweightServo::writeMicroseconds(int aTargetMicrosecond){
+    // The last false parameter requires 8 byte more than DISABLE_SERVO_TIMER_AUTO_INITIALIZE, but saves around 60 bytes anyway
+    writeMicrosecondsLightweightServoPin(aTargetMicrosecond, LightweightServoPin, false, false);
+}
+
 #if defined(LOCAL_DEBUG)
 #undef LOCAL_DEBUG
 #endif
-#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega2560__)
 #endif // _LIGHTWEIGHT_SERVO_HPP
+#endif // defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega2560__)
diff --git a/examples/SmartCarFollower/Distance.hpp b/examples/SmartCarFollower/Distance.hpp
index 5cbcee2..50e2109 100644
--- a/examples/SmartCarFollower/Distance.hpp
+++ b/examples/SmartCarFollower/Distance.hpp
@@ -33,12 +33,16 @@
 #include "HCSR04.hpp" // include sources
 #include "RobotCarConfigurations.h" // helps the pretty printer / Ctrl F
 
-#if defined(CAR_HAS_SERVO) && defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE // saves 70 bytes program space
+#if defined(CAR_HAS_DISTANCE_SERVO)
+#  if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 #include "LightweightServo.hpp"
-#endif
+#    if defined(_LIGHTWEIGHT_SERVO_HPP)
+LightweightServo DistanceServo;    // The pan servo instance for distance sensor
+#    else // LightweightServo is not applicable for this CPU
+#undef USE_LIGHTWEIGHT_SERVO_LIBRARY
+#    endif
+#  endif // defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 
-#if defined(CAR_HAS_DISTANCE_SERVO)
 #  if !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
 Servo DistanceServo;    // The pan servo instance for distance sensor
 #  endif
@@ -87,7 +91,7 @@ ForwardDistancesInfoStruct sForwardDistancesInfo;
 void initDistance() {
     getDistanceModesFromPins();
 
-#if defined(CAR_HAS_DISTANCE_SERVO) && !defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
+#if defined(CAR_HAS_DISTANCE_SERVO)
     DistanceServo.attach(DISTANCE_SERVO_PIN);
 #endif
 
@@ -492,11 +496,8 @@ void DistanceServoWriteAndWaitForStop(uint8_t aTargetDegrees, bool doWaitForStop
     // The servo is top down and therefore inverted
     aTargetDegrees = 180 - aTargetDegrees;
 #endif
-#if defined(USE_LIGHTWEIGHT_SERVO_LIBRARY)
-    write10(aTargetDegrees);
-#else
     DistanceServo.write(aTargetDegrees);
-#endif
+
 
     /*
      * Delay until stopped
diff --git a/examples/SmartCarFollower/RobotCarIRCommands.hpp b/examples/SmartCarFollower/RobotCarIRCommands.hpp
index 0331900..a41968b 100644
--- a/examples/SmartCarFollower/RobotCarIRCommands.hpp
+++ b/examples/SmartCarFollower/RobotCarIRCommands.hpp
@@ -80,19 +80,19 @@ void doReset() {
 void goForward() {
     if (IRDispatcher.IRReceivedData.isRepeat) {
         // if repeat was pressed, we enable a "fast" stop
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
                 DIRECTION_FORWARD);
     } else {
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
                 DIRECTION_FORWARD);
     }
 }
 void goBackward() {
     if (IRDispatcher.IRReceivedData.isRepeat) {
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER / 4,
                 DIRECTION_BACKWARD);
     } else {
-        RobotCar.startGoDistanceMillimeter(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
+        RobotCar.startGoDistanceMillimeterWithSpeed(RobotCar.rightCarMotor.DriveSpeedPWM, DEFAULT_CIRCUMFERENCE_MILLIMETER,
                 DIRECTION_BACKWARD);
     }
 }
diff --git a/library.json b/library.json
index dd051c6..710fc20 100644
--- a/library.json
+++ b/library.json
@@ -6,7 +6,7 @@
     "type": "git",
     "url": "https://github.com/ArminJo/PWMMotorControl"
   },
-  "version": "2.1.0",
+  "version": "2.2.0",
   "exclude": "pictures",
   "authors": {
     "name": "Armin Joachimsmeyer",
diff --git a/library.properties b/library.properties
index 5701a36..bd692b3 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=PWMMotorControl
-version=2.1.0
+version=2.2.0
 author=Armin Joachimsmeyer
 maintainer=Armin Joachimsmeyer <armin.arduino@gmail.com>
 sentence=Control brushed DC motors by PWM and uses optional attached encoders to drive fixed distances. For L298 or TB6612, or Adafruit Motor Shield
diff --git a/src/CarPWMMotorControl.h b/src/CarPWMMotorControl.h
index e65b526..d444eb8 100644
--- a/src/CarPWMMotorControl.h
+++ b/src/CarPWMMotorControl.h
@@ -141,14 +141,17 @@ class CarPWMMotorControl {
      * Functions for moving a fixed distance
      */
     // With signed distance
-    void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
-            uint8_t aRequestedDirection); // only setup values
-    void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values
     void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors()
-
+    void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values
+    void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+    uint8_t aRequestedDirection)__attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed()."))); // only setup values
     void goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped
+    void goDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped
     void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection,
             void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // only setup values
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+            uint8_t aRequestedDirection); // only setup values
 
     bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); // used internally
 
diff --git a/src/CarPWMMotorControl.hpp b/src/CarPWMMotorControl.hpp
index 3dbb1ff..25967ac 100644
--- a/src/CarPWMMotorControl.hpp
+++ b/src/CarPWMMotorControl.hpp
@@ -553,13 +553,17 @@ void CarPWMMotorControl::startRampUpAndWaitForDriveSpeedPWM(uint8_t aRequestedDi
 }
 
 void CarPWMMotorControl::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) {
-    startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
 }
 
+void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+        uint8_t aRequestedDirection) {
+    startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+}
 /*
  * initialize motorInfo fields LastDirection and CompensatedSpeedPWM
  */
-void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+void CarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
         uint8_t aRequestedDirection) {
 
 #if defined(USE_MPU6050_IMU)
@@ -572,26 +576,34 @@ void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, u
             setSpeedPWMWithRamp(aRequestedSpeedPWM, aRequestedDirection);
 #else
     checkAndHandleDirectionChange(aRequestedDirection);
-    rightCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
-    leftCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    rightCarMotor.startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    leftCarMotor.startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
 #endif
 }
 
 void CarPWMMotorControl::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection,
         void (*aLoopCallback)(void)) {
-    startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
     waitUntilStopped(aLoopCallback);
 }
 
 void CarPWMMotorControl::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) {
     if (aRequestedDistanceMillimeter < 0) {
         aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
-        startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+        startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
     } else {
-        startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+        startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
     }
 }
 
+void CarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) {
+    if (aRequestedDistanceMillimeter < 0) {
+        aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+    } else {
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+    }
+}
 /**
  * Wait until distance is reached
  * @param  aLoopCallback called until car has stopped to avoid blocking
@@ -601,6 +613,11 @@ void CarPWMMotorControl::goDistanceMillimeter(int aRequestedDistanceMillimeter,
     waitUntilStopped(aLoopCallback);
 }
 
+void CarPWMMotorControl::goDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)) {
+    startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter);
+    waitUntilStopped(aLoopCallback);
+}
+
 /*
  * Stop car (with ramp)
  */
@@ -797,8 +814,8 @@ void CarPWMMotorControl::startRotate(int aRotationDegrees, turn_direction_t aTur
             tLeftMotorIfPositiveTurn->setSpeedPWMAndDirection(tTurnSpeedPWMLeft, DIRECTION_BACKWARD);
         }
 #else
-    tRightMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMRight, tDistanceMillimeterRight, DIRECTION_FORWARD);
-    tLeftMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMLeft, tDistanceMillimeterLeft, DIRECTION_BACKWARD);
+    tRightMotorIfPositiveTurn->startGoDistanceMillimeterWithSpeed(tTurnSpeedPWMRight, tDistanceMillimeterRight, DIRECTION_FORWARD);
+    tLeftMotorIfPositiveTurn->startGoDistanceMillimeterWithSpeed(tTurnSpeedPWMLeft, tDistanceMillimeterLeft, DIRECTION_BACKWARD);
 #endif
 }
 
diff --git a/src/EncoderMotor.h b/src/EncoderMotor.h
index 4ad7ee2..566208d 100644
--- a/src/EncoderMotor.h
+++ b/src/EncoderMotor.h
@@ -1,7 +1,6 @@
 /*
  * EncoderMotor.h
  *
- *  Created on: 12.05.2019
  *  Copyright (C) 2019-2020  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
@@ -79,7 +78,9 @@ class EncoderMotor: public PWMDcMotor {
      */
     void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // Signed distance count
     void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection);
-    void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection);
+    void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection)__attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed().")));
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // Signed distance count
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection);
     bool updateMotor();
 
     /*
diff --git a/src/EncoderMotor.hpp b/src/EncoderMotor.hpp
index d5a506d..2438f6b 100644
--- a/src/EncoderMotor.hpp
+++ b/src/EncoderMotor.hpp
@@ -11,8 +11,7 @@
  *
  *  Tested for Adafruit Motor Shield and plain TB6612 breakout board.
  *
- *  Created on: 12.05.2019
- *  Copyright (C) 2019-2022  Armin Joachimsmeyer
+ *  Copyright (C) 2019-2024  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
  *  This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl.
@@ -90,10 +89,14 @@ void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMP
 }
 #endif
 
+void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+        uint8_t aRequestedDirection) {
+    startGoDistanceMillimeterWithSpeed( aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+}
 /*
  * If motor is already running, adjust TargetDistanceMillimeter to go to aRequestedDistanceMillimeter
  */
-void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+void EncoderMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
         uint8_t aRequestedDirection) {
     if (aRequestedDistanceMillimeter == 0) {
         stop(DefaultStopMode); // In case motor was running
@@ -118,7 +121,7 @@ void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigne
 }
 
 void EncoderMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) {
-    startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
 }
 
 /*
@@ -128,9 +131,18 @@ void EncoderMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMill
 void EncoderMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) {
     if (aRequestedDistanceMillimeter < 0) {
         aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
-        startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+        startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+    } else {
+        startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+    }
+}
+
+void EncoderMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) {
+    if (aRequestedDistanceMillimeter < 0) {
+        aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
     } else {
-        startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
     }
 }
 
diff --git a/src/IMUCarData.h b/src/IMUCarData.h
index f2404d8..1be7df5 100644
--- a/src/IMUCarData.h
+++ b/src/IMUCarData.h
@@ -3,7 +3,6 @@
  *
  *  Functions for getting IMU data from MPU6050 for car control.
  *
- *  Created on: 19.11.2020
  *  Copyright (C) 2020-2022  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
diff --git a/src/IMUCarData.hpp b/src/IMUCarData.hpp
index 935f428..fd7ea45 100644
--- a/src/IMUCarData.hpp
+++ b/src/IMUCarData.hpp
@@ -9,7 +9,6 @@
  *  Automatic offset recalculation is done, if we do not detect any movement for NUMBER_OF_OFFSET_CALIBRATION_SAMPLES samples.
  *  It can be activated (default) or suspended.
  *
- *  Created on: 19.11.2020
  *  Copyright (C) 2020-2022  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
diff --git a/src/MecanumWheelCarPWMMotorControl.h b/src/MecanumWheelCarPWMMotorControl.h
index e731d13..ed55f2d 100644
--- a/src/MecanumWheelCarPWMMotorControl.h
+++ b/src/MecanumWheelCarPWMMotorControl.h
@@ -3,7 +3,7 @@
  *
  *  Contains functions for control of the 4 motors of a mecanum wheel car.
  *
- *  Copyright (C) 2022  Armin Joachimsmeyer
+ *  Copyright (C) 2022-2024  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
  *  This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl.
@@ -71,10 +71,13 @@ class MecanumWheelCarPWMMotorControl : public CarPWMMotorControl {
      * Functions for moving a fixed distance
      */
     // With signed distance
+    void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors()
+    void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values
     void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+            uint8_t aRequestedDirection) __attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed().")));
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors()
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
             uint8_t aRequestedDirection); // only setup values
-    void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values
-    void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors()
 
     void goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped
     void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection,
diff --git a/src/MecanumWheelCarPWMMotorControl.hpp b/src/MecanumWheelCarPWMMotorControl.hpp
index 973a191..2433e08 100644
--- a/src/MecanumWheelCarPWMMotorControl.hpp
+++ b/src/MecanumWheelCarPWMMotorControl.hpp
@@ -424,13 +424,17 @@ void MecanumWheelCarPWMMotorControl::startRampUpAndWaitForDriveSpeedPWM(uint8_t
 
 void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter,
         uint8_t aRequestedDirection) {
-    startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
 }
 
+void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM,
+        unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) {
+    startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+}
 /*
  * initialize motorInfo fields LastDirection and CompensatedSpeedPWM
  */
-void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM,
+void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM,
         unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) {
 
 #if defined(USE_MPU6050_IMU)
@@ -443,23 +447,32 @@ void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequeste
     setSpeedPWMWithRamp(aRequestedSpeedPWM, aRequestedDirection);
 #else
     checkAndHandleDirectionChange(aRequestedDirection);
-    rightCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    rightCarMotor.startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
     setDirection(aRequestedDirection); // this sets the direction for all the other motors
 #endif
 }
 
 void MecanumWheelCarPWMMotorControl::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection,
         void (*aLoopCallback)(void)) {
-    startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
     waitUntilStopped(aLoopCallback);
 }
 
 void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) {
     if (aRequestedDistanceMillimeter < 0) {
         aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
-        startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+        startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+    } else {
+        startGoDistanceMillimeterWithSpeed(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+    }
+}
+
+void MecanumWheelCarPWMMotorControl::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) {
+    if (aRequestedDistanceMillimeter < 0) {
+        aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
     } else {
-        startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
     }
 }
 
@@ -567,7 +580,7 @@ void MecanumWheelCarPWMMotorControl::startRotate(int aRotationDegrees, turn_dire
         tTurnSpeed = DEFAULT_START_SPEED_PWM;
     }
     // Use direction set by setDirection() above
-    rightCarMotor.startGoDistanceMillimeter(tTurnSpeed, tDistanceMillimeter, rightCarMotor.getDirection());
+    rightCarMotor.startGoDistanceMillimeterWithSpeed(tTurnSpeed, tDistanceMillimeter, rightCarMotor.getDirection());
 #if defined(LOCAL_DEBUG)
     Serial.print(F("RotationDegrees="));
     Serial.print(aRotationDegrees);
diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h
index 08f81b6..1a2cd67 100644
--- a/src/PWMDcMotor.h
+++ b/src/PWMDcMotor.h
@@ -14,7 +14,7 @@
  * With encoder: - distance is measured by Encoder.
  *
  *
- *  Copyright (C) 2019-2022  Armin Joachimsmeyer
+ *  Copyright (C) 2019-2024  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
  *  This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl.
@@ -378,7 +378,11 @@ class PWMDcMotor {
     void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // Signed distance
     void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection);
     void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+            uint8_t aRequestedDirection) __attribute__ ((deprecated ("Renamed to startGoDistanceMillimeterWithSpeed().")));
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter); // Signed distance
+    void startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
             uint8_t aRequestedDirection);
+
     uint32_t convertMillimeterToMillis(uint8_t aSpeedPWM, unsigned int aRequestedDistanceMillimeter);
     unsigned int convertMillisToMillimeter(uint8_t aSpeedPWM, unsigned int aMillis);
     unsigned int convertMillisToCentimeterFor2Volt(unsigned int aMillis);
@@ -462,6 +466,11 @@ class PWMDcMotor {
 };
 
 /*
+ * Version 2.2.0 - 11/2024
+ * - Added 2 functions startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, ...).
+ * - ESP32 core 3.x support.
+ * - Improved examples, especially follower examples.
+ *
  * Version 2.1.0 - 09/2023
  * - Added convertMillimeterToMillis() etc.
  * - Added Variable computedMillisOfMotorForDistance.
diff --git a/src/PWMDcMotor.hpp b/src/PWMDcMotor.hpp
index f18bb72..256855d 100644
--- a/src/PWMDcMotor.hpp
+++ b/src/PWMDcMotor.hpp
@@ -19,8 +19,7 @@
  * A fixed speed compensation PWM value to be subtracted can be specified.
  *
  *
- *  Created on: 12.05.2019
- *  Copyright (C) 2019-2022  Armin Joachimsmeyer
+ *  Copyright (C) 2019-2024  Armin Joachimsmeyer
  *  armin.joachimsmeyer@gmail.com
  *
  *  This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl.
@@ -807,7 +806,7 @@ void PWMDcMotor::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter,
  */
 void PWMDcMotor::goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
         uint8_t aRequestedDirection) {
-    startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
 #if defined(DO_NOT_SUPPORT_RAMP)
     delay(computedMillisOfMotorForDistance);
 #else
@@ -819,7 +818,7 @@ void PWMDcMotor::goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int a
 }
 
 void PWMDcMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) {
-    startGoDistanceMillimeter(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
+    startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, aRequestedDirection);
 }
 
 /*
@@ -828,9 +827,18 @@ void PWMDcMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillim
 void PWMDcMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) {
     if (aRequestedDistanceMillimeter < 0) {
         aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
-        startGoDistanceMillimeter(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+        startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
     } else {
-        startGoDistanceMillimeter(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+        startGoDistanceMillimeterWithSpeed(DriveSpeedPWMFor2Volt, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
+    }
+}
+
+void PWMDcMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, int aRequestedDistanceMillimeter) {
+    if (aRequestedDistanceMillimeter < 0) {
+        aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter;
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD);
+    } else {
+        startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD);
     }
 }
 
@@ -848,10 +856,15 @@ unsigned int PWMDcMotor::convertMillisToCentimeterFor2Volt(unsigned int aMillis)
     return aMillis / MillisPerCentimeter;
 }
 
+void PWMDcMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+        uint8_t aRequestedDirection) {
+    startGoDistanceMillimeterWithSpeed(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection);
+}
+
 /*
  * If motor is already running, just update speed and new stop time
  */
-void PWMDcMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
+void PWMDcMotor::startGoDistanceMillimeterWithSpeed(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter,
         uint8_t aRequestedDirection) {
     if (aRequestedDistanceMillimeter == 0) {
         stop(STOP_MODE_BRAKE); // In case motor was running