diff --git a/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java b/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java index 535a8f0..aca4590 100644 --- a/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java +++ b/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java @@ -165,10 +165,9 @@ public void run() { name, defPath); var node = robot.getFromDef(defPath); if (node == null) { - LOG.log(Level.ERROR, - "Could not find node for the following DEF path: {0}", - defPath); - return; + throw new RuntimeException( + "Could not find node for the following DEF path: %s" + .formatted(defPath)); } switch (name) { case NTConstants.POSITION_TOPIC_NAME: diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/DBSRotationalMotorBase.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/DBSRotationalMotorBase.proto index c35d3b7..b58ead0 100644 --- a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/DBSRotationalMotorBase.proto +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/DBSRotationalMotorBase.proto @@ -5,7 +5,7 @@ # A Motor with the properties necessary to interface with DeepBlueSim PROTO DBSRotationalMotorBase [ - unconnectedField SFString{"Spark Max", "Talon SRX", "Victor SPX", "PWM"} controllerType "Spark Max" + unconnectedField SFString{"Spark Max", "Spark Flex", "Talon SRX", "Victor SPX", "PWM"} controllerType "Spark Max" unconnectedField SFInt32 port 0 field SFFloat gearing 1 unconnectedField SFBool inverted FALSE diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSLinearMotor.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSLinearMotor.proto index 63f86ef..0c7b984 100644 --- a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSLinearMotor.proto +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSLinearMotor.proto @@ -7,8 +7,8 @@ EXTERNPROTO "DBSLinearMotorBase.proto" PROTO PreconfiguredDBSLinearMotor [ - unconnectedField SFString{"AndyMark9015", "AndyMarkRs775_125", "Bag", "BanebotsRs550", "BanebotsRs775", "CIM", "Falcon500", "MiniCIM", "NEO", "NEO550", "RomiBuiltin", "Vex775Pro"} motorType "NEO" - field SFString{"Spark Max", "Talon SRX", "Victor SPX", "PWM"} controllerType "Spark Max" + unconnectedField SFString{"AndyMark9015", "AndyMarkRs775_125", "Bag", "BanebotsRs550", "BanebotsRs775", "CIM", "Falcon500", "MiniCIM", "NEO", "NEO550", "NEOVortex", "RomiBuiltin", "Vex775Pro"} motorType "NEO" + field SFString{"Spark Max", "Spark Flex", "Talon SRX", "Victor SPX", "PWM"} controllerType "Spark Max" field SFInt32 port 0 field SFFloat gearing 1 field SFFloat outputRadiusMeters 0.0254 # 2in diameter @@ -18,8 +18,10 @@ PROTO PreconfiguredDBSLinearMotor [ { %< import {assert} from 'wbutility.js'; - assert(!fields.motorType.value.startsWith("NEO") || ["Spark Max", "PWM"].includes(fields.controllerType.value), - `PreconfiguredDCDBSMotor: Brushless motor "${fields.motorType.value}" cannot be driven by brushed motor controller "${fields.controllerType.value}"!`) + assert(!fields.motorType.value.startsWith("NEO") || ["Spark Max", "Spark Flex", "PWM"].includes(fields.controllerType.value), + `PreconfiguredDCDBSMotor: Brushless motor "${fields.motorType.value}" cannot be driven by brushed motor controller "${fields.controllerType.value}"!`); + assert(fields.controllerType.value !== "Spark Flex" || ["NEOVortex"].includes(fields.motorType.value), + `PreconfiguredDCDBSMotor: Spark Flex controller can not drive a "${fields.motorType.value}"!`); let motor = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsmotor.js`)); >% DBSLinearMotorBase { diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto index 64e272e..d27cf76 100644 --- a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto @@ -7,8 +7,8 @@ EXTERNPROTO "DBSRotationalMotorBase.proto" PROTO PreconfiguredDBSRotationalMotor [ - unconnectedField SFString{"AndyMark9015", "AndyMarkRs775_125", "Bag", "BanebotsRs550", "BanebotsRs775", "CIM", "Falcon500", "MiniCIM", "NEO", "NEO550", "RomiBuiltin", "Vex775Pro"} motorType "NEO" - field SFString{"Spark Max", "Talon SRX", "Victor SPX", "PWM"} controllerType "Spark Max" + unconnectedField SFString{"AndyMark9015", "AndyMarkRs775_125", "Bag", "BanebotsRs550", "BanebotsRs775", "CIM", "Falcon500", "MiniCIM", "NEO", "NEO550", "NEOVortex", "RomiBuiltin", "Vex775Pro"} motorType "NEO" + field SFString{"Spark Max", "Spark Flex", "Talon SRX", "Victor SPX", "PWM"} controllerType "Spark Max" field SFInt32 port 0 field SFFloat gearing 1 field SFBool inverted FALSE @@ -17,8 +17,10 @@ PROTO PreconfiguredDBSRotationalMotor [ { %< import {assert} from 'wbutility.js'; - assert(!fields.motorType.value.startsWith("NEO") || ["Spark Max", "PWM"].includes(fields.controllerType.value), - `PreconfiguredDBSRotationalMotor: Brushless motor "${fields.motorType.value}" cannot be driven by brushed motor controller "${fields.controllerType.value}"!`) + assert(!fields.motorType.value.startsWith("NEO") || ["Spark Max", "Spark Flex", "PWM"].includes(fields.controllerType.value), + `PreconfiguredDCDBSMotor: Brushless motor "${fields.motorType.value}" cannot be driven by brushed motor controller "${fields.controllerType.value}"!`); + assert(fields.controllerType.value !== "Spark Flex" || ["NEOVortex"].includes(fields.motorType.value), + `PreconfiguredDCDBSMotor: Spark Flex controller can not drive a "${fields.motorType.value}"!`); let motor = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsmotor.js`)); >% DBSRotationalMotorBase { diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsmotor.js b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsmotor.js index 8269ace..ca16820 100644 --- a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsmotor.js +++ b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsmotor.js @@ -86,6 +86,13 @@ freeCurrentAmps: 1.4, freeSpeedRPM: 11000 }, + NEOVortex: { + nominalVoltageVolts: 12, + stallTorqueNewtonMeters: 3.60, + stallCurrentAmps: 211, + freeCurrentAmps: 3.6, + freeSpeedRPM: 6784 + }, RomiBuiltin: { nominalVoltageVolts: 4.5, stallTorqueNewtonMeters: 0.1765, diff --git a/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg b/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg index 59011d0..d588a29 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 bd1e189..0eed261 100644 --- a/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt +++ b/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt @@ -24,7 +24,7 @@ RectangleArena { DEF ROBOT Robot { translation 0 0 0.06 children [ - DEF PWM_BASE Solid { + DEF MINICIM_BASE Solid { children [ PoweredHingeJoint { devices [ @@ -37,7 +37,7 @@ DEF ROBOT Robot { gearing 97.3333 } ] - endPoint DEF PWM_SHAFT Solid { + endPoint DEF MINICIM_SHAFT Solid { children [ Shape { appearance PBRAppearance { @@ -60,7 +60,7 @@ DEF ROBOT Robot { } ] } - DEF CAN_BASE Solid { + DEF NEO_BASE Solid { translation 0 2 0 children [ PoweredHingeJoint { @@ -74,7 +74,7 @@ DEF ROBOT Robot { gearing 94.6 } ] - endPoint DEF CAN_SHAFT Solid { + endPoint DEF NEO_SHAFT Solid { children [ Shape { appearance PBRAppearance { @@ -98,6 +98,46 @@ DEF ROBOT Robot { ] name "solid(1)" } + DEF VORTEX_BASE Solid { + translation 2 2 0 + children [ + PoweredHingeJoint { + devices [ + REVBuiltinEncoder { + } + DBSBrake { + } + PreconfiguredDBSRotationalMotor { + motorType "NEOVortex" + controllerType "Spark Flex" + port 3 + gearing 113.067 + } + ] + endPoint DEF VORTEX_SHAFT Solid { + children [ + Shape { + appearance PBRAppearance { + } + geometry Box { + size 1 1 0.648614 + } + } + ] + boundingObject Cylinder { + height 0.648614 + radius 0.5 + } + physics Physics { + } + } + jointParameters HingeJointParameters { + axis 0 0 1 + } + } + ] + name "solid(2)" + } ] controller "DeepBlueSim" supervisor TRUE diff --git a/tests/src/main/java/frc/robot/MotorControllerRobot.java b/tests/src/main/java/frc/robot/MotorControllerRobot.java index 782ebac..736679e 100644 --- a/tests/src/main/java/frc/robot/MotorControllerRobot.java +++ b/tests/src/main/java/frc/robot/MotorControllerRobot.java @@ -17,14 +17,16 @@ import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.MotorConfig; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; public class MotorControllerRobot extends TimedRobot { private final Joystick m_stick = new Joystick(0); private final Timer m_timer = new Timer(); - private PWMMotorController m_motorController; - private CANSparkMax m_canMotorController; + private PWMMotorController m_pwmMotorController; + private CANSparkMax m_sparkMaxMotorController; + private CANSparkFlex m_sparkFlexMotorController; /** * This function is run when the robot is first started up and should be used for any @@ -32,9 +34,11 @@ public class MotorControllerRobot extends TimedRobot { */ @Override public void robotInit() { - m_motorController = new PWMVictorSPX(1); - m_canMotorController = + m_pwmMotorController = new PWMVictorSPX(1); + m_sparkMaxMotorController = MotorControllerFactory.createSparkMax(2, MotorConfig.NEO); + m_sparkFlexMotorController = MotorControllerFactory.createSparkFlex(3, + MotorConfig.NEO_VORTEX); } @Override @@ -44,8 +48,9 @@ public void robotPeriodic() { public void close() { System.out.println("Closing motors in Robot.close()"); - m_motorController.close(); - m_canMotorController.close(); + m_pwmMotorController.close(); + m_sparkMaxMotorController.close(); + m_sparkFlexMotorController.close(); } /** @@ -64,11 +69,13 @@ public void autonomousInit() { public void autonomousPeriodic() { // Run the motor at 50% for 2 seconds. if (m_timer.get() < 2.0) { - m_motorController.set(0.5); - m_canMotorController.set(0.5); + m_pwmMotorController.set(0.5); + m_sparkMaxMotorController.set(0.5); + m_sparkFlexMotorController.set(0.5); } else { - m_motorController.stopMotor(); - m_canMotorController.stopMotor(); + m_pwmMotorController.stopMotor(); + m_sparkMaxMotorController.stopMotor(); + m_sparkFlexMotorController.stopMotor(); } } @@ -77,8 +84,9 @@ public void autonomousPeriodic() { */ @Override public void teleopPeriodic() { - m_motorController.set(m_stick.getX()); - m_canMotorController.set(m_stick.getX()); + m_pwmMotorController.set(m_stick.getX()); + m_sparkMaxMotorController.set(m_stick.getX()); + m_sparkFlexMotorController.set(m_stick.getX()); } } diff --git a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java index 692aa15..071dad5 100644 --- a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java +++ b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java @@ -310,8 +310,9 @@ private Consumer stateChecker( // @RepeatedTest(value = 20, failureThreshold = 1) @RepeatedTest(1) void testCorrectRotationInAutonomous() throws Exception { - models.add(new Model("NEO", "CAN_SHAFT", 94.6, 0.392)); - models.add(new Model("MiniCIM", "PWM_SHAFT", 97.3333, 0.215)); + 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)); try (var manager = new WebotsSimulator( "../plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt",