Fix Make PWMMotorMediator physically realistic #88.

.github/workflows/ci.yml

@@ -11,7 +11,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ windows-latest, macos-latest, ubuntu-latest ]
+        os: [ windows-latest, macos-13, ubuntu-latest ]
     runs-on: ${{ matrix.os }}
 
     steps:

example/src/main/java/frc/robot/Robot.java

@@ -1,8 +1,8 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
+/* the project. */
 /*----------------------------------------------------------------------------*/
 
 package frc.robot;
@@ -15,10 +15,9 @@
 import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
 
 /**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the manifest file in the resource
  * directory.
  */
 public class Robot extends TimedRobot {
@@ -78,7 +77,7 @@ public void autonomousPeriodic() {
         // Drive for 2 seconds
         if (m_timer.get() < 2.0) {
             m_robotDrive.arcadeDrive(0.5, 0.0); // drive forwards half speed
-            m_elevator.set(0.1);
+            m_elevator.set(0.3);
         } else {
             m_robotDrive.stopMotor(); // stop robot
         }

example/src/systemTest/java/frc/robot/SystemTestRobot.java

@@ -37,12 +37,12 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception {
                         0.2, "Robot close to target position");
                 assertEquals(0.0,
                         s.position("ELEVATOR")
-                                .getDistance(new Translation3d(2.6, 0, 0.9)),
+                                .getDistance(new Translation3d(2.6, 0, 1.244)),
                         0.2, "Elevator close to target position");
                 assertEquals(0.0,
                         s.velocity("ROBOT")
                                 .getDistance(new Translation3d(0, 0, 0)),
-                        0.01, "Robot close to target velocity");
+                        0.05, "Robot close to target velocity");
                 assertEquals(0.0,
                         Units.radiansToDegrees(
                                 s.angularVelocity("ROBOT").getAngle()),
@@ -61,7 +61,7 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception {
                 s.enableTeleop();
                 DriverStationSim.setJoystickAxisCount(0, 2);
                 DriverStationSim.setJoystickAxis(0, 1, 0.0);
-                DriverStationSim.setJoystickAxis(0, 0, 0.15);
+                DriverStationSim.setJoystickAxis(0, 0, 0.6);
                 DriverStationSim.notifyNewData();
             }).everyStep(s -> {
                 var yawDegrees =
@@ -72,27 +72,28 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception {
                     DriverStationSim.notifyNewData();
                     stoppedTryingToTurn = true;
                 }
-            }).atSec(1.0, s -> {
+            }).atSec(0.3, s -> {
                 assertEquals(0.0,
                         s.velocity("ROBOT")
                                 .getDistance(new Translation3d(0, 0, 0)),
                         0.1, "Robot close to target velocity");
-                assertEquals(19.0,
-                        Units.radiansToDegrees(
+                assertEquals(63.9, Units.radiansToDegrees(
                                 s.angularVelocity("ROBOT").getAngle()),
-                        1, "Robot close to target angular velocity");
+                        5.0, "Robot close to target angular velocity");
                 assertEquals(0.0,
                         new Translation3d(s.angularVelocity("ROBOT").getAxis())
                                 .getDistance(new Translation3d(0, 0, 1)),
                         0.1, "Robot close to target angular velocity axis");
-            }).atSec(4.0, s -> {
+            }).atSec(1.5, s -> {
+                // Note: Large tolerance because turning at a high speed means we can overshoot
+                // significantly in 1 step.
                 assertEquals(45.0,
-                        Units.radiansToDegrees(s.rotation("ROBOT").getZ()), 2.0,
-                        "Robot close to target rotation");
+                        Units.radiansToDegrees(s.rotation("ROBOT").getZ()),
+                        10.0, "Robot close to target rotation");
                 assertEquals(0.0,
                         s.velocity("ROBOT")
                                 .getDistance(new Translation3d(0, 0, 0)),
-                        0.01, "Robot close to target velocity");
+                        0.1, "Robot close to target velocity");
                 assertEquals(0.0,
                         Units.radiansToDegrees(
                                 s.angularVelocity("ROBOT").getAngle()),

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

@@ -3,6 +3,7 @@
 import org.carlmontrobotics.wpiws.devices.PWMSim;
 
 import com.cyberbotics.webots.controller.Motor;
+import com.cyberbotics.webots.controller.Node;
 
 import edu.wpi.first.math.system.plant.DCMotor;
 
@@ -35,11 +36,31 @@ public PWMMotorMediator(Motor motor, PWMSim simDevice, DCMotor motorConstants, d
         // Use velocity control
         motor.setPosition(Double.POSITIVE_INFINITY);
 
-        // Disable braking
-        if(motor.getBrake() != null) motor.getBrake().setDampingConstant(0);
+        if (motor.getBrake() != null) {
+            // gearing^2*k_T/(R*k_v)
+            double dampingConstant = gearing * gearing
+                    * motorConstants.KtNMPerAmp / (motorConstants.rOhms
+                            * motorConstants.KvRadPerSecPerVolt);
+            motor.getBrake().setDampingConstant(dampingConstant);
+        }
 
-        motorDevice.registerSpeedCallback((deviceName, speed) -> {
-            double velocity = speed * motorConstants.freeSpeedRadPerSec;
+        motorDevice.registerSpeedCallback((deviceName, currentOutput) -> {
+            double velocity = currentOutput * motorConstants.freeSpeedRadPerSec;
+            double maxTorque = gearing * motorConstants.stallTorqueNewtonMeters;
+            switch (motor.getNodeType()) {
+                case Node.ROTATIONAL_MOTOR:
+                    motor.setAvailableTorque(
+                            Math.abs(currentOutput) * maxTorque);
+                    break;
+                case Node.LINEAR_MOTOR:
+                    motor.setAvailableForce(
+                            Math.abs(currentOutput) * 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);
         }, true);
     }

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

@@ -0,0 +1,18 @@
+#VRML_SIM R2023b utf8
+# license: MIT
+# license url: https://github.com/DeepBlueRobotics/DeepBlueSim/blob/master/LICENSE.md
+# template language: javascript
+# tags: nonDeterministic
+
+# A Brake with a unique name to workaround https://github.com/cyberbotics/webots/issues/6578
+
+PROTO DBSBrake [
+]
+{
+    %<
+        let name = "DBSBrake " + context.id;
+    >%
+    Brake {
+        name "%<= name >%"
+    }
+}

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

@@ -14,12 +14,14 @@ EXTERNPROTO "SparkMaxAbsoluteEncoder.proto"
 EXTERNPROTO "SparkMaxAlternateEncoder.proto"
 EXTERNPROTO "SparkMaxAnalogSensor.proto"
 
+EXTERNPROTO "DBSBrake.proto"
+
 PROTO PoweredHingeJoint [
     field MFNode{
         DBSRotationalMotorBase{}, PreconfiguredDBSRotationalMotor{}
         CANCoder{}, DBSQuadratureEncoder{}, REVBuiltinEncoder{}, SparkMaxAbsoluteEncoder{}, SparkMaxAlternateEncoder{}, SparkMaxAnalogSensor{},
-        Brake{}
-    } devices [PreconfiguredDBSRotationalMotor{} REVBuiltinEncoder{} Brake{}]
+        DBSBrake{}
+    } devices [PreconfiguredDBSRotationalMotor{} REVBuiltinEncoder{} DBSBrake{}]
     field SFNode{Solid{}} endPoint Solid{}
     field SFNode{HingeJointParameters{}} jointParameters HingeJointParameters{}
 ]
@@ -32,15 +34,15 @@ PROTO PoweredHingeJoint [
         let encoderDevice = null;
         for (let i = 0; i < devices.length; i++) {
             let node_name = devices[i].node_name;
-            if (node_name === "Brake") {
+            if (node_name.endsWith("Brake")) {
                 hasBrake = true;
             } else if (node_name.endsWith("Motor")) {
                 motorDevice = devices[i];
             } else if (node_name.endsWith("Encoder")) {
                 encoderDevice = devices[i];
             }
         }
-        assert(hasBrake, "PoweredHingeJoint is missing the required Brake device!");
+        assert(hasBrake, "PoweredHingeJoint is missing the required DBSBrake device!");
         assert(motorDevice !== null, "PoweredHingeJoint is missing the required motor device!");
         let motorType = motorDevice?.node_name;
         let controllerType = motorDevice?.fields.controllerType.value;

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

@@ -14,12 +14,14 @@ EXTERNPROTO "SparkMaxAbsoluteEncoder.proto"
 EXTERNPROTO "SparkMaxAlternateEncoder.proto"
 EXTERNPROTO "SparkMaxAnalogSensor.proto"
 
+EXTERNPROTO "DBSBrake.proto"
+
 PROTO PoweredSliderJoint [
     field MFNode{
         DBSLinearMotorBase{}, PreconfiguredDBSLinearMotor{}
         CANCoder{}, DBSQuadratureEncoder{}, REVBuiltinEncoder{}, SparkMaxAbsoluteEncoder{}, SparkMaxAlternateEncoder{}, SparkMaxAnalogSensor{},
-        Brake{}
-    } devices [PreconfiguredDBSLinearMotor{} REVBuiltinEncoder{} Brake{}]
+        DBSBrake{}
+    } devices [PreconfiguredDBSLinearMotor{} REVBuiltinEncoder{} DBSBrake{}]
     field SFNode{Solid{}} endPoint Solid{}
     field SFNode{JointParameters{}} jointParameters JointParameters{}
 ]
@@ -32,15 +34,15 @@ PROTO PoweredSliderJoint [
         let encoderDevice = null;
         for (let i = 0; i < devices.length; i++) {
             let node_name = devices[i].node_name;
-            if (node_name === "Brake") {
+            if (node_name.endsWith("Brake")) {
                 hasBrake = true;
             } else if (node_name.endsWith("Motor")) {
                 motorDevice = devices[i];
             } else if (node_name.endsWith("Encoder")) {
                 encoderDevice = devices[i];
             }
         }
-        assert(hasBrake, "PoweredSliderJoint is missing the required Brake device!");
+        assert(hasBrake, "PoweredSliderJoint is missing the required DBSBrake device!");
         assert(motorDevice !== null, "PoweredSliderJoint is missing the required motor device!");
         let motorType = motorDevice?.node_name;
         let controllerType = motorDevice?.fields.controllerType.value;