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

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

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