diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 070dec7..0a66e29 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -32,9 +32,11 @@ jobs: - name: Setup Webots id: setupWebots - uses: DeepBlueRobotics/setup-webots@v2.0.3 + uses: DeepBlueRobotics/setup-webots@v2.1 with: - webotsVersion: R2023b + webotsVersion: R2024a + webotsRepository: DeepBlueRobotics/webots + webotsTag: R2024a_DeepBlueSim_2024_07_29 - name: Do the system test uses: ./.github/actions/run-system-test @@ -80,9 +82,11 @@ jobs: - name: Setup Webots id: setupWebots - uses: DeepBlueRobotics/setup-webots@v2 + uses: DeepBlueRobotics/setup-webots@v2.1 with: - webotsVersion: R2023b + webotsVersion: R2024a + webotsRepository: DeepBlueRobotics/webots + webotsTag: R2024a_DeepBlueSim_2024_07_29 - name: Checkout source uses: actions/checkout@v4 diff --git a/README.md b/README.md index 46d6945..5a2f599 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ advantage of the WPILib's WebSockets server desktop simulation extension. ### Installation - 1. Install Webots if you don't already have it installed. + 1. Install the latest [official nightly build of Webots](https://github.com/cyberbotics/webots/releases). 1. Add the DeepBlueSim Gradle plugin to your `build.gradle` by adding the following line in your `plugins` section: ``` diff --git a/example/src/systemTest/java/frc/robot/SystemTestRobot.java b/example/src/systemTest/java/frc/robot/SystemTestRobot.java index 70ebcec..62672a6 100644 --- a/example/src/systemTest/java/frc/robot/SystemTestRobot.java +++ b/example/src/systemTest/java/frc/robot/SystemTestRobot.java @@ -28,7 +28,7 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception { 0.1, "Robot close to target velocity"); assertEquals(0.0, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), + s.angularVelocity("ROBOT").norm()), 1, "Robot close to target angular velocity"); }).atSec(3.0, s -> { assertEquals(0.0, @@ -45,7 +45,7 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception { 0.05, "Robot close to target velocity"); assertEquals(0.0, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), + s.angularVelocity("ROBOT").norm()), 1, "Robot close to target angular velocity"); }).run(); } @@ -78,10 +78,10 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception { .getDistance(new Translation3d(0, 0, 0)), 0.1, "Robot close to target velocity"); assertEquals(63.9, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), - 5.0, "Robot close to target angular velocity"); + s.angularVelocity("ROBOT").norm()), 5.0, + "Robot close to target angular velocity"); assertEquals(0.0, - new Translation3d(s.angularVelocity("ROBOT").getAxis()) + new Translation3d(s.angularVelocity("ROBOT").unit()) .getDistance(new Translation3d(0, 0, 1)), 0.1, "Robot close to target angular velocity axis"); }).atSec(1.5, s -> { @@ -96,7 +96,7 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception { 0.1, "Robot close to target velocity"); assertEquals(0.0, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), + s.angularVelocity("ROBOT").norm()), 1, "Robot close to target angular velocity"); }).run(); } diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHinge2Joint.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHinge2Joint.proto new file mode 100644 index 0000000..abdb1d5 --- /dev/null +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHinge2Joint.proto @@ -0,0 +1,48 @@ +#VRML_SIM R2023b utf8 +# license: MIT +# license url: https://github.com/DeepBlueRobotics/DeepBlueSim/blob/master/LICENSE.md +# template language: javascript +# A HingeJoint powered by a DeepBlueSim-compatible motor + +EXTERNPROTO "DBSRotationalMotorBase.proto" +EXTERNPROTO "PreconfiguredDBSRotationalMotor.proto" + +EXTERNPROTO "CANCoder.proto" +EXTERNPROTO "DBSQuadratureEncoder.proto" +EXTERNPROTO "REVBuiltinEncoder.proto" +EXTERNPROTO "SparkMaxAbsoluteEncoder.proto" +EXTERNPROTO "SparkMaxAlternateEncoder.proto" +EXTERNPROTO "SparkMaxAnalogSensor.proto" + +EXTERNPROTO "DBSBrake.proto" + +PROTO PoweredHinge2Joint [ + field MFNode{ + DBSRotationalMotorBase{}, PreconfiguredDBSRotationalMotor{} + CANCoder{}, DBSQuadratureEncoder{}, REVBuiltinEncoder{}, SparkMaxAbsoluteEncoder{}, SparkMaxAlternateEncoder{}, SparkMaxAnalogSensor{}, + DBSBrake{} + } devices [PreconfiguredDBSRotationalMotor{} REVBuiltinEncoder{} DBSBrake{}] + field MFNode{ + DBSRotationalMotorBase{}, PreconfiguredDBSRotationalMotor{} + CANCoder{}, DBSQuadratureEncoder{}, REVBuiltinEncoder{}, SparkMaxAbsoluteEncoder{}, SparkMaxAlternateEncoder{}, SparkMaxAnalogSensor{}, + DBSBrake{} + } devices2 [PreconfiguredDBSRotationalMotor{} REVBuiltinEncoder{} DBSBrake{}] + field SFNode{Solid{}} endPoint Solid{} + field SFNode{HingeJointParameters{}} jointParameters HingeJointParameters{} + field SFNode{JointParameters{}} jointParameters2 JointParameters{} +] +{ + %< + import {assert} from 'wbutility.js'; + let dbsutility = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsutility.js`)); + dbsutility.assertValidPoweredJointDevices(fields.devices.value, "PoweredHinge2Joint.devices", assert); + dbsutility.assertValidPoweredJointDevices(fields.devices2.value, "PoweredHinge2Joint.devices2", assert); + >% + Hinge2Joint { + device IS devices + device2 IS devices2 + endPoint IS endPoint + jointParameters IS jointParameters + jointParameters2 IS jointParameters2 + } +} diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHingeJoint.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHingeJoint.proto index 0b6e81d..11175f9 100644 --- a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHingeJoint.proto +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHingeJoint.proto @@ -28,31 +28,9 @@ PROTO PoweredHingeJoint [ { %< import {assert} from 'wbutility.js'; - let devices = fields.devices.value; - let hasBrake = false; - let motorDevice = null; - let encoderDevice = null; - for (let i = 0; i < devices.length; i++) { - let node_name = devices[i].node_name; - 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 DBSBrake device!"); - assert(motorDevice !== null, "PoweredHingeJoint is missing the required motor device!"); - let motorType = motorDevice?.node_name; - let controllerType = motorDevice?.fields.controllerType.value; - if (controllerType?.startsWith("Spark")) { - assert(encoderDevice !== null, "PoweredHingeJoint is using a " + controllerType + " motor controller but is missing the required encoder device! " - + "Consider adding a REVBuiltinEncoder."); - } else if (controllerType === "PWM") { - let encoderType = encoderDevice?.node_name; - assert(!encoderType?.endsWith("BuiltinEncoder"), "PoweredHingeJoint is using a " + controllerType + " motor controller, so a built-in encoder is not allowed!"); - } + let dbsutility = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsutility.js`)); + dbsutility.assertValidPoweredJointDevices(fields.devices.value, "PoweredHingeJoint.devices", assert); + >% HingeJoint { device IS devices diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredSliderJoint.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredSliderJoint.proto index 084b127..3a0d536 100644 --- a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredSliderJoint.proto +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredSliderJoint.proto @@ -28,31 +28,8 @@ PROTO PoweredSliderJoint [ { %< import {assert} from 'wbutility.js'; - let devices = fields.devices.value; - let hasBrake = false; - let motorDevice = null; - let encoderDevice = null; - for (let i = 0; i < devices.length; i++) { - let node_name = devices[i].node_name; - 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 DBSBrake device!"); - assert(motorDevice !== null, "PoweredSliderJoint is missing the required motor device!"); - let motorType = motorDevice?.node_name; - let controllerType = motorDevice?.fields.controllerType.value; - if (controllerType?.startsWith("Spark")) { - assert(encoderDevice !== null, "PoweredSliderJoint is using a " + controllerType + " motor controller but is missing the required encoder device! " - + "Consider adding a REVBuiltinEncoder."); - } else if (controllerType === "PWM") { - let encoderType = encoderDevice?.node_name; - assert(!encoderType?.endsWith("BuiltinEncoder"), "PoweredSliderJoint is using a " + controllerType + " motor controller, so a built-in encoder is not allowed!"); - } + let dbsutility = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsutility.js`)); + dbsutility.assertValidPoweredJointDevices(fields.devices.value, "PoweredSliderJoint.devices", assert); >% SliderJoint { device IS devices diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsutility.js b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsutility.js new file mode 100644 index 0000000..df8f15a --- /dev/null +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsutility.js @@ -0,0 +1,28 @@ +({ + assertValidPoweredJointDevices: function (devices, fieldDesc, assert) { + let hasBrake = false; + let motorDevice = null; + let encoderDevice = null; + for (let i = 0; i < devices.length; i++) { + let node_name = devices[i].node_name; + 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, fieldDesc + " is missing the required DBSBrake device!"); + assert(motorDevice !== null, fieldDesc + " is missing the required motor device!"); + let motorType = motorDevice?.node_name; + let controllerType = motorDevice?.fields.controllerType.value; + if (controllerType?.startsWith("Spark")) { + assert(encoderDevice !== null, fieldDesc + " is using a " + controllerType + " motor controller but is missing the required encoder device! " + + "Consider adding a REVBuiltinEncoder."); + } else if (controllerType === "PWM") { + let encoderType = encoderDevice?.node_name; + assert(!encoderType?.endsWith("BuiltinEncoder"), fieldDesc + " is using a " + controllerType + " motor controller, so a built-in encoder is not allowed!"); + } + }, +}) diff --git a/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg b/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg index d588a29..e375227 100644 Binary files a/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg and b/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg differ diff --git a/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt b/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt index 0eed261..0fe5024 100644 --- a/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt +++ b/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2023b utf8 +#VRML_SIM R2024a utf8 EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" @@ -7,12 +7,13 @@ EXTERNPROTO "../protos/deepbluesim/PoweredHingeJoint.proto" EXTERNPROTO "../protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto" EXTERNPROTO "../protos/deepbluesim/DBSBrake.proto" EXTERNPROTO "../protos/deepbluesim/REVBuiltinEncoder.proto" +EXTERNPROTO "../protos/deepbluesim/PoweredHinge2Joint.proto" WorldInfo { } Viewpoint { orientation -0.24402898166491363 0.33039742727224586 0.9117496345814823 1.5165266004776918 - position -0.6005906535473244 -4.740227792969629 3.113219051699273 + position -1.136298742206523 -8.781932059872242 5.731292223532602 } TexturedBackground { } @@ -60,6 +61,78 @@ DEF ROBOT Robot { } ] } + DEF HINGE2JOINT Solid { + translation 0 -2 0 + rotation 0 1 0 0 + children [ + DEF HINGE2JOINT_BASE Pose { + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 1 1 0.1 + } + } + ] + } + PoweredHinge2Joint { + devices [ + PreconfiguredDBSRotationalMotor { + motorType "MiniCIM" + controllerType "PWM" + port 2 + gearing 97.3333 + } + DBSBrake { + } + ] + devices2 [ + PreconfiguredDBSRotationalMotor { + motorType "MiniCIM" + controllerType "PWM" + port 3 + gearing 97.3333 + } + DBSBrake { + } + ] + endPoint DEF HINGE2JOINT_SHAFT Solid { + translation 0 0 2 + children [ + DEF HINGE2JOINT_SHAFT_GEOM Pose { + rotation 0 1 0 1.5701 + children [ + Shape { + appearance PBRAppearance { + } + geometry Cylinder { + height 0.215 + radius 0.5 + subdivision 8 + } + } + ] + } + ] + boundingObject USE HINGE2JOINT_SHAFT_GEOM + physics Physics { + } + } + jointParameters HingeJointParameters { + axis 0 1 0 + anchor 0 0 2 + } + jointParameters2 JointParameters { + axis 1 0 0 + } + } + ] + name "solid(3)" + boundingObject USE HINGE2JOINT_BASE + physics Physics { + } + } DEF NEO_BASE Solid { translation 0 2 0 children [ diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/Watcher.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/Watcher.java index 25d6e49..3432020 100644 --- a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/Watcher.java +++ b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/Watcher.java @@ -9,8 +9,11 @@ import org.carlmontrobotics.libdeepbluesim.internal.NTConstants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoubleArrayTopic; import edu.wpi.first.networktables.NetworkTable; @@ -40,7 +43,8 @@ class Watcher { private volatile CompletableFuture velocityReady = new CompletableFuture<>(); private volatile Translation3d position = null, velocity = null; - private volatile Rotation3d rotation = null, angularVelocity = null; + private volatile Vector angularVelocity = null; + private volatile Rotation3d rotation = null; private final NetworkTableInstance inst; private final String defPath; private final NetworkTable table; @@ -106,8 +110,8 @@ private Watcher(String defPath) { value.valueData.value.getDoubleArray(); velocity = new Translation3d(velAsArray[0], velAsArray[1], velAsArray[2]); - angularVelocity = new Rotation3d(velAsArray[3], - velAsArray[4], velAsArray[5]); + angularVelocity = VecBuilder.fill(velAsArray[3], + velAsArray[4], velAsArray[5]); velocityReady.complete(null); break; } @@ -204,7 +208,7 @@ Rotation3d getRotation() { * * @return the node's angular velocity. */ - Rotation3d getAngularVelocity() { + Vector getAngularVelocity() { if (!inst.isConnected()) { LOG.log(Level.DEBUG, "NetworkTables is not connected, so starting server"); diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java index 0fea0c5..05034fc 100644 --- a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java +++ b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java @@ -31,8 +31,10 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.LogMessage; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent.Kind; @@ -673,11 +675,11 @@ public Rotation3d rotation(String defPath) { /** * Gets the angular velocity of a specific node in the simulated world. - * + * * @param defPath the DEF path to the Webots node to get the position of. * @return the angular velocity of the requested node. */ - public Rotation3d angularVelocity(String defPath) { + public Vector angularVelocity(String defPath) { // ntTransientLogLevel = LogMessage.kDebug4; try { var watcher = Watcher.getByDefPath(defPath); diff --git a/tests/src/main/java/frc/robot/MotorControllerRobot.java b/tests/src/main/java/frc/robot/MotorControllerRobot.java index 736679e..30d3a1a 100644 --- a/tests/src/main/java/frc/robot/MotorControllerRobot.java +++ b/tests/src/main/java/frc/robot/MotorControllerRobot.java @@ -13,6 +13,8 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; +import edu.wpi.first.wpilibj.motorcontrol.PWMTalonFX; +import edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX; import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.MotorConfig; @@ -22,9 +24,16 @@ public class MotorControllerRobot extends TimedRobot { + // Run the motors at 50% for 2 seconds. + public static final double tStartMotorsSecs = 0.0; + public static final double tStopMotorsSecs = 2.0; + public static final double throttle = 0.5; + private final Joystick m_stick = new Joystick(0); private final Timer m_timer = new Timer(); private PWMMotorController m_pwmMotorController; + private PWMMotorController m_hinge2TurnController; + private PWMMotorController m_hinge2DriveController; private CANSparkMax m_sparkMaxMotorController; private CANSparkFlex m_sparkFlexMotorController; @@ -35,6 +44,8 @@ public class MotorControllerRobot extends TimedRobot { @Override public void robotInit() { m_pwmMotorController = new PWMVictorSPX(1); + m_hinge2TurnController = new PWMTalonSRX(2); + m_hinge2DriveController = new PWMTalonFX(3); m_sparkMaxMotorController = MotorControllerFactory.createSparkMax(2, MotorConfig.NEO); m_sparkFlexMotorController = MotorControllerFactory.createSparkFlex(3, @@ -49,6 +60,8 @@ public void robotPeriodic() { public void close() { System.out.println("Closing motors in Robot.close()"); m_pwmMotorController.close(); + m_hinge2TurnController.close(); + m_hinge2DriveController.close(); m_sparkMaxMotorController.close(); m_sparkFlexMotorController.close(); } @@ -67,15 +80,19 @@ public void autonomousInit() { */ @Override public void autonomousPeriodic() { - // Run the motor at 50% for 2 seconds. - if (m_timer.get() < 2.0) { + double t = m_timer.get(); + if (t >= tStartMotorsSecs && t < tStopMotorsSecs) { m_pwmMotorController.set(0.5); m_sparkMaxMotorController.set(0.5); m_sparkFlexMotorController.set(0.5); + m_hinge2DriveController.set(throttle); + m_hinge2TurnController.set(throttle); } else { m_pwmMotorController.stopMotor(); m_sparkMaxMotorController.stopMotor(); m_sparkFlexMotorController.stopMotor(); + m_hinge2DriveController.stopMotor(); + m_hinge2TurnController.stopMotor(); } } @@ -85,6 +102,8 @@ public void autonomousPeriodic() { @Override public void teleopPeriodic() { m_pwmMotorController.set(m_stick.getX()); + m_hinge2TurnController.set(m_stick.getX()); + m_hinge2DriveController.set(m_stick.getY()); m_sparkMaxMotorController.set(m_stick.getX()); m_sparkFlexMotorController.set(m_stick.getX()); } diff --git a/tests/src/systemTest/java/frc/robot/DBSExampleTest.java b/tests/src/systemTest/java/frc/robot/DBSExampleTest.java index aa3486b..4dfd3c0 100644 --- a/tests/src/systemTest/java/frc/robot/DBSExampleTest.java +++ b/tests/src/systemTest/java/frc/robot/DBSExampleTest.java @@ -32,7 +32,7 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception { 0.1, "Robot close to target velocity"); assertEquals(0.0, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), + s.angularVelocity("ROBOT").norm()), 1, "Robot close to target angular velocity"); }).atSec(3.0, s -> { assertEquals(0.0, @@ -49,7 +49,7 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception { 0.05, "Robot close to target velocity"); assertEquals(0.0, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), + s.angularVelocity("ROBOT").norm()), 1, "Robot close to target angular velocity"); }).run(); } @@ -83,10 +83,10 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception { .getDistance(new Translation3d(0, 0, 0)), 0.1, "Robot close to target velocity"); assertEquals(63.9, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), - 5.0, "Robot close to target angular velocity"); + s.angularVelocity("ROBOT").norm()), 5.0, + "Robot close to target angular velocity"); assertEquals(0.0, - new Translation3d(s.angularVelocity("ROBOT").getAxis()) + new Translation3d(s.angularVelocity("ROBOT").unit()) .getDistance(new Translation3d(0, 0, 1)), 0.1, "Robot close to target angular velocity axis"); }).atSec(1.5, s -> { @@ -101,7 +101,7 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception { 0.1, "Robot close to target velocity"); assertEquals(0.0, Units.radiansToDegrees( - s.angularVelocity("ROBOT").getAngle()), + s.angularVelocity("ROBOT").norm()), 1, "Robot close to target angular velocity"); }).run(); } diff --git a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java index 071dad5..a98dc8d 100644 --- a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java +++ b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java @@ -2,7 +2,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; import org.carlmontrobotics.libdeepbluesim.WebotsSimulator; import org.carlmontrobotics.libdeepbluesim.WebotsSimulator.SimulationState; @@ -17,7 +16,7 @@ import java.util.function.BiConsumer; import java.util.function.Consumer; -import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Timer; @@ -92,104 +91,6 @@ public class MotorControllerTest { // Rearranging gives: // theta(t) = theta_0 + w_f*t + (w_0 - w_f) * t_c * (1-exp(-t/t_c)) - - // The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e. - // sets the speed to 0 and lets it stop on it's own). - private static double tMotorStartsSecs = 0.0; - private static double tMotorStopsSecs = timeOfNextStepSecs(2.0); - - private static double computeSpeedRadPerSec(DCMotor gearMotor, - double moiKgM2, double throttle, double initialSpeedRadPerSec, - double tSecs) { - double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts - * gearMotor.KvRadPerSecPerVolt; - double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2); - // w(t) = w_f + (w_0 - w_f) * exp(-t/t_c) - return targetSpeedRadPerSec - + (initialSpeedRadPerSec - targetSpeedRadPerSec) - * Math.exp(-tSecs / timeConstantSecs); - } - - private static double computeAngleRadians(DCMotor gearMotor, double moiKgM2, - double throttle, double initialSpeedRadPerSec, - double initialAngleRad, double tSecs) { - double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2); - double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts - * gearMotor.KvRadPerSecPerVolt; - // theta(t) = theta_0 + w_f*t + (w_0 - w_f) * t_c * (1-exp(-t/t_c)) - return initialAngleRad + targetSpeedRadPerSec * tSecs - + (initialSpeedRadPerSec - targetSpeedRadPerSec) - * timeConstantSecs - * (1 - Math.exp(-tSecs / timeConstantSecs)); - } - - private static double computeTimeConstantSecs(DCMotor gearMotor, - double moiKgM2) { - // t_c = R*J*k_v/k_T - return gearMotor.rOhms * moiKgM2 * gearMotor.KvRadPerSecPerVolt - / gearMotor.KtNMPerAmp; - } - - private static double computeCylinderMoiKgM2(double radiusMeters, - double heightMeters, double densityKgPerM3) { - double massKg = densityKgPerM3 * Math.PI * radiusMeters * radiusMeters - * heightMeters; - return massKg * radiusMeters * radiusMeters / 2.0; - } - - private static double timeOfNextStepSecs(double tSecs) { - return Math.ceil(tSecs / simStepSizeSecs) * simStepSizeSecs; - } - - private static double expectedSpeedRadPerSec(DCMotor gearMotor, - double moiKgM2, double tSecs) { - - if (tSecs <= tMotorStartsSecs) { - return 0.0; - } - if (tSecs <= tMotorStopsSecs) { - return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0, - tSecs - tMotorStartsSecs); - } - return computeSpeedRadPerSec(gearMotor, moiKgM2, 0, - expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs), - tSecs - tMotorStopsSecs); - } - - private static double expectedAngleRadians(DCMotor gearMotor, - double moiKgM2, double tSecs) { - if (tSecs <= tMotorStartsSecs) { - return 0.0; - } else if (tSecs <= tMotorStopsSecs) { - return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, - tSecs - tMotorStartsSecs); - } - return computeAngleRadians(gearMotor, moiKgM2, 0, - expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs), - expectedAngleRadians(gearMotor, moiKgM2, tMotorStopsSecs), - tSecs - tMotorStopsSecs); - } - - private static double computeGearing(DCMotor motor, - double desiredFreeSpeedRPS) { - return motor.freeSpeedRadPerSec / (2 * Math.PI) / desiredFreeSpeedRPS; - } - - private static double computeFlywheelThickness(DCMotor gearMotor, - double flywheelRadiusMeters, double flywheelDensityKgPerM3, - double desiredTimeConstantSecs) { - // for a time constant of t_c seconds: - // t_c = R*J*k_v/k_T - // J = t_c*k_T/(k_v*R) - // J = 0.5*density * pi * r^2 * h * r^2 - // h = 2*J/(density*pi*r^4) - // h = 2*t_c*k_T/(k_v*R*density*pi*r^4) - return 2 * desiredTimeConstantSecs * gearMotor.KtNMPerAmp - / (gearMotor.KvRadPerSecPerVolt * gearMotor.rOhms - * flywheelDensityKgPerM3 * Math.PI - * Math.pow(flywheelRadiusMeters, 4)); - } - private static record Measurement(double simTimeSec, double speedRadPerSec) { } @@ -201,6 +102,39 @@ private static class Model { double moiKgM2; Measurement m1, m2; + double computeTimeConstantSecs(DCMotor gearMotor, double moiKgM2) { + // t_c = R*J*k_v/k_T + return gearMotor.rOhms * moiKgM2 * gearMotor.KvRadPerSecPerVolt + / gearMotor.KtNMPerAmp; + } + + protected double computeCylinderMoiKgM2(double radiusMeters, + double heightMeters, double densityKgPerM3) { + double massKg = densityKgPerM3 * Math.PI * radiusMeters + * radiusMeters * heightMeters; + return massKg * radiusMeters * radiusMeters / 2.0; + } + + double computeGearing(DCMotor motor, double desiredFreeSpeedRPS) { + return motor.freeSpeedRadPerSec / (2 * Math.PI) + / desiredFreeSpeedRPS; + } + + double computeFlywheelThickness(DCMotor gearMotor, + double flywheelRadiusMeters, double flywheelDensityKgPerM3, + double desiredTimeConstantSecs) { + // for a time constant of t_c seconds: + // t_c = R*J*k_v/k_T + // J = t_c*k_T/(k_v*R) + // J = 0.5*density * pi * r^2 * h * r^2 + // h = 2*J/(density*pi*r^4) + // h = 2*t_c*k_T/(k_v*R*density*pi*r^4) + return 2 * desiredTimeConstantSecs * gearMotor.KtNMPerAmp + / (gearMotor.KvRadPerSecPerVolt * gearMotor.rOhms + * flywheelDensityKgPerM3 * Math.PI + * Math.pow(flywheelRadiusMeters, 4)); + } + Model(String motorModelName, String shaftDefPath, double gearing, double flywheelThicknessMeters) throws Exception { this.motorModelName = motorModelName; @@ -236,44 +170,12 @@ private static class Model { } public String toString() { - return "Model(\"%s\", \"%s\")".formatted(motorModelName, - shaftDefPath); + return "%s(\"%s\", \"%s\")".formatted(getClass().getName(), + motorModelName, shaftDefPath); } - private void assertShaftCorrectAtSecs(double actualSpeedRadPerSec, - double actualAngleRadians, double tSecs) { - double jitterSecs = 0.5 * simStepSizeSecs; - double expectedEarlierSpeedRadPerSec = expectedSpeedRadPerSec( - gearMotor, moiKgM2, tSecs - jitterSecs); - double expectedLaterSpeedRadPerSec = expectedSpeedRadPerSec( - gearMotor, moiKgM2, tSecs + jitterSecs); - LOG.log(Level.DEBUG, "expectedEarlierSpeedRadPerSec = {0}", - expectedEarlierSpeedRadPerSec); - LOG.log(Level.DEBUG, "expectedLaterSpeedRadPerSec = {0}", - expectedLaterSpeedRadPerSec); - assertEquals( - (expectedEarlierSpeedRadPerSec - + expectedLaterSpeedRadPerSec) / 2, - actualSpeedRadPerSec, - Math.abs(expectedEarlierSpeedRadPerSec - - expectedLaterSpeedRadPerSec) / 2, - "Shaft not close enough to target angular velocity"); - double expectedEarlierAngleRadians = expectedAngleRadians(gearMotor, - moiKgM2, tSecs - jitterSecs); - double expectedLaterAngleRadians = expectedAngleRadians(gearMotor, - moiKgM2, tSecs + jitterSecs); - double expectedAngleRadians = - (expectedEarlierAngleRadians + expectedLaterAngleRadians) - / 2; - double toleranceRadians = Math.abs( - expectedEarlierAngleRadians - expectedLaterAngleRadians) - / 2; - assertTrue( - MathUtil.isNear(expectedAngleRadians, actualAngleRadians, - toleranceRadians, 0, 2 * Math.PI), - "Shaft not close enough to target rotation. Expected %g +/- %g radians, but got %g radians." - .formatted(expectedAngleRadians, toleranceRadians, - actualAngleRadians)); + protected double getActualShaftSpeedRadPerSec(SimulationState s) { + return s.angularVelocity(shaftDefPath).norm(); } private void assertCorrectTimeConstant(double throttle) { @@ -291,7 +193,49 @@ private void assertCorrectTimeConstant(double throttle) { / (Math.log((m2.speedRadPerSec() - targetSpeedRadPerSec) / (m1.speedRadPerSec() - targetSpeedRadPerSec))); assertEquals(computeTimeConstantSecs(gearMotor, moiKgM2), - actualTimeConstantSecs, 0.02, "Time constant incorrect"); + actualTimeConstantSecs, 0.05, "Time constant incorrect"); + } + } + + private static class Hinge2DriveModel extends Model { + + Hinge2DriveModel(String motorModelName, String shaftDefPath, + double gearing, double flywheelThicknessMeters) + throws Exception { + super(motorModelName, shaftDefPath, gearing, + flywheelThicknessMeters); + } + + @Override + protected double getActualShaftSpeedRadPerSec(SimulationState s) { + var v = s.angularVelocity(shaftDefPath); + return v.elementTimes(VecBuilder.fill(1, 0, 1)).normF(); + } + } + + private static class Hinge2TurnModel extends Model { + + Hinge2TurnModel(String motorModelName, String shaftDefPath, + double gearing, double flywheelThicknessMeters) + throws Exception { + super(motorModelName, shaftDefPath, gearing, + flywheelThicknessMeters); + } + + @Override + protected double computeCylinderMoiKgM2(double radiusMeters, + double heightMeters, double densityKgPerM3) { + // Calculate the MOI around the x (or y) axis, not the z axis. + double massKg = densityKgPerM3 * Math.PI * radiusMeters + * radiusMeters * heightMeters; + return 1 / 12.0 * massKg * (3 * radiusMeters * radiusMeters + + heightMeters * heightMeters); + } + + @Override + protected double getActualShaftSpeedRadPerSec(SimulationState s) { + var v = s.angularVelocity(shaftDefPath); + return v.get(1, 0); } } @@ -313,6 +257,10 @@ void testCorrectRotationInAutonomous() throws Exception { models.add(new Model("NeoVortex", "VORTEX_SHAFT", 113.067, 0.648614)); models.add(new Model("NEO", "NEO_SHAFT", 94.6, 0.392)); models.add(new Model("MiniCIM", "MINICIM_SHAFT", 97.3333, 0.215)); + models.add(new Hinge2DriveModel("MiniCIM", "HINGE2JOINT_SHAFT", 97.3333, + 0.215)); + models.add(new Hinge2TurnModel("MiniCIM", "HINGE2JOINT_SHAFT", 97.3333, + 0.215)); try (var manager = new WebotsSimulator( "../plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt", @@ -327,28 +275,35 @@ void testCorrectRotationInAutonomous() throws Exception { LOG.log(Level.DEBUG, "robotTime = {0}, simTimeSec = {1}, speedRadPerSec = {2}", s.getRobotTimeSec(), s.getSimTimeSec(), - s.angularVelocity(m.shaftDefPath).getAngle()); - })).atSec(5 * simStepSizeSecs, stateChecker((s, m) -> { - m.m1 = new Measurement(s.getSimTimeSec(), - s.angularVelocity(m.shaftDefPath).getAngle()); - })).atSec(1.0, stateChecker((s, m) -> { - m.m2 = new Measurement(s.getSimTimeSec(), - s.angularVelocity(m.shaftDefPath).getAngle()); - m.assertCorrectTimeConstant(0.5); - m.assertShaftCorrectAtSecs( - s.angularVelocity(m.shaftDefPath).getAngle(), - s.rotation(m.shaftDefPath).getZ(), s.getRobotTimeSec()); - })).atSec(2.0 + 5 * simStepSizeSecs, stateChecker((s, m) -> { - m.m1 = new Measurement(s.getSimTimeSec(), - s.angularVelocity(m.shaftDefPath).getAngle()); - })).atSec(3.0, stateChecker((s, m) -> { - m.m2 = new Measurement(s.getSimTimeSec(), - s.angularVelocity(m.shaftDefPath).getAngle()); - m.assertCorrectTimeConstant(0.0); - m.assertShaftCorrectAtSecs( - s.angularVelocity(m.shaftDefPath).getAngle(), - s.rotation(m.shaftDefPath).getZ(), s.getRobotTimeSec()); - })).run(); + s.angularVelocity(m.shaftDefPath).norm()); + })).atSec( + MotorControllerRobot.tStartMotorsSecs + 5 * simStepSizeSecs, + stateChecker((s, m) -> { + m.m1 = new Measurement(s.getSimTimeSec(), + m.getActualShaftSpeedRadPerSec(s)); + })) + .atSec((MotorControllerRobot.tStartMotorsSecs + + MotorControllerRobot.tStopMotorsSecs) / 2, + stateChecker((s, m) -> { + m.m2 = new Measurement(s.getSimTimeSec(), + m.getActualShaftSpeedRadPerSec(s)); + m.assertCorrectTimeConstant(0.5); + })) + .atSec(MotorControllerRobot.tStopMotorsSecs + + 5 * simStepSizeSecs, stateChecker((s, m) -> { + m.m1 = new Measurement(s.getSimTimeSec(), + m.getActualShaftSpeedRadPerSec(s)); + })) + .atSec(MotorControllerRobot.tStopMotorsSecs + + (MotorControllerRobot.tStopMotorsSecs + - MotorControllerRobot.tStartMotorsSecs) + / 2, + stateChecker((s, m) -> { + m.m2 = new Measurement(s.getSimTimeSec(), + m.getActualShaftSpeedRadPerSec(s)); + m.assertCorrectTimeConstant(0.0); + })) + .run(); } } }