   motorSpeed = map(
     speedInPercentage,
     0, 100,
-    0, 255
+    0, maxMotorSpeed
   );
   
   Serial.print(String("Set speed to ") + motorSpeed + " ("+ speedInPercentage +"%)\n");

{
  int currentMotorSpeedInPercentage = map(
    motorSpeed,
    0, 255,
    0, 100
  );
  

  if (safetyDriveMode) {
    int checkDistanceInterval = map(
      motorSpeed,
      0, maxMotorSpeed,
      500, 0
    );
    
    int distanceToStop = map(
      motorSpeed,
      0, maxMotorSpeed,
      15, 50
    );
    
