Fix to work with spec compliant CAN sims.

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

@@ -4,12 +4,18 @@
 import java.util.Optional;
 import java.util.concurrent.CopyOnWriteArraySet;
 
+import org.team199.deepbluesim.mediators.AnalogInputMediator;
 import org.team199.deepbluesim.mediators.GyroMediator;
 import org.team199.deepbluesim.mediators.PWMMotorMediator;
-import org.team199.deepbluesim.mediators.SimDeviceMotorMediator;
-import org.team199.deepbluesim.mediators.SimDeviceEncoderMediator;
+import org.team199.deepbluesim.mediators.CANMotorMediator;
+import org.team199.deepbluesim.mediators.CANEncoderMediator;
+import org.team199.deepbluesim.mediators.DutyCycleMediator;
 import org.team199.deepbluesim.mediators.WPILibEncoderMediator;
 import org.team199.wpiws.ScopedObject;
+import org.team199.wpiws.devices.AnalogInputSim;
+import org.team199.wpiws.devices.CANMotorSim;
+import org.team199.wpiws.devices.CANEncoderSim;
+import org.team199.wpiws.devices.DutyCycleSim;
 import org.team199.wpiws.devices.EncoderSim;
 import org.team199.wpiws.devices.PWMSim;
 import org.team199.wpiws.devices.SimDeviceSim;
@@ -128,22 +134,29 @@ public static void connectEncoder(PositionSensor device, Supervisor robot) {
                 unboundEncoders.add(device.getName());
             }
         } else if(node.getField("id") != null) { // CANCoder
-            new SimDeviceEncoderMediator(device, new SimDeviceSim("CANCoder[" + node.getField("id").getSFInt32() + "]"), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+            new DutyCycleMediator(device, new DutyCycleSim("CANDutyCycle:CANCoder[" + node.getField("id").getSFInt32() + "]", "SimDevice"), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
         } else if(node.getTypeName().startsWith("SparkMax")) { // One of the SparkMax encoder types
             Motor motor = device.getMotor();
 
             String motorName;
-            if(motor == null || !(motorName = motor.getName()).startsWith("DBSim_Motor_Spark Max")) {
+            if(motor == null || !(motorName = motor.getName()).startsWith("DBSim_Motor_Spark")) {
                 System.err.println("Warning: Spark Max Encoder \"" + device.getName() + "\" is not attached to a Spark Max motor!");
                 return;
             }
 
             String[] motorNameParts = motorName.split("_");
             int motorId = Integer.parseInt(motorNameParts[3]);
 
-            String simDeviceName = "SparkMax[" + motorId + "]_" + node.getTypeName().substring("SparkMax".length());
-
-            new SimDeviceEncoderMediator(device, new SimDeviceSim(simDeviceName), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+            if (node.getTypeName().equals("SparkMaxAlternateEncoder")) {
+                String simDeviceName = "CANEncoder:CANSparkMax[" + motorId + "]-alternate";
+                new CANEncoderMediator(device, new CANEncoderSim(simDeviceName, "SimDevice"), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+            } else if (node.getTypeName().equals("SparkMaxAbsoluteEncoder")) {
+                String simDeviceName = "CANDutyCycle:CANSparkMax[" + motorId + "]";
+                new DutyCycleMediator(device, new DutyCycleSim(simDeviceName, "SimDevice"), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+            } else if (node.getTypeName().equals("SparkMaxAnalogSensor")) {
+                String simDeviceName = "CANAIn:CANSparkMax[" + motorId + "]";
+                new AnalogInputMediator(device, new AnalogInputSim(simDeviceName, "SimDevice"), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+            }
         } else {
             System.err.println("Warning: Ignoring invalid encoder: " + device.getName() + "!");
         }
@@ -166,8 +179,8 @@ public static void connectMotor(Motor device, Supervisor robot) {
         if(controllerType.equals("PWM")) {
             new PWMMotorMediator(device, new PWMSim(Integer.toString(port)), motorConstants, gearing, inverted, CALLBACKS);
         } else {
-            String simDeviceName = controllerType.replaceAll("\\s", "") + "[" + port + "]";
-            new SimDeviceMotorMediator(device, new SimDeviceSim(simDeviceName), motorConstants, gearing, inverted, CALLBACKS);
+            String simDeviceName = "CANMotor:CAN" + controllerType.replaceAll("\\s", "") + "[" + port + "]";
+            new CANMotorMediator(device, new CANMotorSim(simDeviceName, "SimDevice"), motorConstants, gearing, inverted, CALLBACKS);
         }
     }
 

plugin/controller/src/main/java/org/team199/deepbluesim/mediators/AnalogInputMediator.java

@@ -0,0 +1,29 @@
+package org.team199.deepbluesim.mediators;
+
+import org.team199.wpiws.devices.AnalogInputSim;
+
+import com.cyberbotics.webots.controller.PositionSensor;
+
+public class AnalogInputMediator extends EncoderMediatorBase {
+
+    public final AnalogInputSim device;
+
+    public AnalogInputMediator(PositionSensor encoder, AnalogInputSim device, boolean isOnMotorShaft, boolean isAbsolute, double absoluteOffsetDeg, boolean isInverted, int countsPerRevolution, double gearing) {
+        super(encoder, isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+        this.device = device;
+    }
+
+    @Override
+    public void setPosition(int positionCounts) {
+        // 1 Volt = 1 Rotation. The robot code can use
+        // SparkAnalogSensor.setPositionConversionFactor() to change what
+        // SparkAnalogSensor.getPosition() returns.
+        device.setVoltage(1.0 * positionCounts / countsPerRevolution);
+    }
+
+    @Override
+    public void setVelocity(int velocityCountsPerSecond) {
+        // AnalogInput data doesn't include velocity so we do nothing.
+    }
+
+}

plugin/controller/src/main/java/org/team199/deepbluesim/mediators/CANEncoderMediator.java

@@ -0,0 +1,26 @@
+package org.team199.deepbluesim.mediators;
+
+import org.team199.wpiws.devices.CANEncoderSim;
+
+import com.cyberbotics.webots.controller.PositionSensor;
+
+public class CANEncoderMediator extends EncoderMediatorBase {
+
+    public final CANEncoderSim device;
+
+    public CANEncoderMediator(PositionSensor encoder, CANEncoderSim device, boolean isOnMotorShaft, boolean isAbsolute, double absoluteOffsetDeg, boolean isInverted, int countsPerRevolution, double gearing) {
+        super(encoder, isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+        this.device = device;
+    }
+
+    @Override
+    public void setPosition(int positionCounts) {
+        device.setPosition(1.0 * positionCounts / countsPerRevolution);
+    }
+
+    @Override
+    public void setVelocity(int velocityCountsPerSecond) {
+        device.setVelocity(1.0 * velocityCountsPerSecond / countsPerRevolution);
+    }
+
+}

plugin/controller/src/main/java/org/team199/deepbluesim/mediators/SimDeviceMotorMediator.java → plugin/controller/src/main/java/org/team199/deepbluesim/mediators/CANMotorMediator.java

@@ -5,6 +5,8 @@
 import org.team199.deepbluesim.ParseUtils;
 import org.team199.deepbluesim.Simulation;
 import org.team199.wpiws.ScopedObject;
+import org.team199.wpiws.devices.CANEncoderSim;
+import org.team199.wpiws.devices.CANMotorSim;
 import org.team199.wpiws.devices.SimDeviceSim;
 
 import com.cyberbotics.webots.controller.Brake;
@@ -16,15 +18,15 @@
 /**
  * Links WPILib motor controllers to Webots
  */
-public class SimDeviceMotorMediator implements Runnable {
+public class CANMotorMediator implements Runnable {
 
     public static final int NEO_BUILTIN_ENCODER_CPR = 42;
 
     public final Motor motor;
     public final double gearing;
     public final boolean inverted;
     public final DCMotor motorConstants;
-    public final SimDeviceSim motorDevice;
+    public final CANMotorSim motorDevice;
     public final Brake brake;
 
     private double requestedOutput = 0;
@@ -39,19 +41,19 @@ public class SimDeviceMotorMediator implements Runnable {
      * @param gearing the gear ratio to use
      * @param callbackStore a collection to store callbacks in
      */
-    public SimDeviceMotorMediator(Motor motor, SimDeviceSim simDevice, DCMotor motorConstants, double gearing, boolean inverted, Collection<ScopedObject<?>> callbackStore) {
+    public CANMotorMediator(Motor motor, CANMotorSim simDevice, DCMotor motorConstants, double gearing, boolean inverted, Collection<ScopedObject<?>> callbackStore) {
         this.motor = motor;
         motorDevice = simDevice;
         this.motorConstants = motorConstants;
         this.gearing = gearing;
         this.inverted = inverted;
 
-        if(motor.getName().startsWith("DBSim_Motor_Spark Max")) {
+        if(motor.getName().startsWith("DBSim_Motor_Spark ")) {
             PositionSensor encoder = motor.getPositionSensor();
             if(encoder == null) {
-                System.err.println(String.format("WARNING: Spark Max encoder not found for motor: \"%s\", no position data will be reported!", motor.getName()));
+                System.err.println(String.format("WARNING: Spark encoder not found for motor: \"%s\", no position data will be reported!", motor.getName()));
             } else {
-                new SimDeviceEncoderMediator(encoder, new SimDeviceSim(motorDevice.id + "_RelativeEncoder"), true, false, 0, inverted, NEO_BUILTIN_ENCODER_CPR, gearing);
+                new CANEncoderMediator(encoder, new CANEncoderSim(motorDevice.id.replace("CANMotor:", "CANEncoder:"), "SimDevice"), true, false, 0, inverted, NEO_BUILTIN_ENCODER_CPR, gearing);
             }
         }
 
@@ -65,14 +67,14 @@ public SimDeviceMotorMediator(Motor motor, SimDeviceSim simDevice, DCMotor motor
 
         brake.setDampingConstant(motorConstants.stallTorqueNewtonMeters * gearing);
 
-        callbackStore.add(motorDevice.registerValueChangedCallback("Brake Mode", (name, enabled) -> {
-            brakeMode = Boolean.parseBoolean(enabled);
+        callbackStore.add(motorDevice.registerBrakemodeCallback((name, enabled) -> {
+            brakeMode = enabled;
         }, true));
-        callbackStore.add(motorDevice.registerValueChangedCallback("Neutral Deadband", (name, deadband) -> {
-            neutralDeadband = Math.abs(ParseUtils.parseDoubleOrDefault(deadband, neutralDeadband));
+        callbackStore.add(motorDevice.registerNeutraldeadbandCallback((name, deadband) -> {
+            neutralDeadband = deadband;
         }, true));
-        callbackStore.add(motorDevice.registerValueChangedCallback("Speed", (name, speed) -> {
-            requestedOutput = ParseUtils.parseDoubleOrDefault(speed, requestedOutput);
+        callbackStore.add(motorDevice.registerPercentoutputCallback((name, percentOutput) -> {
+            requestedOutput = percentOutput;
         }, true));
 
         Simulation.registerPeriodicMethod(this);
@@ -96,7 +98,7 @@ public void run() {
         double currentDraw = motorConstants.getCurrent(velocity, currentOutput * motorConstants.nominalVoltageVolts);
         motor.setAvailableTorque(motorConstants.getTorque(currentDraw) * gearing);
 
-        motorDevice.set("Current Draw", currentDraw);
+        motorDevice.setMotorcurrent(currentDraw);
     }
 
 }

plugin/controller/src/main/java/org/team199/deepbluesim/mediators/DutyCycleMediator.java

@@ -0,0 +1,27 @@
+package org.team199.deepbluesim.mediators;
+
+import org.team199.wpiws.devices.DutyCycleSim;
+
+import com.cyberbotics.webots.controller.PositionSensor;
+
+public class DutyCycleMediator extends EncoderMediatorBase {
+
+    public final DutyCycleSim device;
+
+    public DutyCycleMediator(PositionSensor encoder, DutyCycleSim device, boolean isOnMotorShaft, boolean isAbsolute, double absoluteOffsetDeg, boolean isInverted, int countsPerRevolution, double gearing) {
+        super(encoder, isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
+        this.device = device;
+        device.setConnected(true);
+    }
+
+    @Override
+    public void setPosition(int positionCounts) {
+        device.setPosition(1.0 * positionCounts / countsPerRevolution);
+    }
+
+    @Override
+    public void setVelocity(int velocityCountsPerSecond) {
+        // DutyCycle data doesn't include velocity so we do nothing.
+    }
+
+}