Fix Make PWMMotorMediator physically realistic #88.

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(62.5, Units.radiansToDegrees(
                                 s.angularVelocity("ROBOT").getAngle()),
-                        1, "Robot close to target angular velocity");
+                        2.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.0, 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

@@ -35,11 +35,16 @@ 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) {
+            double dampingConstant = motorConstants.stallTorqueNewtonMeters
+                    * gearing / (motorConstants.freeSpeedRadPerSec / gearing);
+            motor.getBrake().setDampingConstant(dampingConstant);
+        }
 
         motorDevice.registerSpeedCallback((deviceName, speed) -> {
             double velocity = speed * motorConstants.freeSpeedRadPerSec;
+            motor.setAvailableTorque(Math.abs(speed)
+                    * motorConstants.stallTorqueNewtonMeters * gearing);
             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;

plugin/controller/src/webotsFolder/dist/worlds/DBSExample.wbt

@@ -8,12 +8,13 @@ EXTERNPROTO "../protos/deepbluesim/PoweredHingeJoint.proto"
 EXTERNPROTO "../protos/deepbluesim/PoweredSliderJoint.proto"
 EXTERNPROTO "../protos/deepbluesim/PreconfiguredDBSLinearMotor.proto"
 EXTERNPROTO "../protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto"
+EXTERNPROTO "../protos/deepbluesim/DBSBrake.proto"
 
 WorldInfo {
 }
 Viewpoint {
   orientation -0.17483905906784158 -0.1007392188381911 0.9794298919331301 4.169165907224898
-  position 1.742990312035277 3.046841469870379 1.4226819047141128
+  position 5.451052061691213 7.144320599246478 3.0635976233453825
 }
 DEF Field Group {
   children [
@@ -30,15 +31,13 @@ TexturedBackgroundLight {
 }
 DEF ROBOT Robot {
   translation -8.000411853504155e-08 0.06914638371366336 0.0592928837522033
-  rotation 0 0 1 0
   children [
     Solid {
       children [
         PoweredSliderJoint {
-          jointParameters JointParameters {
-            maxStop 1
-          }
           devices [
+            DBSBrake {
+            }
             PreconfiguredDBSLinearMotor {
               motorType "MiniCIM"
               controllerType "PWM"
@@ -50,8 +49,6 @@ DEF ROBOT Robot {
               channelA 8
               channelB 9
             }
-            Brake {
-            }
           ]
           endPoint DEF ELEVATOR Solid {
             translation 0 0 0.17
@@ -69,9 +66,14 @@ DEF ROBOT Robot {
             physics Physics {
             }
           }
+          jointParameters JointParameters {
+            maxStop 1
+          }
         }
         PoweredHingeJoint {
           devices [
+            DBSBrake {
+            }
             PreconfiguredDBSRotationalMotor {
               motorType "MiniCIM"
               controllerType "PWM"
@@ -80,8 +82,6 @@ DEF ROBOT Robot {
             }
             DBSQuadratureEncoder {
             }
-            Brake {
-            }
           ]
           endPoint Solid {
             translation 0.16699999999999576 -0.23200000000253404 -2.4730714204904136e-06
@@ -103,13 +103,14 @@ DEF ROBOT Robot {
             }
           }
           jointParameters HingeJointParameters {
-            position 0
             axis 0 -1 0
             anchor 0.167 -0.232 0
           }
         }
         PoweredHingeJoint {
           devices [
+            DBSBrake {
+            }
             PreconfiguredDBSRotationalMotor {
               motorType "MiniCIM"
               controllerType "PWM"
@@ -120,8 +121,6 @@ DEF ROBOT Robot {
               channelA 2
               channelB 3
             }
-            Brake {
-            }
           ]
           endPoint Solid {
             translation -0.1670000000000024 -0.232000000002534 -2.4730714204834747e-06
@@ -143,13 +142,14 @@ DEF ROBOT Robot {
             }
           }
           jointParameters HingeJointParameters {
-            position 0
             axis 0 -1 0
             anchor -0.167 -0.232 0
           }
         }
         PoweredHingeJoint {
           devices [
+            DBSBrake {
+            }
             DBSQuadratureEncoder {
               channelA 4
               channelB 5
@@ -160,8 +160,6 @@ DEF ROBOT Robot {
               port 1
               gearing 6.9973
             }
-            Brake {
-            }
           ]
           endPoint Solid {
             translation 0.1670000000000422 0.231999999997466 -2.47307141028677e-06
@@ -190,6 +188,8 @@ DEF ROBOT Robot {
         }
         PoweredHingeJoint {
           devices [
+            DBSBrake {
+            }
             DBSQuadratureEncoder {
               channelA 6
               channelB 7
@@ -199,8 +199,6 @@ DEF ROBOT Robot {
               controllerType "PWM"
               gearing 6.9973
             }
-            Brake {
-            }
           ]
           endPoint Solid {
             translation -0.1669999999999658 0.23199999999746604 -2.473071410269423e-06
@@ -222,31 +220,12 @@ DEF ROBOT Robot {
             }
           }
           jointParameters HingeJointParameters {
-            position 0
             axis 0 1 0
             anchor -0.167 0.232 0
           }
         }
         Gyro {
         }
-        HingeJoint {
-          jointParameters HingeJointParameters {
-            position 0
-            axis 0 1 0
-            anchor 0.167 -0.232 0
-          }
-          device [
-            PreconfiguredDBSRotationalMotor {
-              motorType "MiniCIM"
-              controllerType "PWM"
-              port 3
-              gearing 6.9973
-            }
-            PositionSensor {
-              name "Front Right Encoder"
-            }
-          ]
-        }
         Shape {
           appearance PBRAppearance {
           }
@@ -261,7 +240,6 @@ DEF ROBOT Robot {
     }
     Pen {
       translation 0 0 0.001
-      rotation 0 0 1 0
       inkColor 1 0 0
       inkDensity 1
       leadSize 0.1
@@ -274,6 +252,4 @@ DEF ROBOT Robot {
   }
   controller "DeepBlueSim"
   supervisor TRUE
-  linearVelocity 0 0 0
-  angularVelocity 0 0 0
 }