action rescaling

android/app/src/main/java/org/openbot/autopilot/AutopilotFragment.java

@@ -297,7 +297,7 @@ protected void processControllerKeyData(String commandType) {
     switch (commandType) {
       case Constants.CMD_DRIVE:
         binding.controllerContainer.controlInfo.setText(
-            String.format(Locale.US, "%.0f,%.0f", vehicle.getLeftSpeed(), vehicle.getRightSpeed()));
+            String.format(Locale.US, "%.2f,%.2f", vehicle.getLeftSpeed(), vehicle.getRightSpeed()));
         break;
 
       case Constants.CMD_DRIVE_MODE:
@@ -403,7 +403,7 @@ protected void handleDriveCommand(Control control) {
         .runOnUiThread(
             () ->
                 binding.controllerContainer.controlInfo.setText(
-                    String.format(Locale.US, "%.0f,%.0f", left, right)));
+                    String.format(Locale.US, "%.2f,%.2f", left, right)));
   }
 
   @Override
@@ -472,7 +472,7 @@ private void setSpeedMode(Enums.SpeedMode speedMode) {
 
       Timber.d("Updating  controlSpeed: %s", speedMode);
       preferencesManager.setSpeedMode(speedMode.getValue());
-      vehicle.setSpeedMultiplier(speedMode.getValue());
+      vehicle.setSpeedFactor(speedMode.getValue());
     }
   }
 

android/app/src/main/java/org/openbot/logging/LogDataUtils.java

@@ -13,10 +13,12 @@ public static Message generateIndicatorMessage(int indicator) {
     return msg;
   }
 
-  public static Message generateControlDataMessage(int left, int right) {
+  public static Message generateControlDataMessage(float left, float right) {
     Message msg = Message.obtain();
-    msg.arg1 = left;
-    msg.arg2 = right;
+    Bundle bundle = new Bundle();
+    bundle.putFloat("left", left);
+    bundle.putFloat("right", right);
+    msg.setData(bundle);
     msg.what = SensorService.MSG_CONTROL;
     return msg;
   }

android/app/src/main/java/org/openbot/logging/LoggerFragment.java

@@ -234,7 +234,7 @@ protected void sendControlToSensorService() {
       try {
         sensorMessenger.send(
             LogDataUtils.generateControlDataMessage(
-                (int) vehicle.getLeftSpeed(), (int) vehicle.getRightSpeed()));
+                vehicle.getControl().getLeft(), vehicle.getControl().getRight()));
       } catch (RemoteException e) {
         e.printStackTrace();
       }
@@ -426,7 +426,7 @@ protected void handleDriveCommand() {
     float left = vehicle.getLeftSpeed();
     float right = vehicle.getRightSpeed();
     binding.controllerContainer.controlInfo.setText(
-        String.format(Locale.US, "%.0f,%.0f", left, right));
+        String.format(Locale.US, "%.2f,%.2f", left, right));
     runInBackground(this::sendControlToSensorService);
   }
 
@@ -446,7 +446,7 @@ private void setSpeedMode(Enums.SpeedMode speedMode) {
 
       Timber.d("Updating  controlSpeed: %s", speedMode);
       preferencesManager.setSpeedMode(speedMode.getValue());
-      vehicle.setSpeedMultiplier(speedMode.getValue());
+      vehicle.setSpeedFactor(speedMode.getValue());
     }
   }
 

android/app/src/main/java/org/openbot/logging/SensorService.java

@@ -410,9 +410,11 @@ public void handleMessage(Message msg) {
           if (inferenceLog != null) appendLog(inferenceLog, frameNumber + "," + inferenceTime);
         } else if (msg.what == MSG_CONTROL) {
           // msg.arg1 and msg.arg2 contain left and right control signals respectively
-          if (ctrlLog != null)
-            appendLog(
-                ctrlLog, SystemClock.elapsedRealtimeNanos() + "," + msg.arg1 + "," + msg.arg2);
+          if (ctrlLog != null) {
+            float left = msg.getData().getFloat("left");
+            float right = msg.getData().getFloat("right");
+            appendLog(ctrlLog, SystemClock.elapsedRealtimeNanos() + "," + left + "," + right);
+          }
         } else if (msg.what == MSG_INDICATOR) {
           // msg.arg1 contains indicator signal
           if (indicatorLog != null)

android/app/src/main/java/org/openbot/objectNav/ObjectNavFragment.java

@@ -346,7 +346,7 @@ protected void processControllerKeyData(String commandType) {
     switch (commandType) {
       case Constants.CMD_DRIVE:
         binding.controllerContainer.controlInfo.setText(
-            String.format(Locale.US, "%.0f,%.0f", vehicle.getLeftSpeed(), vehicle.getRightSpeed()));
+            String.format(Locale.US, "%.2f,%.2f", vehicle.getLeftSpeed(), vehicle.getRightSpeed()));
         break;
 
       case Constants.CMD_NETWORK:
@@ -469,7 +469,7 @@ protected void handleDriveCommand(Control control) {
         .runOnUiThread(
             () ->
                 binding.controllerContainer.controlInfo.setText(
-                    String.format(Locale.US, "%.0f,%.0f", left, right)));
+                    String.format(Locale.US, "%.2f,%.2f", left, right)));
   }
 
   protected Model getModel() {
@@ -538,7 +538,7 @@ private void setSpeedMode(Enums.SpeedMode speedMode) {
 
       Timber.d("Updating  controlSpeed: %s", speedMode);
       preferencesManager.setSpeedMode(speedMode.getValue());
-      vehicle.setSpeedMultiplier(speedMode.getValue());
+      vehicle.setSpeedFactor(speedMode.getValue());
     }
   }
 

android/app/src/main/java/org/openbot/original/CameraActivity.java

@@ -903,7 +903,7 @@ private void setSpeedMode(SpeedMode speedMode) {
       this.speedMode = speedMode;
       preferencesManager.setSpeedMode(speedMode.getValue());
       speedModeSpinner.setSelection(speedMode.ordinal());
-      vehicle.setSpeedMultiplier(speedMode.getValue());
+      vehicle.setSpeedFactor(speedMode.getValue());
     }
   }
 
@@ -1055,10 +1055,9 @@ protected void sendInferenceTimeToSensorService(long frameNumber, long inference
 
   protected void sendControlToSensorService() {
     if (sensorMessenger != null) {
-      Message msg = Message.obtain();
-      msg.arg1 = (int) (vehicle.getLeftSpeed());
-      msg.arg2 = (int) (vehicle.getRightSpeed());
-      msg.what = SensorService.MSG_CONTROL;
+      Message msg =
+          LogDataUtils.generateControlDataMessage(
+              vehicle.getControl().getLeft(), vehicle.getControl().getRight());
       try {
         sensorMessenger.send(msg);
       } catch (RemoteException e) {
@@ -1069,9 +1068,7 @@ protected void sendControlToSensorService() {
 
   protected void sendIndicatorToSensorService() {
     if (sensorMessenger != null) {
-      Message msg = Message.obtain();
-      msg.arg1 = vehicle.getIndicator();
-      msg.what = SensorService.MSG_INDICATOR;
+      Message msg = LogDataUtils.generateIndicatorMessage(vehicle.getIndicator());
       try {
         sensorMessenger.send(msg);
       } catch (RemoteException e) {

android/app/src/main/java/org/openbot/original/DefaultActivity.java

@@ -274,7 +274,7 @@ protected void updateVehicleControl() {
           if (controlValueTextView != null)
             controlValueTextView.setText(
                 String.format(
-                    Locale.US, "%.0f,%.0f", vehicle.getLeftSpeed(), vehicle.getRightSpeed()));
+                    Locale.US, "%.2f,%.2f", vehicle.getLeftSpeed(), vehicle.getRightSpeed()));
         });
   }
 

android/app/src/main/java/org/openbot/robot/FreeRoamFragment.java

@@ -160,7 +160,7 @@ protected void handleDriveCommand() {
     float left = vehicle.getLeftSpeed();
     float right = vehicle.getRightSpeed();
     binding.controllerContainer.controlInfo.setText(
-        String.format(Locale.US, "%.0f,%.0f", left, right));
+        String.format(Locale.US, "%.2f,%.2f", left, right));
 
     binding.speed.speedPercentTo(vehicle.getSpeedPercent());
 
@@ -185,7 +185,7 @@ private void setSpeedMode(SpeedMode speedMode) {
 
       Timber.d("Updating  controlSpeed: %s", speedMode);
       preferencesManager.setSpeedMode(speedMode.getValue());
-      vehicle.setSpeedMultiplier(speedMode.getValue());
+      vehicle.setSpeedFactor(speedMode.getValue());
     }
   }
 

android/app/src/main/java/org/openbot/utils/Enums.java

@@ -82,8 +82,8 @@ public static ControlMode switchControlMode(ControlMode mode) {
   }
 
   public enum SpeedMode {
-    SLOW(128),
-    NORMAL(192),
+    SLOW(85),
+    NORMAL(128),
     FAST(255);
 
     private final int value;

android/app/src/main/java/org/openbot/vehicle/Vehicle.java

@@ -14,7 +14,7 @@ public class Vehicle {
   private boolean noiseEnabled = false;
 
   private int indicator = 0;
-  private int speedMultiplier = 192; // 128,192,255
+  private float speedFactor = 0.5f;
   private Control control = new Control(0, 0);
 
   private final SensorReading batteryVoltage = new SensorReading();
@@ -236,7 +236,7 @@ public float getRotation() {
 
   public int getSpeedPercent() {
     float throttle = (getLeftSpeed() + getRightSpeed()) / 2;
-    return Math.abs((int) (throttle * 100 / 255)); // 255 is the max speed
+    return Math.abs((int) (throttle * 100));
   }
 
   public String getDriveGear() {
@@ -313,12 +313,16 @@ public void stopNoise() {
     sendControl();
   }
 
-  public int getSpeedMultiplier() {
-    return speedMultiplier;
+  public float getSpeedFactor() {
+    return speedFactor;
   }
 
-  public void setSpeedMultiplier(int speedMultiplier) {
-    this.speedMultiplier = speedMultiplier;
+  public void setSpeedFactor(float speedFactor) {
+    this.speedFactor = speedFactor;
+  }
+
+  public void setSpeedFactor(int speedFactor) {
+    this.speedFactor = (float) speedFactor / 255.f;
   }
 
   public int getIndicator() {
@@ -373,25 +377,57 @@ private void sendStringToUsb(String message) {
   }
 
   public float getLeftSpeed() {
-    return control.getLeft() * speedMultiplier;
+    return control.getLeft() * speedFactor;
   }
 
   public float getRightSpeed() {
-    return control.getRight() * speedMultiplier;
+    return control.getRight() * speedFactor;
+  }
+
+  // Remap an action from [-1, -eps] .. [-eps, eps] .. [eps, 1] to [-speedFactor *
+  // maxBatteryVoltage,
+  // -minMotorVoltage] .. 0 .. [minMotorVoltage, factor * speedFactor * maxBatteryVoltage].
+  // The purpose of this remapping is to avoid the dead band of the robot.
+  private float actionToVoltage(float action, float eps, float speedFactor) {
+    assert (speedFactor * maxBatteryVoltage > minMotorVoltage);
+
+    if (Math.abs(action) < eps) {
+      return 0.f;
+    } else {
+      float voltage =
+          minMotorVoltage + (speedFactor * maxBatteryVoltage - minMotorVoltage) * Math.abs(action);
+
+      if (action < 0) {
+        voltage *= -1.f;
+      }
+
+      return voltage;
+    }
+  }
+
+  private int voltageToCommand(float voltage) {
+    float cmd = voltage / maxBatteryVoltage * 255.f;
+
+    if (hasVoltageDivider && batteryVoltage.getReading() != 0) {
+      cmd *= maxBatteryVoltage / batteryVoltage.getReading();
+    }
+
+    cmd = Math.max(-255.f, Math.min(cmd, 255.f));
+
+    return (int) (cmd);
   }
 
   public void sendControl() {
-    int left = (int) (getLeftSpeed());
-    int right = (int) (getRightSpeed());
-    if (noiseEnabled && noise.getDirection() < 0)
-      left =
-          (int)
-              ((control.getLeft() - noise.getValue())
-                  * speedMultiplier); // since noise value does not have speedMultiplier component,
-    // raw control value is used
-    if (noiseEnabled && noise.getDirection() > 0)
-      right = (int) ((control.getRight() - noise.getValue()) * speedMultiplier);
-    sendStringToUsb(String.format(Locale.US, "c%d,%d\n", left, right));
+    float left = control.getLeft();
+    float right = control.getRight();
+    if (noiseEnabled && noise.getDirection() < 0) left -= noise.getValue();
+    if (noiseEnabled && noise.getDirection() > 0) right -= noise.getValue();
+    float leftVoltage = actionToVoltage(left, 0.01f, speedFactor);
+    float rightVoltage = actionToVoltage(right, 0.01f, speedFactor);
+    int leftCmd = voltageToCommand(leftVoltage);
+    int rightCmd = voltageToCommand(rightVoltage);
+
+    sendStringToUsb(String.format(Locale.US, "c%d,%d\n", leftCmd, rightCmd));
   }
 
   protected void sendHeartbeat(int timeout_ms) {

android/app/src/test/java/org/openbot/env/VehicleTest.java

@@ -23,33 +23,33 @@ public void setupVehicle() {
 
   @Test
   public void getRotation() {
-    assertEquals(0, vehicle.getRotation(), 0.0);
+    assertEquals(0, vehicle.getRotation(), 0.01f);
 
-    vehicle.setControl(new Control(0.5f, 1));
-    assertEquals(-60, vehicle.getRotation(), 0.0);
+    vehicle.setControl(new Control(0.5f, 1.0f));
+    assertEquals(-60, vehicle.getRotation(), 0.01f);
 
-    vehicle.setControl(new Control(0f, 1));
-    assertEquals(-180, vehicle.getRotation(), 0.0);
+    vehicle.setControl(new Control(0.0f, 1.0f));
+    assertEquals(-180, vehicle.getRotation(), 0.01f);
   }
 
   @Test
   public void getSpeed() {
-    vehicle.setSpeedMultiplier(Enums.SpeedMode.SLOW.getValue());
-    vehicle.setControl(new Control(-1, -1));
+    vehicle.setSpeedFactor(Enums.SpeedMode.SLOW.getValue());
+    vehicle.setControl(new Control(-1.0f, 1.0f));
 
-    assertEquals(-128, vehicle.getLeftSpeed(), 0.0);
-    assertEquals(-128, vehicle.getRightSpeed(), 0.0);
+    assertEquals(-0.33f, vehicle.getLeftSpeed(), 0.01f);
+    assertEquals(0.33f, vehicle.getRightSpeed(), 0.01f);
 
-    vehicle.setSpeedMultiplier(Enums.SpeedMode.NORMAL.getValue());
-    vehicle.setControl(new Control(-1, -1));
+    vehicle.setSpeedFactor(Enums.SpeedMode.NORMAL.getValue());
+    vehicle.setControl(new Control(1.0f, -1.0f));
 
-    assertEquals(-192, vehicle.getLeftSpeed(), 0.0);
-    assertEquals(-192, vehicle.getRightSpeed(), 0.0);
+    assertEquals(0.5f, vehicle.getLeftSpeed(), 0.01f);
+    assertEquals(-0.5f, vehicle.getRightSpeed(), 0.01f);
 
-    vehicle.setSpeedMultiplier(Enums.SpeedMode.FAST.getValue());
-    vehicle.setControl(new Control(1, 1));
+    vehicle.setSpeedFactor(Enums.SpeedMode.FAST.getValue());
+    vehicle.setControl(new Control(1.0f, 1.0f));
 
-    assertEquals(255, vehicle.getLeftSpeed(), 0.0);
-    assertEquals(255, vehicle.getRightSpeed(), 0.0);
+    assertEquals(1.0f, vehicle.getLeftSpeed(), 0.01f);
+    assertEquals(1.0f, vehicle.getRightSpeed(), 0.01f);
   }
 }