Merge pull request #112 from DeepBlueRobotics/fix-unrealistic-CANMoto…

…rMediator

Fix-unrealistic-CANMotorMediator

plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/SimRegisterer.java

@@ -212,8 +212,10 @@ public static void connectMotor(Motor device) {
         if(controllerType.equals("PWM")) {
             new PWMMotorMediator(device, new PWMSim(Integer.toString(port)), motorConstants, gearing, inverted);
         } else {
-            String simDeviceName = "CANMotor:CAN" + controllerType.replaceAll("\\s", "") + "[" + port + "]";
-            new CANMotorMediator(device, new CANMotorSim(simDeviceName, "SimDevice"), motorConstants, gearing, inverted);
+            String simDeviceName = "CAN" + controllerType.replaceAll("\\s", "")
+                    + "[" + port + "]";
+            new CANMotorMediator(device, new CANMotorSim(simDeviceName),
+                    motorConstants, gearing, inverted);
         }
     }
 

plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java

@@ -248,7 +248,8 @@ public static void init(Supervisor robot, int basicTimeStep) {
                             // Unpause if necessary
                             updateUsersSimulationSpeed(robot);
                             robot.simulationSetMode(usersSimulationSpeed);
-                            boolean isDone = (robot.step(basicTimeStep) == -1);
+                            boolean isDone = (stepSimulation(robot,
+                                    basicTimeStep) == -1);
 
                             simTimeSec = robot.getTime();
                             LOG.log(Level.DEBUG, "Sending simTimeSec of {0}",
@@ -338,6 +339,11 @@ private static void addNetworkTablesLogger() {
         }
     }
 
+    private static int stepSimulation(Supervisor robot, int basicTimeStep) {
+        Simulation.runPeriodicMethods();
+        return robot.step(basicTimeStep);
+    }
+
     /**
      * Processes messages from the robot code while advancing the simulation and running periodic
      * methods in time with the code.
@@ -352,7 +358,6 @@ public static void runUntilTermination(Supervisor robot, int basicTimeStep)
         // Process events until simulation finishes
         while (isDoneFuture.getNow(false).booleanValue() == false) {
             try {
-                Simulation.runPeriodicMethods();
                 // Process any pending user interface events.
                 if (robot.step(0) == -1) {
                     break;
@@ -379,12 +384,12 @@ public static void runUntilTermination(Supervisor robot, int basicTimeStep)
                     // The robot code isn't going to tell us when to step the simulation and the
                     // user has unpaused it.
                     LOG.log(Level.DEBUG,
-                            "Simulation is not paused and no robot code in control. Calling robot.step(basicTimeStep)");
+                            "Simulation is not paused and no robot code in control. Stepping simulation.");
 
-                    if (robot.step(basicTimeStep) == -1) {
+                    if (stepSimulation(robot, basicTimeStep) == -1) {
                         break;
                     }
-                    LOG.log(Level.DEBUG, "robot.step(basicTimeStep) returned");
+                    LOG.log(Level.DEBUG, "Step finished.");
                 } else {
                     // The simulation is paused and robot code isn't in control so wait a beat
                     // before checking again (so we don't suck up all the CPU)

plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/mediators/CANMotorMediator.java

@@ -24,10 +24,14 @@ public class CANMotorMediator implements Runnable {
     public final DCMotor motorConstants;
     public final CANMotorSim motorDevice;
     public final Brake brake;
+    public final PositionSensor positionSensor;
 
     private double requestedOutput = 0;
+    private double actualOutput = 0;
     private boolean brakeMode = true;
     private double neutralDeadband = 0.04;
+    private double dampingConstant = 0.0;
+    private double lastPosRads = Double.NaN;
 
     /**
      * Creates a new CANMotorMediator
@@ -56,63 +60,110 @@ public CANMotorMediator(Motor motor, CANMotorSim simDevice, DCMotor motorConstan
             }
         }
 
+        // Bus voltage is not motor specific. It would need to be simulated by a simulated PDP/PDH.
+        // Since we don't simulate it, we set it to 0.0 to be compliant with the spec.
+        motorDevice.setBusvoltage(0.0);
+
+        // We simulate motor current (see the run() method below), not supply current, so we set
+        // supply current to 0.0 to be compliant with the spec.
+        motorDevice.setSupplycurrent(0.0);
+
         this.brake = motor.getBrake();
         if(brake == null) {
             System.err.println(String.format("WARNING: Brake not found for motor: \"%s\", braking will be disabled!", motor.getName()));
         }
 
+        this.positionSensor = motor.getPositionSensor();
+        if (positionSensor == null) {
+            System.err.println(String.format(
+                    "WARNING: Encoder not found for motor: \"%s\", current will not be reported!",
+                    motor.getName()));
+        }
+
         // Use velocity control
         motor.setPosition(Double.POSITIVE_INFINITY);
-        if (brake != null)
-            brake.setDampingConstant(
-                    motorConstants.stallTorqueNewtonMeters * gearing);
+        if (brake != null) {
+            // gearing^2*k_T/(R*k_v)
+            dampingConstant = gearing * gearing * motorConstants.KtNMPerAmp
+                    / (motorConstants.rOhms
+                            * motorConstants.KvRadPerSecPerVolt);
+            brake.setDampingConstant(dampingConstant);
 
+        }
         motorDevice.registerBrakemodeCallback((name, enabled) -> {
             brakeMode = enabled;
+            updateMotorAndBrakeSettings();
         }, true);
         motorDevice.registerNeutraldeadbandCallback((name, deadband) -> {
             neutralDeadband = deadband;
+            updateMotorAndBrakeSettings();
         }, true);
         motorDevice.registerPercentoutputCallback((name, percentOutput) -> {
             requestedOutput = percentOutput;
+            updateMotorAndBrakeSettings();
         }, true);
 
         Simulation.registerPeriodicMethod(this);
     }
 
-    @Override
-    public void run() {
-        // Apply the speed changes periodically so that changes to variables (ie brake mode) don't require a speed update to be applied
-        // Copy requested output so that decreasing the neutral deadband can take effect without a speed update
-        double currentOutput = requestedOutput;
-        if (Math.abs(currentOutput) < neutralDeadband && brake != null) {
-            currentOutput = 0;
-            brake.setDampingConstant(brakeMode ? motorConstants.stallTorqueNewtonMeters * gearing : 0);
-        } else {
-            brake.setDampingConstant(0);
+    private void updateMotorAndBrakeSettings() {
+        actualOutput = requestedOutput;
+        if (Math.abs(actualOutput) < neutralDeadband) {
+            actualOutput = 0.0;
         }
-
-        double velocity = currentOutput * motorConstants.freeSpeedRadPerSec;
-        motor.setVelocity((inverted ? -1 : 1) * velocity / gearing);
-
-        double currentDraw = motorConstants.getCurrent(velocity, currentOutput * motorConstants.nominalVoltageVolts);
+        if (brake != null) {
+            if (!brakeMode && actualOutput == 0.0) {
+                brake.setDampingConstant(0);
+            } else {
+                brake.setDampingConstant(dampingConstant);
+            }
+        }
+        double velocity = actualOutput * motorConstants.freeSpeedRadPerSec;
+        double maxTorque = gearing * motorConstants.stallTorqueNewtonMeters;
         switch (motor.getNodeType()) {
             case Node.ROTATIONAL_MOTOR:
                 motor.setAvailableTorque(
-                        motorConstants.getTorque(currentDraw) * gearing);
+                        Math.abs(actualOutput) * maxTorque);
                 break;
             case Node.LINEAR_MOTOR:
                 motor.setAvailableForce(
-                        motorConstants.getTorque(currentDraw) * gearing);
+                        Math.abs(actualOutput) * maxTorque);
                 break;
             default:
                 throw new UnsupportedOperationException(
                         "Unsupported motor node type %d. Must be either a RotationalMotor or a LinearMotor: "
                                 .formatted(motor.getNodeType()));
         }
+        motor.setVelocity((inverted ? -1 : 1) * velocity / gearing);
+    }
+
 
-        motorDevice.setMotorcurrent(currentDraw);
-        motorDevice.setBusvoltage(12);
+    @Override
+    public void run() {
+        if (positionSensor == null) {
+            return;
+        }
+        double samplingPeriodMs = positionSensor.getSamplingPeriod();
+        if (samplingPeriodMs == 0) {
+            // Sensor was not enabled.
+            return;
+        }
+        double posRads = positionSensor.getValue();
+        if (!Double.isNaN(lastPosRads)) {
+            double velRadPerSec = (posRads - lastPosRads)
+                    / (samplingPeriodMs / 1000.0) * gearing;
+            // i = (V-w/k_v)/R
+            double currentDraw =
+                    (actualOutput * motorConstants.nominalVoltageVolts
+                            - velRadPerSec / motorConstants.KvRadPerSecPerVolt)
+                            / motorConstants.rOhms;
+            // Arguably, we should be able to set a negative motor current, but the current spec
+            // says it needs to be >= 0.0
+            // TODO: Create a wpilib issue to address this.
+            currentDraw = Math.abs(currentDraw);
+            motorDevice.setMotorcurrent(currentDraw);
+        }
+        lastPosRads = posRads;
     }
 
 }

plugin/controller/src/webotsFolder/dist/protos/deepbluesim/DBSBuiltinEncoderBase.proto

@@ -2,22 +2,22 @@
 # license: MIT
 # license url: https://github.com/DeepBlueRobotics/DeepBlueSim/blob/master/LICENSE.md
 # template language: javascript
-# tags: hidden, no3dView
+# tags: hidden, no3dView, nonDeterministic
 # A base proto for robot encoders built into motors
 
+# An Encoder with a unique name to workaround https://github.com/cyberbotics/webots/issues/6578
+
 EXTERNPROTO "DBSEncoderBase.proto"
 
 PROTO DBSBuiltinEncoderBase [
-    field SFString name ""
-    unconnectedField SFBool absolute FALSE
-    unconnectedField SFFloat absoluteOffsetDeg 0
-    unconnectedField SFBool inverted FALSE
-    unconnectedField SFInt32 CPR 4096
     field SFFloat noiseStdDevRad 0
 ]
 {
+    %<
+        let name = "DBSEncoder " + context.id;
+    >%
     DBSEncoderBase {
-        name IS name
+        name "%<= name >%"
         noiseStdDevRad IS noiseStdDevRad
     }
 }

plugin/controller/src/webotsFolder/dist/protos/deepbluesim/DBSStandaloneEncoderBase.proto

@@ -2,7 +2,7 @@
 # license: MIT
 # license url: https://github.com/DeepBlueRobotics/DeepBlueSim/blob/master/LICENSE.md
 # template language: javascript
-# tags: hidden, no3dView
+# tags: hidden, no3dView, nonDeterministic
 # A base proto for standalone robot encoders
 
 EXTERNPROTO "DBSEncoderBase.proto"
@@ -17,7 +17,8 @@ PROTO DBSStandaloneEncoderBase [
 ]
 {
     DBSEncoderBase {
-        name %<= '"' + ["DBSim_Encoder", fields.location.value === "Motor Shaft", fields.absolute.value, fields.absoluteOffsetDeg.value, fields.inverted.value, fields.CPR.value].join('_') + '"' >%
+        # Name includes unique context.id to workaround https://github.com/cyberbotics/webots/issues/6578
+        name %<= '"' + ["DBSim_Encoder", fields.location.value === "Motor Shaft", fields.absolute.value, fields.absoluteOffsetDeg.value, fields.inverted.value, fields.CPR.value, context.id].join('_') + '"' >%
         noiseStdDevRad IS noiseStdDevRad
     }
 }

plugin/controller/src/webotsFolder/dist/worlds/PWMMotorController.wbt → plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt

@@ -6,6 +6,7 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/project
 EXTERNPROTO "../protos/deepbluesim/PoweredHingeJoint.proto"
 EXTERNPROTO "../protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto"
 EXTERNPROTO "../protos/deepbluesim/DBSBrake.proto"
+EXTERNPROTO "../protos/deepbluesim/REVBuiltinEncoder.proto"
 
 WorldInfo {
 }
@@ -23,7 +24,7 @@ RectangleArena {
 DEF ROBOT Robot {
   translation 0 0 0.06
   children [
-    DEF BASE Solid {
+    DEF PWM_BASE Solid {
       children [
         PoweredHingeJoint {
           devices [
@@ -36,7 +37,7 @@ DEF ROBOT Robot {
               gearing 97.3333
             }
           ]
-          endPoint DEF SHAFT Solid {
+          endPoint DEF PWM_SHAFT Solid {
             children [
               Shape {
                 appearance PBRAppearance {
@@ -59,6 +60,44 @@ DEF ROBOT Robot {
         }
       ]
     }
+    DEF CAN_BASE Solid {
+      translation 0 2 0
+      children [
+        PoweredHingeJoint {
+          devices [
+            REVBuiltinEncoder {
+            }
+            DBSBrake {
+            }
+            PreconfiguredDBSRotationalMotor {
+              port 2
+              gearing 94.6
+            }
+          ]
+          endPoint DEF CAN_SHAFT Solid {
+            children [
+              Shape {
+                appearance PBRAppearance {
+                }
+                geometry Box {
+                  size 1 1 0.392
+                }
+              }
+            ]
+            boundingObject Cylinder {
+              height 0.392
+              radius 0.5
+            }
+            physics Physics {
+            }
+          }
+          jointParameters HingeJointParameters {
+            axis 0 0 1
+          }
+        }
+      ]
+      name "solid(1)"
+    }
   ]
   controller "DeepBlueSim"
   supervisor TRUE

plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java

@@ -737,10 +737,8 @@ public void run() throws InstantiationException, IllegalAccessException,
                     robot.getPeriod(), -robot.getPeriod());
 
             this.endNotifier = endNotifier;
-            // HAL must be initialized or SimDeviceSim.resetData() will crash and SmartDashboard
-            // might not work.
+            // HAL must be initialized or SmartDashboard might not work.
             HAL.initialize(500, 0);
-            SimDeviceSim.resetData();
             SimHooks.restartTiming();
             SimHooks.resumeTiming();
             isRobotCodeRunning = true;
@@ -760,7 +758,6 @@ public void run() throws InstantiationException, IllegalAccessException,
         } finally {
             LOG.log(Level.DEBUG, "startCompetition() returned");
             isRobotCodeRunning = false;
-            SimDeviceSim.resetData();
             // HAL.shutdown();
             LOG.log(Level.DEBUG, "run() returning");
         }

tests/build.gradle

@@ -43,6 +43,7 @@ deploy {
 
 repositories {
     mavenCentral()
+    maven { url 'https://jitpack.io' } // for lib199
 }
 
 def deployArtifact = deploy.targets.roborio.artifacts.frcJava
@@ -58,6 +59,7 @@ def includeDesktopSupport = true
 dependencies {
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
+    implementation "com.github.deepbluerobotics:lib199:5785769c553ade2a9e53640896efe3cd6ffdce45"
 
     roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
     roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)