Skip to content

Commit

Permalink
Merge pull request #53 from DeepBlueRobotics/fix-to-work-with-complia…
Browse files Browse the repository at this point in the history
…nt-can-sims

Fix to work with spec compliant CAN sims.
  • Loading branch information
brettle authored May 25, 2024
2 parents 0162364 + b164a55 commit 5efa0a6
Show file tree
Hide file tree
Showing 7 changed files with 121 additions and 49 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,18 @@
import java.util.Optional;
import java.util.concurrent.CopyOnWriteArraySet;

import org.team199.deepbluesim.mediators.AnalogInputEncoderMediator;
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;
Expand Down Expand Up @@ -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 AnalogInputEncoderMediator(device, new AnalogInputSim(simDeviceName, "SimDevice"), isOnMotorShaft, isAbsolute, absoluteOffsetDeg, isInverted, countsPerRevolution, gearing);
}
} else {
System.err.println("Warning: Ignoring invalid encoder: " + device.getName() + "!");
}
Expand All @@ -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);
}
}

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
package org.team199.deepbluesim.mediators;

import org.team199.wpiws.devices.AnalogInputSim;

import com.cyberbotics.webots.controller.PositionSensor;

public class AnalogInputEncoderMediator extends EncoderMediatorBase {

public final AnalogInputSim device;

public AnalogInputEncoderMediator(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.
}

}
Original file line number Diff line number Diff line change
@@ -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);
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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);
}
}

Expand All @@ -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);
Expand All @@ -96,7 +98,8 @@ 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);
motorDevice.setBusvoltage(12);
}

}
Original file line number Diff line number Diff line change
@@ -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.
}

}

This file was deleted.

0 comments on commit 5efa0a6

Please sign in to comment.