From 50af5f778a230fc0107212f1d99886c72deb061e Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sun, 6 Aug 2023 19:29:22 -0700 Subject: [PATCH 01/18] fix motor following semantics --- .../lib199/sim/MockPhoenixController.java | 42 +++++++++++-------- .../lib199/sim/MockSparkMax.java | 29 +++++++++---- 2 files changed, 45 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java index af092a76..971bda1f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java @@ -2,31 +2,38 @@ import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.CopyOnWriteArrayList; +import java.util.function.DoubleConsumer; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.IMotorController; import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; -abstract class MockPhoenixController implements AutoCloseable { +abstract class MockPhoenixController implements AutoCloseable, DoubleConsumer { private final int portPWM; private boolean isInverted; // Assign the CAN port to a PWM port so it works with the simulator. Not a fan of this solution though // CAN ports should be separate from PWM ports protected PWMMotorController motorPWM; // Since we need to keep a record of all the motor's followers - protected static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); + protected static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); public MockPhoenixController(int portPWM) { this.portPWM = portPWM; isInverted = false; } + @Override + public void accept(double value) { + set(value); + } + public void set(double speed) { - speed = (getInverted() ? -1.0 : 1.0) * speed; - motorPWM.set(speed); + motorPWM.set((getInverted() ? -1.0 : 1.0) * speed); if (followMap.containsKey(getDeviceID())) { - for (PWMMotorController motor : followMap.get(getDeviceID())) motor.set(speed); + // For CTRE controllers, the follower receives the pre-leader-inversion speed and depends on the inversion state from setInverted + // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive + for (DoubleConsumer motorOutputSetter : followMap.get(getDeviceID())) motorOutputSetter.accept(speed); } } @@ -35,29 +42,30 @@ public double get() { } public void follow(IMotorController leader) { + // For CTRE controllers, the follower receives the pre-leader-inversion speed and depends on the inversion state from setInverted + // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive if (!followMap.containsKey(leader.getDeviceID())) { - CopyOnWriteArrayList arr = new CopyOnWriteArrayList(); - arr.add(motorPWM); - followMap.put(leader.getDeviceID(), arr); - } else { - followMap.get(leader.getDeviceID()).add(motorPWM); + followMap.put(leader.getDeviceID(), new CopyOnWriteArrayList<>()); } + followMap.values().forEach(followerList -> followerList.remove(this)); + followMap.get(leader.getDeviceID()).add(this); } - - public void setInverted(boolean invert) { - isInverted = invert; + + public void setInverted(boolean invert) { + isInverted = invert; } - public boolean getInverted() { - return isInverted; + public boolean getInverted() { + return isInverted; } - + public int getDeviceID() { return portPWM; } + public ControlMode getControlMode() { return ControlMode.PercentOutput; } @Override public void close() { motorPWM.close(); - followMap.values().forEach(followList -> followList.remove(motorPWM)); + followMap.values().forEach(followerList -> followerList.remove(this)); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index d6185133..9b6a27b8 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -2,6 +2,7 @@ import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.CopyOnWriteArrayList; +import java.util.function.DoubleConsumer; import org.carlmontrobotics.lib199.DummySparkMaxAnswer; import org.carlmontrobotics.lib199.Mocks; @@ -29,8 +30,10 @@ public class MockSparkMax { private RelativeEncoder encoder; private SparkMaxPIDController pidController; private boolean isInverted; + // We need to store the function so we can remove it from the follower list when this motor is no longer a follower + private DoubleConsumer followFunction; // Since we need to keep a record of all the motor's followers - private static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); + private static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); public MockSparkMax(int port, MotorType type) { this.port = port; @@ -44,12 +47,14 @@ public MockSparkMax(int port, MotorType type) { public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); } - + public void set(double speed) { speed = (isInverted ? -1.0 : 1.0) * speed; this.speed.set(speed); if (followMap.containsKey(getDeviceId())) { - for (SimDouble motorOutput : followMap.get(getDeviceId())) motorOutput.set(speed); + // For spark maxes, the follower receives the post-leader-inversion speed and does not depend on the inversion state from setInverted + // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive + for (DoubleConsumer motorOutputSetter : followMap.get(getDeviceId())) motorOutputSetter.accept(speed); } } @@ -60,23 +65,29 @@ public REVLibError follow(CANSparkMax leader) { public REVLibError follow(CANSparkMax leader, boolean invert) { return follow(ExternalFollower.kFollowerSparkMax, leader.getDeviceId(), invert); } - + public REVLibError follow(ExternalFollower leader, int deviceID) { return follow(leader, deviceID, false); } public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { + // For spark maxes, the follower receives the post-leader-inversion speed and does not depend on the inversion state from setInverted + // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive if (!followMap.containsKey(deviceID)) { - followMap.put(deviceID, new CopyOnWriteArrayList()); + followMap.put(deviceID, new CopyOnWriteArrayList<>()); + } + if(followFunction != null) { + followMap.values().forEach(followList -> followList.remove(followFunction)); } - followMap.get(deviceID).add(speed); + double inversionMultiplier = (invert ? -1.0 : 1.0); + followMap.get(deviceID).add(newSpeed -> speed.set(inversionMultiplier * newSpeed)); return REVLibError.kOk; } - + public double get() { return speed.get(); } - + public int getDeviceId() { return port; } @@ -104,7 +115,7 @@ public REVLibError enableVoltageCompensation(double nominalVoltage) { public REVLibError disableVoltageCompensation() { return REVLibError.kOk; } - + public REVLibError setSmartCurrentLimit(int limit) { return REVLibError.kOk; } From 1c57ea73a34c65faee4baac46487a90c49c11c3d Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sun, 6 Aug 2023 23:22:22 -0700 Subject: [PATCH 02/18] bugfix to previous commit --- src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 9b6a27b8..b195f0cf 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -80,7 +80,7 @@ public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) followMap.values().forEach(followList -> followList.remove(followFunction)); } double inversionMultiplier = (invert ? -1.0 : 1.0); - followMap.get(deviceID).add(newSpeed -> speed.set(inversionMultiplier * newSpeed)); + followMap.get(deviceID).add(followFunction = (newSpeed -> speed.set(inversionMultiplier * newSpeed))); return REVLibError.kOk; } From 1e9e905d44a818aaa6e7b3f856b23c43bb2a8fd5 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Mon, 7 Aug 2023 18:33:17 -0700 Subject: [PATCH 03/18] use correct spark max encoder cpr --- .../org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java index 68ff907f..a6bc230a 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java @@ -17,8 +17,8 @@ public class MockedSparkEncoder implements AutoCloseable, Runnable { private SimDevice device; private SimDouble count; private SimDouble gearing; - // Default value for a CANEncoder - private final int countsPerRevolution = 4096; + // Default value for a NEO/NEO 550 + private final int countsPerRevolution = 42; private double velocity; private double positionConversionFactor = 1; private double velocityConversionFactor = 1; From 0cb502a112bb06c01cd2c61cdf7f6a21a1e49017 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Mon, 7 Aug 2023 19:03:05 -0700 Subject: [PATCH 04/18] more features --- .../lib199/sim/MockPhoenixController.java | 8 +++ .../lib199/sim/MockSparkMax.java | 53 ++++++++++++++----- 2 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java index 971bda1f..2ddb98ff 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockPhoenixController.java @@ -68,4 +68,12 @@ public void close() { motorPWM.close(); followMap.values().forEach(followerList -> followerList.remove(this)); } + + public void disable() { + set(0); + } + + public void setVoltage(double outputVolts) { + set(outputVolts / 12.0); + } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index b195f0cf..e7cd094e 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -10,17 +10,20 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ExternalFollower; -import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxRelativeEncoder; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; public class MockSparkMax { + + public static final double defaultNominalVoltage = 12.0; + // Assign the CAN port to a PWM port so it works with the simulator. Not a fan // of this solution though // CAN ports should be separate from PWM ports @@ -30,6 +33,7 @@ public class MockSparkMax { private RelativeEncoder encoder; private SparkMaxPIDController pidController; private boolean isInverted; + private double voltageCompensationNominalVoltage = defaultNominalVoltage; // We need to store the function so we can remove it from the follower list when this motor is no longer a follower private DoubleConsumer followFunction; // Since we need to keep a record of all the motor's followers @@ -49,6 +53,7 @@ public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { } public void set(double speed) { + speed *= voltageCompensationNominalVoltage / defaultNominalVoltage; speed = (isInverted ? -1.0 : 1.0) * speed; this.speed.set(speed); if (followMap.containsKey(getDeviceId())) { @@ -76,14 +81,24 @@ public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) if (!followMap.containsKey(deviceID)) { followMap.put(deviceID, new CopyOnWriteArrayList<>()); } - if(followFunction != null) { + if(isFollower()) { followMap.values().forEach(followList -> followList.remove(followFunction)); } double inversionMultiplier = (invert ? -1.0 : 1.0); - followMap.get(deviceID).add(followFunction = (newSpeed -> speed.set(inversionMultiplier * newSpeed))); + // Because ExternalFollower does not implement equals, this could result in bugs if the user passes in a custom ExternalFollower object, + // but I think that it's unlikely and users should use the builtin definitions anyway + if(leader.equals(ExternalFollower.kFollowerSparkMax)) { + followMap.get(deviceID).add(followFunction = (newSpeed -> speed.set(inversionMultiplier * newSpeed))); + } else if(leader.equals(ExternalFollower.kFollowerPhoenix)) { + MockPhoenixController.followMap.get(deviceID).add(followFunction = (newSpeed -> speed.set(inversionMultiplier * newSpeed))); + } return REVLibError.kOk; } + public boolean isFollower() { + return followFunction != null; + } + public double get() { return speed.get(); } @@ -96,28 +111,30 @@ public RelativeEncoder getEncoder() { return encoder; } - public void setInverted(boolean inverted) { - isInverted = inverted; + public RelativeEncoder getEncoder(SparkMaxRelativeEncoder.Type type, int countsPerRev) { + return getEncoder(); } - public REVLibError restoreFactoryDefaults() { - return REVLibError.kOk; + public void setInverted(boolean inverted) { + isInverted = inverted; } - public REVLibError setIdleMode(IdleMode mode) { - return REVLibError.kOk; + public boolean getInverted() { + return isInverted; } public REVLibError enableVoltageCompensation(double nominalVoltage) { + voltageCompensationNominalVoltage = nominalVoltage; return REVLibError.kOk; } public REVLibError disableVoltageCompensation() { + voltageCompensationNominalVoltage = defaultNominalVoltage; return REVLibError.kOk; } - public REVLibError setSmartCurrentLimit(int limit) { - return REVLibError.kOk; + public double getVoltageCompensationNominalVoltage() { + return voltageCompensationNominalVoltage; } public SparkMaxPIDController getPIDController() { @@ -127,4 +144,16 @@ public SparkMaxPIDController getPIDController() { public void setVoltage(double outputVolts) { set(outputVolts / 12); } -} \ No newline at end of file + + public void disable() { + set(0); + } + + public double getAppliedOutput() { + return get(); + } + + public double getBusVoltage() { + return defaultNominalVoltage; + } +} From 8983fb37f2fa8773f61a0b8f69e9f3db2fb303d1 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Mon, 7 Aug 2023 23:07:36 -0700 Subject: [PATCH 05/18] remove unnecessary comment --- .../java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index e7cd094e..2de956c5 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -24,9 +24,6 @@ public class MockSparkMax { public static final double defaultNominalVoltage = 12.0; - // Assign the CAN port to a PWM port so it works with the simulator. Not a fan - // of this solution though - // CAN ports should be separate from PWM ports private final int port; private final SimDevice motor; private final SimDouble speed; From bb00f0dc3933ef937632e51c0c8b578de9b7a6e4 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 11 Aug 2023 20:54:48 -0700 Subject: [PATCH 06/18] create MockedMotorBase and MockedSparkMaxPIDController --- .../lib199/sim/MockedMotorBase.java | 51 +++ .../sim/MockedSparkMaxPIDController.java | 423 +++++++++++++++++- 2 files changed, 451 insertions(+), 23 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java new file mode 100644 index 00000000..8d8ff81f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -0,0 +1,51 @@ +package org.carlmontrobotics.lib199.sim; + +import org.carlmontrobotics.lib199.Lib199Subsystem; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.filter.SlewRateLimiter; + +public class MockedMotorBase implements Runnable { + + public final SimDevice device; + public final SimDouble speed; + public final SimDouble neutralDeadband; + public final SimBoolean brakeModeEnabled; + private SlewRateLimiter rampRateLimiter = null; + private double requestedSpeedPercent = 0.0; + + public MockedMotorBase(String type, int port) { + device = SimDevice.create(type, port); + speed = device.createDouble("Speed", Direction.kOutput, 0.0); + neutralDeadband = device.createDouble("Neutral Deadband", Direction.kOutput, 0.04); + brakeModeEnabled = device.createBoolean("Brake Mode", Direction.kOutput, true); + + Lib199Subsystem.registerAsyncSimulationPeriodic(this); + } + + public void set(double percent) { + requestedSpeedPercent = percent; + } + + public void setNeutralDeadband(double deadbandPercent) { + this.neutralDeadband.set(Math.abs(deadbandPercent)); + } + + public void setBrakeMode(boolean brakeMode) { + this.brakeModeEnabled.set(brakeMode); + } + + public void setRampRate(double rampRatePercentPerSec) { + rampRatePercentPerSec = Math.abs(rampRatePercentPerSec); + rampRateLimiter = new SlewRateLimiter(rampRatePercentPerSec, -rampRatePercentPerSec, speed.get()); + } + + @Override + public void run() { + speed.set(rampRateLimiter.calculate(requestedSpeedPercent)); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index a773758a..1cbe318b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -1,64 +1,441 @@ package org.carlmontrobotics.lib199.sim; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxAlternateEncoder; +import com.revrobotics.SparkMaxAnalogSensor; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.SparkMaxRelativeEncoder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +// NOT THREAD SAFE public class MockedSparkMaxPIDController { - private PIDController pidController; + + public final Map slots = new ConcurrentHashMap<>(); + public Slot activeSlot; + public CANSparkMax.ControlType controlType = CANSparkMax.ControlType.kDutyCycle; + public double setpoint = 0.0; + public double arbFF = 0.0; + public FeedbackDevice feedbackDevice; + public boolean positionPIDWrappingEnabled = false; + public double positionPIDWrappingMinInput = 0.0; + public double positionPIDWrappingMaxInput = 0.0; public MockedSparkMaxPIDController() { - pidController = new PIDController(0.0, 0.0, 0.0); + slots.put(0, activeSlot = new Slot(positionPIDWrappingMinInput, positionPIDWrappingMaxInput, positionPIDWrappingEnabled)); } - public REVLibError setP(double gain) { - pidController.setP(gain); + public double getD() { + return getD(0); + } + + public double getD(int slotID) { + Slot slot = getSlot(slotID); + return slot.pidController.getD(); + } + + public double getFF() { + return getFF(0); + } + + public double getFF(int slotID) { + return getSlot(slotID).ff; + } + + public double getI() { + return getI(0); + } + + public double getI(int slotID) { + Slot slot = getSlot(slotID); + return slot.pidController.getI(); + } + + public double getIAccum() { + System.err.println("WARNING (MockedSparkMaxPIDController): getIAccum() is not currently implemented"); + return 0; + } + + public double getIMaxAccum(int slotID) { + return getSlot(slotID).iMaxAccum; + } + + public double getIZone() { + return getIZone(0); + } + + public double getIZone(int slotID) { + return 0; + } + + public double getOutputMax() { + return getOutputMax(0); + } + + public double getOutputMax(int slotID) { + return getSlot(slotID).outputMax; + } + + public double getOutputMin() { + return getOutputMin(0); + } + + public double getOutputMin(int slotID) { + return getSlot(slotID).outputMin; + } + + public boolean getPositionPIDWrappingEnabled() { + return positionPIDWrappingEnabled; + + } + + public double getPositionPIDWrappingMaxInput() { + return positionPIDWrappingMaxInput; + } + + public double getPositionPIDWrappingMinInput() { + return positionPIDWrappingMinInput; + } + + public REVLibError setPositionPIDWrappingEnable(boolean enable) { + if(enable == positionPIDWrappingEnabled) return REVLibError.kOk; + positionPIDWrappingEnabled = enable; + slots.values().forEach(slot -> { + if(enable) { + slot.pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + slot.profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + } else { + slot.pidController.disableContinuousInput(); + slot.profiledPIDController.disableContinuousInput(); + } + }); return REVLibError.kOk; } - public REVLibError setP(double gain, int slotID) { - return setP(gain); + public REVLibError setPositionPIDWrappingMaxInput(double max) { + if(max == positionPIDWrappingMaxInput) return REVLibError.kOk; + positionPIDWrappingMaxInput = max; + slots.values().forEach(slot -> { + slot.pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + slot.profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + }); + return REVLibError.kOk; + } + + public REVLibError setPositionPIDWrappingMinInput(double min) { + if(min == positionPIDWrappingMinInput) return REVLibError.kOk; + positionPIDWrappingMinInput = min; + slots.values().forEach(slot -> { + slot.pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + slot.profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + }); + return REVLibError.kOk; } public double getP() { - return pidController.getP(); + return getP(0); } public double getP(int slotID) { - return getP(); + Slot slot = getSlot(slotID); + return slot.pidController.getP(); } - public REVLibError setI(double gain) { - pidController.setI(gain); + public SparkMaxPIDController.AccelStrategy getSmartMotionAccelStrategy(int slotID) { + return SparkMaxPIDController.AccelStrategy.kTrapezoidal; + } + + public double getSmartMotionAllowedClosedLoopError(int slotID) { + return getSlot(slotID).profiledPIDController.getPositionTolerance(); + } + + public double getSmartMotionMaxAccel(int slotID) { + return getSlot(slotID).constraints.maxAcceleration; + } + + public double getSmartMotionMaxVelocity(int slotID) { + return getSlot(slotID).constraints.maxVelocity; + } + + public double getSmartMotionMinOutputVelocity(int slotID) { + return getSlot(slotID).constraints.maxAcceleration; + } + + public REVLibError setD(double gain) { + return setD(gain, 0); + } + + public REVLibError setD(double gain, int slotID) { + Slot slot = getSlot(slotID); + slot.pidController.setD(gain); + slot.profiledPIDController.setD(gain); return REVLibError.kOk; } + public REVLibError setFeedbackDevice(MotorFeedbackSensor sensor) { + if (sensor instanceof SparkMaxRelativeEncoder + || sensor instanceof SparkMaxAlternateEncoder + || sensor instanceof SparkMaxAnalogSensor + || sensor instanceof SparkMaxAbsoluteEncoder) { + if (sensor instanceof SparkMaxRelativeEncoder) { + SparkMaxRelativeEncoder encoder = (SparkMaxRelativeEncoder) sensor; + feedbackDevice = new FeedbackDevice() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + + public double getVelocity() { + return encoder.getVelocity(); + } + }; + } else if (sensor instanceof SparkMaxAlternateEncoder) { + SparkMaxAlternateEncoder encoder = (SparkMaxAlternateEncoder) sensor; + feedbackDevice = new FeedbackDevice() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + + public double getVelocity() { + return encoder.getVelocity(); + } + }; + } else if (sensor instanceof SparkMaxAnalogSensor) { + SparkMaxAnalogSensor encoder = (SparkMaxAnalogSensor) sensor; + feedbackDevice = new FeedbackDevice() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + + public double getVelocity() { + return encoder.getVelocity(); + } + }; + } else { + SparkMaxAbsoluteEncoder encoder = (SparkMaxAbsoluteEncoder) sensor; + feedbackDevice = new FeedbackDevice() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + + public double getVelocity() { + return encoder.getVelocity(); + } + }; + } + return REVLibError.kOk; + } else { + // Right now, the SPARK MAX does not support sensors that are not directly connected to itself + throw new IllegalArgumentException( + sensor.getClass().getSimpleName() + + " cannot be used as a feedback device for a SPARK MAX at this time"); + } + } + + public REVLibError setFF(double gain) { + return setFF(gain, 0); + } + + public REVLibError setFF(double gain, int slotID) { + Slot slot = getSlot(slotID); + slot.ff = gain; + return REVLibError.kOk; + } + + public REVLibError setI(double gain) { + return setI(gain, 0); + } + public REVLibError setI(double gain, int slotID) { - return setI(gain); + Slot slot = getSlot(slotID); + slot.pidController.setI(gain); + slot.profiledPIDController.setI(gain); + return REVLibError.kOk; } - public double getI() { - return pidController.getI(); + public REVLibError setIAccum(double iAccum) { + System.err.println("WARNING (MockedSparkMaxPIDController): setIAccum() is not currently implemented"); + return REVLibError.kOk; } - public double getI(int slotID) { - return getI(); + public REVLibError setIMaxAccum(double iMaxAccum, int slotID) { + Slot slot = getSlot(slotID); + slot.pidController.setIntegratorRange(-iMaxAccum, iMaxAccum); + slot.profiledPIDController.setIntegratorRange(-iMaxAccum, iMaxAccum); + slot.iMaxAccum = iMaxAccum; + return REVLibError.kOk; } - public REVLibError setD(double gain) { - pidController.setD(gain); + public REVLibError setIZone(double IZone) { + return setIZone(IZone, 0); + } + + public REVLibError setIZone(double IZone, int slotID) { + System.err.println("WARNING (MockedSparkMaxPIDController): setIZone() is not currently implemented"); return REVLibError.kOk; } - public REVLibError setD(double gain, int slotID) { - return setD(gain); + public REVLibError setOutputRange(double min, double max) { + return setOutputRange(min, max, 0); } - public double getD() { - return pidController.getD(); + public REVLibError setOutputRange(double min, double max, int slotID) { + Slot slot = getSlot(slotID); + slot.outputMin = min; + slot.outputMax = max; + return REVLibError.kOk; } - public double getD(int slotID) { - return getD(); + public REVLibError setP(double gain) { + return setP(gain, 0); } + + public REVLibError setP(double gain, int slotID) { + Slot slot = getSlot(slotID); + slot.pidController.setP(gain); + slot.profiledPIDController.setP(gain); + return REVLibError.kOk; + } + + public REVLibError setReference(double value, CANSparkMax.ControlType ctrl) { + return setReference(value, ctrl, 0); + } + + public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot) { + return setReference(value, ctrl, pidSlot, 0); + } + + public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward) { + return setReference(value, ctrl, pidSlot, arbFeedforward, SparkMaxPIDController.ArbFFUnits.kVoltage); + } + + public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward, SparkMaxPIDController.ArbFFUnits arbFFUnits) { + if(ctrl == CANSparkMax.ControlType.kSmartVelocity) { + throw new IllegalArgumentException("WARNING (MockedSparkMaxPIDController): setReference() with ControlType.kSmartVelocity is not currently implemented"); + } + + setpoint = value; + controlType = ctrl; + + activeSlot = getSlot(pidSlot); + + switch(arbFFUnits) { + case kVoltage: + break; + case kPercentOut: + arbFeedforward *= 12.0; + break; + default: + throw new IllegalArgumentException("Unsupported ArbFFUnits: " + arbFFUnits); + } + + this.arbFF = arbFeedforward; + + return REVLibError.kOk; + } + + public double calculate(double currentDraw) { + double output = 0; + switch(controlType) { + case kDutyCycle: + return setpoint; + case kVoltage: + return setpoint / 12.0; + case kPosition: + output = activeSlot.pidController.calculate(feedbackDevice.getPosition(), setpoint); + break; + case kVelocity: + output = activeSlot.pidController.calculate(feedbackDevice.getVelocity(), setpoint); + break; + case kSmartMotion: + output = activeSlot.profiledPIDController.calculate(feedbackDevice.getPosition(), setpoint); + if(Math.abs(activeSlot.profiledPIDController.getGoal().velocity) < activeSlot.smartMotionMinVelocity) { + output = 0; + } + break; + case kCurrent: + output = activeSlot.pidController.calculate(currentDraw, setpoint); + break; + default: + throw new IllegalArgumentException("Unsupported ControlType: " + controlType); + } + output += activeSlot.ff * setpoint + arbFF; + output = MathUtil.clamp(output, activeSlot.outputMin, activeSlot.outputMax); + return output; + } + + public REVLibError setSmartMotionAccelStrategy(SparkMaxPIDController.AccelStrategy accelStrategy, int slotID) { + if(accelStrategy != SparkMaxPIDController.AccelStrategy.kTrapezoidal) { + System.err.println("(MockedSparkMaxPIDController) Ignoring command to set accel strategy on slot " + slotID + " to " + accelStrategy + ". Only AccelStrategy.kTrapezoidal is supported."); + } + return REVLibError.kOk; + } + + public REVLibError setSmartMotionAllowedClosedLoopError(double allowedErr, int slotId) { + getSlot(slotId).profiledPIDController.setTolerance(allowedErr); + return REVLibError.kOk; + } + + public REVLibError setSmartMotionMaxAccel(double maxAccel, int slotID) { + Slot slot = getSlot(slotID); + slot.constraints = new TrapezoidProfile.Constraints(slot.constraints.maxVelocity, maxAccel); + return REVLibError.kOk; + } + + public REVLibError setSmartMotionMaxVelocity(double maxVel, int slotID) { + Slot slot = getSlot(slotID); + slot.constraints = new TrapezoidProfile.Constraints(maxVel, slot.constraints.maxAcceleration); + return REVLibError.kOk; + } + + public REVLibError setSmartMotionMinOutputVelocity(double minVel, int slotID) { + getSlot(slotID).smartMotionMinVelocity = minVel; + return REVLibError.kOk; + } + + public Slot getSlot(int slotID) { + return slots.computeIfAbsent(slotID, id -> new Slot(positionPIDWrappingMinInput, positionPIDWrappingMaxInput, positionPIDWrappingEnabled)); + } + + public static class Slot { + private PIDController pidController = new PIDController(0, 0, 0); + private TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0, 0); + private ProfiledPIDController profiledPIDController = new ProfiledPIDController(0, 0, 0, constraints); + private double ff = 0; + private double smartMotionMinVelocity = 0; + private double outputMin = -1; + private double outputMax = 1; + private double iMaxAccum = 0; + + public Slot(double positionPIDWrappingMinInput, double positionPIDWrappingMaxInput, boolean positionPIDWrappingEnabled) { + if(positionPIDWrappingEnabled) { + pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); + } else { + pidController.disableContinuousInput(); + profiledPIDController.disableContinuousInput(); + } + } + } + + public static interface FeedbackDevice { + + public double getPosition(); + + public double getVelocity(); + + } + } From e5a5b2972a3ba735e6be707390ac6fe2df4dbac6 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sun, 13 Aug 2023 03:59:45 -0700 Subject: [PATCH 07/18] more spark max simulation functionality --- .../lib199/sim/MockSparkMax.java | 191 ++++++++++++------ .../lib199/sim/MockedEncoder.java | 156 ++++++++++++++ .../lib199/sim/MockedMotorBase.java | 149 +++++++++++++- .../lib199/sim/MockedSparkEncoder.java | 93 --------- .../sim/MockedSparkMaxPIDController.java | 124 ++++++++---- 5 files changed, 508 insertions(+), 205 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java delete mode 100644 src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 2de956c5..16a83f91 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -1,8 +1,6 @@ package org.carlmontrobotics.lib199.sim; import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.CopyOnWriteArrayList; -import java.util.function.DoubleConsumer; import org.carlmontrobotics.lib199.DummySparkMaxAnswer; import org.carlmontrobotics.lib199.Mocks; @@ -13,51 +11,70 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAbsoluteEncoder; +import com.revrobotics.SparkMaxAlternateEncoder; +import com.revrobotics.SparkMaxAnalogSensor; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxRelativeEncoder; +import com.revrobotics.SparkMaxRelativeEncoder.Type; import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; -public class MockSparkMax { +public class MockSparkMax extends MockedMotorBase { - public static final double defaultNominalVoltage = 12.0; + private static final ConcurrentHashMap controllers = new ConcurrentHashMap<>(); - private final int port; - private final SimDevice motor; - private final SimDouble speed; - private RelativeEncoder encoder; - private SparkMaxPIDController pidController; - private boolean isInverted; - private double voltageCompensationNominalVoltage = defaultNominalVoltage; - // We need to store the function so we can remove it from the follower list when this motor is no longer a follower - private DoubleConsumer followFunction; - // Since we need to keep a record of all the motor's followers - private static ConcurrentHashMap> followMap = new ConcurrentHashMap<>(); + public final MotorType type; + private final MockedEncoder encoder; + private final SparkMaxPIDController pidController; + private final MockedSparkMaxPIDController pidControllerImpl; + private SparkMaxAbsoluteEncoder absoluteEncoder = null; + private MockedEncoder alternateEncoder = null; + private SparkMaxAnalogSensor analogSensor = null; public MockSparkMax(int port, MotorType type) { - this.port = port; - motor = SimDevice.create("SparkMax", port); - speed = motor.createDouble("Motor Output", Direction.kOutput, 0); - encoder = Mocks.createMock(RelativeEncoder.class, new MockedSparkEncoder(port), new REVLibErrorAnswer()); - pidController = Mocks.createMock(SparkMaxPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); - isInverted = false; + super("SparkMax", port, false); + this.type = type; + + if(type == MotorType.kBrushless) { + encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.builtinEncoderCountsPerRev, false) { + @Override + public REVLibError setInverted(boolean inverted) { + System.err.println( + "(MockedEncoder) SparkMaxRelativeEncoder cannot be inverted separately from the motor in brushless mode!"); + return REVLibError.kParamInvalid; + } + }; + } else { + encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.builtinEncoderCountsPerRev, false); + } + + pidControllerImpl = new MockedSparkMaxPIDController(this); + pidController = Mocks.createMock(SparkMaxPIDController.class, pidControllerImpl, new REVLibErrorAnswer()); + pidController.setFeedbackDevice(encoder); + + controllers.put(port, this); + } + + @Override + public double getRequestedSpeed() { + return pidControllerImpl.calculate(getCurrentDraw()); + } + + public static MockSparkMax getControllerWithId(int port) { + return controllers.get(port); } public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); } + @Override public void set(double speed) { speed *= voltageCompensationNominalVoltage / defaultNominalVoltage; speed = (isInverted ? -1.0 : 1.0) * speed; - this.speed.set(speed); - if (followMap.containsKey(getDeviceId())) { - // For spark maxes, the follower receives the post-leader-inversion speed and does not depend on the inversion state from setInverted - // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive - for (DoubleConsumer motorOutputSetter : followMap.get(getDeviceId())) motorOutputSetter.accept(speed); - } + pidControllerImpl.setDutyCycle(speed); } public REVLibError follow(CANSparkMax leader) { @@ -65,7 +82,8 @@ public REVLibError follow(CANSparkMax leader) { } public REVLibError follow(CANSparkMax leader, boolean invert) { - return follow(ExternalFollower.kFollowerSparkMax, leader.getDeviceId(), invert); + pidControllerImpl.follow(leader, invert); // No need to lookup the spark max if we already have it + return REVLibError.kOk; } public REVLibError follow(ExternalFollower leader, int deviceID) { @@ -73,31 +91,24 @@ public REVLibError follow(ExternalFollower leader, int deviceID) { } public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) { - // For spark maxes, the follower receives the post-leader-inversion speed and does not depend on the inversion state from setInverted - // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive - if (!followMap.containsKey(deviceID)) { - followMap.put(deviceID, new CopyOnWriteArrayList<>()); - } - if(isFollower()) { - followMap.values().forEach(followList -> followList.remove(followFunction)); - } - double inversionMultiplier = (invert ? -1.0 : 1.0); + MotorController controller = null; // Because ExternalFollower does not implement equals, this could result in bugs if the user passes in a custom ExternalFollower object, // but I think that it's unlikely and users should use the builtin definitions anyway if(leader.equals(ExternalFollower.kFollowerSparkMax)) { - followMap.get(deviceID).add(followFunction = (newSpeed -> speed.set(inversionMultiplier * newSpeed))); + controller = getControllerWithId(deviceID); } else if(leader.equals(ExternalFollower.kFollowerPhoenix)) { - MockPhoenixController.followMap.get(deviceID).add(followFunction = (newSpeed -> speed.set(inversionMultiplier * newSpeed))); + // controller = MockPhoenixController.getControllerWithId(deviceID); + } + if(controller == null) { + System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); + return REVLibError.kFollowConfigMismatch; } + pidControllerImpl.follow(controller, invert); return REVLibError.kOk; } public boolean isFollower() { - return followFunction != null; - } - - public double get() { - return speed.get(); + return pidControllerImpl.isFollower(); } public int getDeviceId() { @@ -109,48 +120,100 @@ public RelativeEncoder getEncoder() { } public RelativeEncoder getEncoder(SparkMaxRelativeEncoder.Type type, int countsPerRev) { + if(type != Type.kHallSensor) { + System.err.println("Error: MockSparkMax only supports hall effect encoders"); + return null; + } return getEncoder(); } + @Override public void setInverted(boolean inverted) { - isInverted = inverted; - } + super.setInverted(inverted); - public boolean getInverted() { - return isInverted; + // Set the encoder inversion directly to avoid the error message + if(type == MotorType.kBrushless) encoder.inverted = inverted; } public REVLibError enableVoltageCompensation(double nominalVoltage) { - voltageCompensationNominalVoltage = nominalVoltage; + super.doEnableVoltageCompensation(nominalVoltage); return REVLibError.kOk; } public REVLibError disableVoltageCompensation() { - voltageCompensationNominalVoltage = defaultNominalVoltage; + super.doDisableVoltageCompensation(); return REVLibError.kOk; } - public double getVoltageCompensationNominalVoltage() { - return voltageCompensationNominalVoltage; - } - public SparkMaxPIDController getPIDController() { return pidController; } - public void setVoltage(double outputVolts) { - set(outputVolts / 12); + public double getAppliedOutput() { + // MockedMotorBase returns speed before rate limiting. + // The current output is the speed after rate limiting. + return (isInverted ? -1.0 : 1.0) * speed.get(); } - public void disable() { - set(0); + public double getBusVoltage() { + return defaultNominalVoltage; } - public double getAppliedOutput() { - return get(); + @Override + public void close() { + controllers.remove(port); + super.close(); } - public double getBusVoltage() { - return defaultNominalVoltage; + public REVLibError enableSoftLimit​(CANSparkMax.SoftLimitDirection direction, boolean enable) { + System.err.println("Error: MockSparkMax does not support soft limits"); + return REVLibError.kNotImplemented; + } + + public synchronized SparkMaxAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type encoderType) { + System.err.println("WARNING: An absolute encoder was created for a simulated Spark Max. Currently, the only way to specify the CPR is to use the REVHardwareClient. A CPR of " + MockedEncoder.builtinEncoderCountsPerRev + " will be assumed."); + if(absoluteEncoder == null) { + MockedEncoder absoluteEncoderImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AbsoluteEncoder"), MockedEncoder.builtinEncoderCountsPerRev, true); + absoluteEncoder = Mocks.createMock(SparkMaxAbsoluteEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer()); + } + return absoluteEncoder; + } + + public RelativeEncoder getAlternateEncoder(int countsPerRev) { + return getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRev); + } + + public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) { + if(alternateEncoder == null) { + alternateEncoder = new MockedEncoder(SimDevice.create(device.getName() + "_AlternateEncoder"), countsPerRev, false); + } + return alternateEncoder; + } + + public synchronized SparkMaxAnalogSensor getAnalog(SparkMaxAnalogSensor.Mode mode) { + if(analogSensor == null) { + MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.analogSensorCPR, true); + analogSensor = Mocks.createMock(SparkMaxAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer()); + } + return analogSensor; } + + public double getClosedLoopRampRate() { + return getRampRateClosedLoop(); + } + + public double getOpenLoopRampRate() { + return getRampRateOpenLoop(); + } + + public REVLibError setClosedLoopRampRate(double secondsFromNeutralToFull) { + setRampRateClosedLoop(secondsFromNeutralToFull); + return REVLibError.kOk; + } + + public REVLibError setOpenLoopRampRate(double secondsFromNeutralToFull) { + setRampRateOpenLoop(secondsFromNeutralToFull); + return REVLibError.kOk; + } + } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java new file mode 100644 index 00000000..0ca37c63 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -0,0 +1,156 @@ +package org.carlmontrobotics.lib199.sim; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.AnalogInput; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.hal.SimDouble; + +public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseable, RelativeEncoder { + + public static final int builtinEncoderCountsPerRev = 42; + public static final double analogSensorMaxVoltage = 3.3; + public static final int analogSensorCPR = 8192; + + public final SimDevice device; + protected final SimDouble position; + protected final SimDouble velocity; + protected final int countsPerRev; + protected final boolean absolute; + protected double positionConversionFactor = 1.0; + protected double velocityConversionFactor = 1.0; + protected double positionOffset = 0.0; + protected boolean inverted = false; + + public MockedEncoder(SimDevice device, int countsPerRev, boolean absolute) { + this.device = device; + position = device.createDouble("Position", Direction.kInput, 0); + velocity = device.createDouble("Velocity", Direction.kInput, 0); + this.countsPerRev = countsPerRev; + this.absolute = absolute; + } + + @Override + public REVLibError setPosition(double position) { + if (absolute) { + System.err.println("(MockedEncoder) setPosition cannot be called on an absolute encoder"); + return REVLibError.kParamAccessMode; + } + positionOffset = position - getRawPosition(); + return REVLibError.kOk; + } + + @Override + public REVLibError setMeasurementPeriod(int period_ms) { + System.err.println("(MockedEncoder) setMeasurementPeriod not implemented"); + return REVLibError.kNotImplemented; + } + + @Override + public int getMeasurementPeriod() { + System.err.println("(MockedEncoder) getMeasurementPeriod not implemented"); + return 0; + } + + @Override + public int getCountsPerRevolution() { + return countsPerRev; + } + + public double getRawPosition() { + return position.get() * (inverted ? -1 : 1) * positionConversionFactor / countsPerRev; + } + + @Override + public double getPosition() { + if (absolute) { + return MathUtil.inputModulus(getRawPosition(), 0, positionConversionFactor) + positionOffset; + } else { + return getRawPosition() + positionOffset; + } + } + + @Override + public double getVelocity() { + return velocity.get() * (inverted ? -1 : 1) * velocityConversionFactor / countsPerRev; + } + + @Override + public REVLibError setPositionConversionFactor(double factor) { + positionConversionFactor = factor; + return REVLibError.kOk; + } + + @Override + public double getPositionConversionFactor() { + return positionConversionFactor; + } + + @Override + public REVLibError setVelocityConversionFactor(double factor) { + velocityConversionFactor = factor; + return REVLibError.kOk; + } + + @Override + public double getVelocityConversionFactor() { + return velocityConversionFactor; + } + + @Override + public REVLibError setInverted(boolean inverted) { + this.inverted = inverted; + return REVLibError.kOk; + } + + public void setInvertedFromMotor(boolean inverted) { + this.inverted = inverted; + } + + @Override + public boolean getInverted() { + return inverted; + } + + @Override + public REVLibError setAverageDepth(int depth) { + System.err.println("(MockedEncoder) setAverageDepth not implemented"); + return REVLibError.kNotImplemented; + } + + @Override + public int getAverageDepth() { + System.err.println("(MockedEncoder) getAverageDepth not implemented"); + return 0; + } + + @Override + public REVLibError setZeroOffset(double offset) { + if (!absolute) { + System.err.println("(MockedEncoder) setZeroOffset cannot be called on a relative encoder"); + return REVLibError.kParamAccessMode; + } + positionOffset = offset; + return REVLibError.kOk; + } + + @Override + public double getZeroOffset() { + return positionOffset; + } + + @Override + public void close() { + device.close(); + } + + @Override + public double getVoltage() { + return MathUtil.inputModulus(position.get() / countsPerRev, 0, 1) * analogSensorMaxVoltage; + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index 8d8ff81f..e03d54cc 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -7,29 +7,40 @@ import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; -public class MockedMotorBase implements Runnable { +public abstract class MockedMotorBase implements AutoCloseable, MotorController, Runnable { + + public static final double defaultNominalVoltage = 12.0; public final SimDevice device; + public final int port; public final SimDouble speed; public final SimDouble neutralDeadband; public final SimBoolean brakeModeEnabled; - private SlewRateLimiter rampRateLimiter = null; + public final SimDouble currentDraw; + public final boolean allowMotorDisable; + protected SlewRateLimiter rampRateLimiter = null; + protected boolean isInverted = false; + protected boolean disabled = false; + protected double voltageCompensationNominalVoltage = defaultNominalVoltage; + protected double closedLoopRampRate = 0.0; + protected double openLoopRampRate = 0.0; + protected boolean runningClosedLoopControl = false; private double requestedSpeedPercent = 0.0; - public MockedMotorBase(String type, int port) { + public MockedMotorBase(String type, int port, boolean allowMotorDisable) { device = SimDevice.create(type, port); + this.port = port; speed = device.createDouble("Speed", Direction.kOutput, 0.0); neutralDeadband = device.createDouble("Neutral Deadband", Direction.kOutput, 0.04); brakeModeEnabled = device.createBoolean("Brake Mode", Direction.kOutput, true); + currentDraw = device.createDouble("Current Draw", Direction.kInput, 0.0); + this.allowMotorDisable = allowMotorDisable; Lib199Subsystem.registerAsyncSimulationPeriodic(this); } - public void set(double percent) { - requestedSpeedPercent = percent; - } - public void setNeutralDeadband(double deadbandPercent) { this.neutralDeadband.set(Math.abs(deadbandPercent)); } @@ -38,14 +49,130 @@ public void setBrakeMode(boolean brakeMode) { this.brakeModeEnabled.set(brakeMode); } - public void setRampRate(double rampRatePercentPerSec) { - rampRatePercentPerSec = Math.abs(rampRatePercentPerSec); - rampRateLimiter = new SlewRateLimiter(rampRatePercentPerSec, -rampRatePercentPerSec, speed.get()); + // The ramp rate method names look weird, but this is just to prevent clashing with the vendor methods + + public void setRampRate(double secondsFromNeutralToFull) { + if(secondsFromNeutralToFull <= 0) { + rampRateLimiter = null; + return; + } + double rateLimit = voltageCompensationNominalVoltage / secondsFromNeutralToFull; + rampRateLimiter = new SlewRateLimiter(rateLimit, -rateLimit, speed.get()); + } + + public void setRampRateClosedLoop(double secondsFromNeutralToFull) { + closedLoopRampRate = secondsFromNeutralToFull; + if(runningClosedLoopControl) setRampRate(secondsFromNeutralToFull); + } + + public void setRampRateOpenLoop(double secondsFromNeutralToFull) { + openLoopRampRate = secondsFromNeutralToFull; + if(!runningClosedLoopControl) setRampRate(secondsFromNeutralToFull); + } + + public void setClosedLoopControl(boolean enabled) { + runningClosedLoopControl = enabled; + if(enabled) { + setRampRate(closedLoopRampRate); + } else { + setRampRate(openLoopRampRate); + } + } + + public boolean getClosedLoopControl() { + return runningClosedLoopControl; + } + + public double getRampRate() { + return runningClosedLoopControl ? closedLoopRampRate : openLoopRampRate; + } + + public double getRampRateClosedLoop() { + return closedLoopRampRate; + } + + public double getRampRateOpenLoop() { + return openLoopRampRate; + } + + public void doEnableVoltageCompensation(double nominalVoltage) { + voltageCompensationNominalVoltage = nominalVoltage; + setRampRate(getRampRate()); // Update the ramp rate to account for the new nominal voltage + } + + public void doDisableVoltageCompensation() { + voltageCompensationNominalVoltage = defaultNominalVoltage; + setRampRate(getRampRate()); // Update the ramp rate to account for the new nominal voltage + } + + public double getVoltageCompensationNominalVoltage() { + return voltageCompensationNominalVoltage; + } + + public double getCurrentDraw() { + return currentDraw.get(); + } + + public void updateRequestedSpeed() { + double percent = getRequestedSpeed(); + percent *= voltageCompensationNominalVoltage / defaultNominalVoltage; + percent *= isInverted ? -1.0 : 1.0; + requestedSpeedPercent = percent; } @Override public void run() { - speed.set(rampRateLimiter.calculate(requestedSpeedPercent)); + if (disabled) { + requestedSpeedPercent = 0; + speed.set(0); + } else { + updateRequestedSpeed(); + if (rampRateLimiter == null) { + speed.set(requestedSpeedPercent); + } else { + speed.set(rampRateLimiter.calculate(requestedSpeedPercent)); + } + } + } + + public abstract double getRequestedSpeed(); + + @Override + public double get() { + return (isInverted ? -1.0 : 1.0) * requestedSpeedPercent; + } + + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + + @Override + public boolean getInverted() { + return isInverted; + } + + @Override + public void disable() { + if (allowMotorDisable) { + disabled = true; + } + set(0); + } + + @Override + public void stopMotor() { + set(0); + } + + @Override + public void setVoltage(double outputVolts) { + set(outputVolts / voltageCompensationNominalVoltage); + } + + @Override + public void close() { + device.close(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java deleted file mode 100644 index a6bc230a..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java +++ /dev/null @@ -1,93 +0,0 @@ -package org.carlmontrobotics.lib199.sim; - -import java.util.HashMap; - -import org.carlmontrobotics.lib199.Lib199Subsystem; - -import com.revrobotics.REVLibError; - -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.hal.SimDevice.Direction; - -public class MockedSparkEncoder implements AutoCloseable, Runnable { - - private static final HashMap sims = new HashMap<>(); - - private SimDevice device; - private SimDouble count; - private SimDouble gearing; - // Default value for a NEO/NEO 550 - private final int countsPerRevolution = 42; - private double velocity; - private double positionConversionFactor = 1; - private double velocityConversionFactor = 1; - private double lastCount = 0; - private double lastTime = 0; - - public MockedSparkEncoder(int id) { - device = SimDevice.create("RelativeEncoder", id); - count = device.createDouble("count", Direction.kInput, 0); - gearing = device.createDouble("gearing", Direction.kOutput, 1); - sims.put(id, this); - Lib199Subsystem.registerAsyncPeriodic(this); - } - - public double getPosition() { - return positionConversionFactor * count.get() / countsPerRevolution; - } - - public REVLibError setPositionConversionFactor(double positionConversionFactor) { - this.positionConversionFactor = positionConversionFactor; - return REVLibError.kOk; - } - - public REVLibError setPosition(double position) { - double revolutions = position / getPositionConversionFactor(); - count.set((int) Math.floor(revolutions * countsPerRevolution)); - return REVLibError.kOk; - } - - public double getPositionConversionFactor() { - return positionConversionFactor; - } - - public double getVelocity() { - return velocity; - } - - public REVLibError setVelocityConversionFactor(double velocityConversionFactor) { - this.velocityConversionFactor = velocityConversionFactor; - return REVLibError.kOk; - } - - public double getVelocityConversionFactor() { - return velocityConversionFactor; - } - - @Override - public void run() { - double t = System.currentTimeMillis() / 1000D; - double dt = t - lastTime; - double curCount = count.get(); - double dCount = curCount - lastCount; - lastTime = t; - lastCount = curCount; - double newVelocity = velocityConversionFactor * ( dCount / dt ) / countsPerRevolution; - velocity = Double.isNaN(newVelocity) ? 0 : newVelocity; - } - - @Override - public void close() { - device.close(); - } - - public void setGearing(double gearing) { - this.gearing.set(gearing); - } - - public static void setGearing(int port, double gearing) { - if(sims.containsKey(port)) sims.get(port).setGearing(gearing); - } - -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index 1cbe318b..93b9e092 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -16,13 +16,17 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; // NOT THREAD SAFE public class MockedSparkMaxPIDController { public final Map slots = new ConcurrentHashMap<>(); + public final MockedMotorBase motor; public Slot activeSlot; public CANSparkMax.ControlType controlType = CANSparkMax.ControlType.kDutyCycle; + public MotorController leader = null; + public boolean invertLeader = false; public double setpoint = 0.0; public double arbFF = 0.0; public FeedbackDevice feedbackDevice; @@ -30,10 +34,68 @@ public class MockedSparkMaxPIDController { public double positionPIDWrappingMinInput = 0.0; public double positionPIDWrappingMaxInput = 0.0; - public MockedSparkMaxPIDController() { + // Methods to interface with MockSparkMax + public double calculate(double currentDraw) { + if(leader != null) { + // For spark maxes, the follower receives the post-leader-inversion speed and does not depend on the inversion state from setInverted + // For following inversion semantics see the "Motor Inversion Testing Results" section of the "Programming Resources/Documentation" document in the the team drive + return (invertLeader ? -1 : 1) * leader.get(); + } + + double output = 0; + switch(controlType) { + case kDutyCycle: + return setpoint; + case kVoltage: + return setpoint / 12.0; + case kPosition: + output = activeSlot.pidController.calculate(feedbackDevice.getPosition(), setpoint); + break; + case kVelocity: + output = activeSlot.pidController.calculate(feedbackDevice.getVelocity(), setpoint); + break; + case kSmartMotion: + output = activeSlot.profiledPIDController.calculate(feedbackDevice.getPosition(), setpoint); + if(Math.abs(activeSlot.profiledPIDController.getGoal().velocity) < activeSlot.smartMotionMinVelocity) { + output = 0; + } + break; + case kCurrent: + output = activeSlot.pidController.calculate(currentDraw, setpoint); + break; + case kSmartVelocity: + default: + throw new IllegalArgumentException("Unsupported ControlType: " + controlType); + } + output += activeSlot.ff * setpoint + arbFF; + output = MathUtil.clamp(output, activeSlot.outputMin, activeSlot.outputMax); + return output; + } + + public void setDutyCycle(double speed) { + setpoint = speed; + controlType = CANSparkMax.ControlType.kDutyCycle; + leader = null; + motor.setClosedLoopControl(false); + } + + public void follow(MotorController leader, boolean invert) { + this.leader = leader; + invertLeader = invert; + motor.setClosedLoopControl(false); + } + + public boolean isFollower() { + return leader != null; + } + + public MockedSparkMaxPIDController(MockedMotorBase motor) { + this.motor = motor; slots.put(0, activeSlot = new Slot(positionPIDWrappingMinInput, positionPIDWrappingMaxInput, positionPIDWrappingEnabled)); } + // Overrides + public double getD() { return getD(0); } @@ -61,7 +123,7 @@ public double getI(int slotID) { } public double getIAccum() { - System.err.println("WARNING (MockedSparkMaxPIDController): getIAccum() is not currently implemented"); + System.err.println("(MockedSparkMaxPIDController): getIAccum() is not currently implemented"); return 0; } @@ -266,8 +328,8 @@ public REVLibError setI(double gain, int slotID) { } public REVLibError setIAccum(double iAccum) { - System.err.println("WARNING (MockedSparkMaxPIDController): setIAccum() is not currently implemented"); - return REVLibError.kOk; + System.err.println("(MockedSparkMaxPIDController): setIAccum() is not currently implemented"); + return REVLibError.kNotImplemented; } public REVLibError setIMaxAccum(double iMaxAccum, int slotID) { @@ -283,8 +345,8 @@ public REVLibError setIZone(double IZone) { } public REVLibError setIZone(double IZone, int slotID) { - System.err.println("WARNING (MockedSparkMaxPIDController): setIZone() is not currently implemented"); - return REVLibError.kOk; + System.err.println("(MockedSparkMaxPIDController): setIZone() is not currently implemented"); + return REVLibError.kNotImplemented; } public REVLibError setOutputRange(double min, double max) { @@ -323,11 +385,28 @@ public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int pidSlot, double arbFeedforward, SparkMaxPIDController.ArbFFUnits arbFFUnits) { if(ctrl == CANSparkMax.ControlType.kSmartVelocity) { - throw new IllegalArgumentException("WARNING (MockedSparkMaxPIDController): setReference() with ControlType.kSmartVelocity is not currently implemented"); + System.err.println("(MockedSparkMaxPIDController): setReference() with ControlType.kSmartVelocity is not currently implemented"); + return REVLibError.kNotImplemented; } setpoint = value; controlType = ctrl; + leader = null; + + switch(ctrl) { + case kDutyCycle: + case kVoltage: + motor.setClosedLoopControl(false); + break; + case kPosition: + case kVelocity: + case kSmartMotion: + case kCurrent: + motor.setClosedLoopControl(true); + break; + case kSmartVelocity: + break; // This should never happen + } activeSlot = getSlot(pidSlot); @@ -346,39 +425,10 @@ public REVLibError setReference(double value, CANSparkMax.ControlType ctrl, int return REVLibError.kOk; } - public double calculate(double currentDraw) { - double output = 0; - switch(controlType) { - case kDutyCycle: - return setpoint; - case kVoltage: - return setpoint / 12.0; - case kPosition: - output = activeSlot.pidController.calculate(feedbackDevice.getPosition(), setpoint); - break; - case kVelocity: - output = activeSlot.pidController.calculate(feedbackDevice.getVelocity(), setpoint); - break; - case kSmartMotion: - output = activeSlot.profiledPIDController.calculate(feedbackDevice.getPosition(), setpoint); - if(Math.abs(activeSlot.profiledPIDController.getGoal().velocity) < activeSlot.smartMotionMinVelocity) { - output = 0; - } - break; - case kCurrent: - output = activeSlot.pidController.calculate(currentDraw, setpoint); - break; - default: - throw new IllegalArgumentException("Unsupported ControlType: " + controlType); - } - output += activeSlot.ff * setpoint + arbFF; - output = MathUtil.clamp(output, activeSlot.outputMin, activeSlot.outputMax); - return output; - } - public REVLibError setSmartMotionAccelStrategy(SparkMaxPIDController.AccelStrategy accelStrategy, int slotID) { if(accelStrategy != SparkMaxPIDController.AccelStrategy.kTrapezoidal) { System.err.println("(MockedSparkMaxPIDController) Ignoring command to set accel strategy on slot " + slotID + " to " + accelStrategy + ". Only AccelStrategy.kTrapezoidal is supported."); + return REVLibError.kParamNotImplementedDeprecated; } return REVLibError.kOk; } From 7ef6d161f034f36797afd7a13c371bf15b586bce Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Tue, 15 Aug 2023 00:22:14 -0700 Subject: [PATCH 08/18] MockSparkMax.setIdleMode --- .../java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 6 ++++++ .../org/carlmontrobotics/lib199/sim/MockedMotorBase.java | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 16a83f91..e1cfb9ca 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -8,6 +8,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ExternalFollower; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; @@ -216,4 +217,9 @@ public REVLibError setOpenLoopRampRate(double secondsFromNeutralToFull) { return REVLibError.kOk; } + public REVLibError setIdleMode(IdleMode mode) { + super.setBrakeModeEnabled(mode == IdleMode.kBrake); + return REVLibError.kOk; + } + } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index e03d54cc..cca4bebb 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -45,7 +45,7 @@ public void setNeutralDeadband(double deadbandPercent) { this.neutralDeadband.set(Math.abs(deadbandPercent)); } - public void setBrakeMode(boolean brakeMode) { + public void setBrakeModeEnabled(boolean brakeMode) { this.brakeModeEnabled.set(brakeMode); } From 25e2d79074eb3232736cb5ccb74d9223921c825b Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Tue, 15 Aug 2023 02:22:14 -0700 Subject: [PATCH 09/18] rename constants --- .../org/carlmontrobotics/lib199/sim/MockSparkMax.java | 10 +++++----- .../org/carlmontrobotics/lib199/sim/MockedEncoder.java | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index e1cfb9ca..8d8af297 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -39,7 +39,7 @@ public MockSparkMax(int port, MotorType type) { this.type = type; if(type == MotorType.kBrushless) { - encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.builtinEncoderCountsPerRev, false) { + encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false) { @Override public REVLibError setInverted(boolean inverted) { System.err.println( @@ -48,7 +48,7 @@ public REVLibError setInverted(boolean inverted) { } }; } else { - encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.builtinEncoderCountsPerRev, false); + encoder = new MockedEncoder(SimDevice.create(device.getName() + "_RelativeEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, false); } pidControllerImpl = new MockedSparkMaxPIDController(this); @@ -172,9 +172,9 @@ public void close() { } public synchronized SparkMaxAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type encoderType) { - System.err.println("WARNING: An absolute encoder was created for a simulated Spark Max. Currently, the only way to specify the CPR is to use the REVHardwareClient. A CPR of " + MockedEncoder.builtinEncoderCountsPerRev + " will be assumed."); + System.err.println("WARNING: An absolute encoder was created for a simulated Spark Max. Currently, the only way to specify the CPR is to use the REVHardwareClient. A CPR of " + MockedEncoder.NEO_BUILTIN_ENCODER_CPR + " will be assumed."); if(absoluteEncoder == null) { - MockedEncoder absoluteEncoderImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AbsoluteEncoder"), MockedEncoder.builtinEncoderCountsPerRev, true); + MockedEncoder absoluteEncoderImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AbsoluteEncoder"), MockedEncoder.NEO_BUILTIN_ENCODER_CPR, true); absoluteEncoder = Mocks.createMock(SparkMaxAbsoluteEncoder.class, absoluteEncoderImpl, new REVLibErrorAnswer()); } return absoluteEncoder; @@ -193,7 +193,7 @@ public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder public synchronized SparkMaxAnalogSensor getAnalog(SparkMaxAnalogSensor.Mode mode) { if(analogSensor == null) { - MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.analogSensorCPR, true); + MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.ANALOG_SENSOR_CPR, true); analogSensor = Mocks.createMock(SparkMaxAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer()); } return analogSensor; diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index 0ca37c63..a3b6f076 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -12,9 +12,9 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseable, RelativeEncoder { - public static final int builtinEncoderCountsPerRev = 42; - public static final double analogSensorMaxVoltage = 3.3; - public static final int analogSensorCPR = 8192; + public static final int NEO_BUILTIN_ENCODER_CPR = 42; + public static final double ANALOG_SENSOR_MAX_VOLTAGE = 3.3; + public static final int ANALOG_SENSOR_CPR = 8192; public final SimDevice device; protected final SimDouble position; @@ -150,7 +150,7 @@ public void close() { @Override public double getVoltage() { - return MathUtil.inputModulus(position.get() / countsPerRev, 0, 1) * analogSensorMaxVoltage; + return MathUtil.inputModulus(position.get() / countsPerRev, 0, 1) * ANALOG_SENSOR_MAX_VOLTAGE; } } From 5ce5601282c32085ade92fef5a42fbb292f57a2e Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Mon, 15 Jan 2024 01:15:41 -0800 Subject: [PATCH 10/18] stuff I apparently forgot to commit (mostly docs) --- .../lib199/sim/MockSparkMax.java | 66 +++++++++++++++++-- .../lib199/sim/MockedEncoder.java | 24 +++++-- .../lib199/sim/MockedMotorBase.java | 55 +++++++++++++--- 3 files changed, 126 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 8d8af297..12b33587 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -22,6 +22,9 @@ import edu.wpi.first.hal.SimDevice; import edu.wpi.first.wpilibj.motorcontrol.MotorController; +/** + * An extension of {@link MockedMotorBase} which implements spark-max-specific functionality + */ public class MockSparkMax extends MockedMotorBase { private static final ConcurrentHashMap controllers = new ConcurrentHashMap<>(); @@ -34,8 +37,13 @@ public class MockSparkMax extends MockedMotorBase { private MockedEncoder alternateEncoder = null; private SparkMaxAnalogSensor analogSensor = null; + /** + * @param port the port to associate this {@code MockSparkMax} with. Will be used to create the {@link SimDevice} and facilitate motor following. + * @param type the type of the simulated motor. If this is set to {@link MotorType#kBrushless}, the builtin encoder simulation will be configured + * to follow the inversion state of the motor and its {@code setInverted} method will be disabled. + */ public MockSparkMax(int port, MotorType type) { - super("SparkMax", port, false); + super("SparkMax", port); this.type = type; if(type == MotorType.kBrushless) { @@ -63,10 +71,21 @@ public double getRequestedSpeed() { return pidControllerImpl.calculate(getCurrentDraw()); } + /** + * @param port the port of the controller to search for + * @return Queries the simulated motor controller with the given port + */ public static MockSparkMax getControllerWithId(int port) { return controllers.get(port); } + /** + * Creates a simulated {@link CANSparkMax} with an instance of this class acting as the underling implementation, and forwarding all unimplemented method calls to {@link DummySparkMaxAnswer} + * @param port the port to associate this {@code MockSparkMax} with. Will be used to create the {@link SimDevice} and facilitate motor following. + * @param type the type of the simulated motor. If this is set to {@link MotorType#kBrushless}, the builtin encoder simulation will be configured + * to follow the inversion state of the motor and its {@code setInverted} method will be disabled. + * @return the simulated {@link CANSparkMax} + */ public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); } @@ -166,11 +185,14 @@ public void close() { super.close(); } - public REVLibError enableSoftLimit​(CANSparkMax.SoftLimitDirection direction, boolean enable) { - System.err.println("Error: MockSparkMax does not support soft limits"); - return REVLibError.kNotImplemented; - } - + /** + * Creates a simulated {@link SparkMaxAbsoluteEncoder} linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * For this reason, the method is also {@code synchronized}. + * + * @param encoderType ignored + * @return the simulated encoder + */ public synchronized SparkMaxAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type encoderType) { System.err.println("WARNING: An absolute encoder was created for a simulated Spark Max. Currently, the only way to specify the CPR is to use the REVHardwareClient. A CPR of " + MockedEncoder.NEO_BUILTIN_ENCODER_CPR + " will be assumed."); if(absoluteEncoder == null) { @@ -180,10 +202,27 @@ public synchronized SparkMaxAbsoluteEncoder getAbsoluteEncoder(SparkMaxAbsoluteE return absoluteEncoder; } + /** + * Creates a simulated alternate encoder linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * This means that only the first call to this method will set the CPR of the encoder. + * For this reason, the method is also {@code synchronized}. + * + * @param countsPerRev the CPR of the absolute encoder + * @return the simulated encoder + */ public RelativeEncoder getAlternateEncoder(int countsPerRev) { return getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRev); } + /** + * Creates a simulated {@link SparkMaxAbsoluteEncoder} linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * For this reason, the method is also {@code synchronized}. + * + * @param encoderType ignored + * @return the simulated encoder + */ public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder.Type encoderType, int countsPerRev) { if(alternateEncoder == null) { alternateEncoder = new MockedEncoder(SimDevice.create(device.getName() + "_AlternateEncoder"), countsPerRev, false); @@ -191,6 +230,15 @@ public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder return alternateEncoder; } + /** + * Creates a simulated {@link SparkMaxAnalogSensor} linked to this simulated controller. + * After this method has been called once, its output is cached for future invocations. + * For this reason, the method is also {@code synchronized}. + * + * @param mode setting this to {@link SparkMaxAnalogSensor.Mode#kAbsolute} makes the position relative to the position on startup. + * We will assume that this value is always zero, so this parameter has no effect. + * @return the simulated encoder + */ public synchronized SparkMaxAnalogSensor getAnalog(SparkMaxAnalogSensor.Mode mode) { if(analogSensor == null) { MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.ANALOG_SENSOR_CPR, true); @@ -222,4 +270,10 @@ public REVLibError setIdleMode(IdleMode mode) { return REVLibError.kOk; } + @Override + public void disable() { + // CANSparkMax sets the motor speed to zero rather than actually disabling the motor + set(0); + } + } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index a3b6f076..dbfe13fa 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -10,6 +10,16 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.hal.SimDouble; +/** + * Represents a base encoder class which can connect to a DeepBlueSim SimDeviceEncoderMediator. + * + * Currently this is only used for spark max simulation, pending #62. The REV classes just implement + * methods defined in {@link AbsoluteEncoder}, {@link AnalogInput}, and {@link RelativeEncoder}, so + * this class implements these interfaces to allow the compiler to check that all necessary methods + * are implemented. + * + * This class can be used as a mock implementation when needed, but if possible, it should be used directly to reduce overhead. + */ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseable, RelativeEncoder { public static final int NEO_BUILTIN_ENCODER_CPR = 42; @@ -26,6 +36,13 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl protected double positionOffset = 0.0; protected boolean inverted = false; + /** + * @param device The device to retrieve position and velocity data from + * @param countsPerRev The cpr of the simulated encoder + * @param absolute Whether the encoder is an absolute encoder. + * This flag caps the position to one rotation via. {@link MathUtil#inputModulus(double, double, double)}, + * disables {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + */ public MockedEncoder(SimDevice device, int countsPerRev, boolean absolute) { this.device = device; position = device.createDouble("Position", Direction.kInput, 0); @@ -61,6 +78,9 @@ public int getCountsPerRevolution() { return countsPerRev; } + /** + * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@link #setZeroOffset(double)}) + */ public double getRawPosition() { return position.get() * (inverted ? -1 : 1) * positionConversionFactor / countsPerRev; } @@ -107,10 +127,6 @@ public REVLibError setInverted(boolean inverted) { return REVLibError.kOk; } - public void setInvertedFromMotor(boolean inverted) { - this.inverted = inverted; - } - @Override public boolean getInverted() { return inverted; diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index cca4bebb..81de52b1 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -1,14 +1,22 @@ package org.carlmontrobotics.lib199.sim; -import org.carlmontrobotics.lib199.Lib199Subsystem; - import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import org.carlmontrobotics.lib199.Lib199Subsystem; +/** + * Represents a base encoder class which can connect to a DeepBlueSim SimDeviceMotorMediator. + * + * This class implements all {@link MotorController} methods except for {@link MotorController#set(double)}. + * Instead, subclasses implement the {@link #getRequestedSpeed()} method which is called periodically to set the + * speed of the simulated motor. + * + * Currently this is only used for spark max simulation, pending #62. + */ public abstract class MockedMotorBase implements AutoCloseable, MotorController, Runnable { public static final double defaultNominalVoltage = 12.0; @@ -19,7 +27,6 @@ public abstract class MockedMotorBase implements AutoCloseable, MotorController, public final SimDouble neutralDeadband; public final SimBoolean brakeModeEnabled; public final SimDouble currentDraw; - public final boolean allowMotorDisable; protected SlewRateLimiter rampRateLimiter = null; protected boolean isInverted = false; protected boolean disabled = false; @@ -29,28 +36,49 @@ public abstract class MockedMotorBase implements AutoCloseable, MotorController, protected boolean runningClosedLoopControl = false; private double requestedSpeedPercent = 0.0; - public MockedMotorBase(String type, int port, boolean allowMotorDisable) { + /** + * Initializes a new {@link SimDevice} with the given parameters, creates the necessary sim values, and + * registers this class's {@link #run()} method to be called asynchronously via {@link Lib199Subsystem#registerAsyncSimulationPeriodic(Runnable)}. + * + * @param type the device type name to pass to {@link SimDevice#create} + * @param port the device port to pass to {@link SimDevice#create} + */ + public MockedMotorBase(String type, int port) { device = SimDevice.create(type, port); this.port = port; speed = device.createDouble("Speed", Direction.kOutput, 0.0); neutralDeadband = device.createDouble("Neutral Deadband", Direction.kOutput, 0.04); brakeModeEnabled = device.createBoolean("Brake Mode", Direction.kOutput, true); currentDraw = device.createDouble("Current Draw", Direction.kInput, 0.0); - this.allowMotorDisable = allowMotorDisable; Lib199Subsystem.registerAsyncSimulationPeriodic(this); } + /** + * Sets the speed range in which this controller will be set to break mode. + * This value should be in the range [0, 1]. + * + * @param deadbandPercent the range in which this controller will be considered stopped + */ public void setNeutralDeadband(double deadbandPercent) { this.neutralDeadband.set(Math.abs(deadbandPercent)); } + /** + * Sets whether this controller should be in brake mode or coast mode when idle. + * + * @param brakeMode whether this controller should be in brake mode + */ public void setBrakeModeEnabled(boolean brakeMode) { this.brakeModeEnabled.set(brakeMode); } // The ramp rate method names look weird, but this is just to prevent clashing with the vendor methods + /** + * Sets the ramp rate of this controller. + * @param secondsFromNeutralToFull the number of seconds it should take to go from 0 to full speed + */ public void setRampRate(double secondsFromNeutralToFull) { if(secondsFromNeutralToFull <= 0) { rampRateLimiter = null; @@ -60,16 +88,28 @@ public void setRampRate(double secondsFromNeutralToFull) { rampRateLimiter = new SlewRateLimiter(rateLimit, -rateLimit, speed.get()); } + /** + * Sets the ramp rate of this controller when in a closed loop control mode. + * @param secondsFromNeutralToFull the number of seconds it should take to go from 0 to full speed + */ public void setRampRateClosedLoop(double secondsFromNeutralToFull) { closedLoopRampRate = secondsFromNeutralToFull; if(runningClosedLoopControl) setRampRate(secondsFromNeutralToFull); } + /** + * Sets the ramp rate of this controller when in an open loop control mode. + * @param secondsFromNeutralToFull the number of seconds it should take to go from 0 to full speed + */ public void setRampRateOpenLoop(double secondsFromNeutralToFull) { openLoopRampRate = secondsFromNeutralToFull; if(!runningClosedLoopControl) setRampRate(secondsFromNeutralToFull); } + /** + * Sets whether this controller is running in a closed or open loop loop control mode. + * @param enabled whether this controller is running in a closed loop control mode + */ public void setClosedLoopControl(boolean enabled) { runningClosedLoopControl = enabled; if(enabled) { @@ -154,10 +194,7 @@ public boolean getInverted() { @Override public void disable() { - if (allowMotorDisable) { - disabled = true; - } - set(0); + disabled = true; } @Override From 54b85fa81e253f440f0c317e04f23e71ce9ee941 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 08:35:22 -0700 Subject: [PATCH 11/18] Make tests pass. --- .../lib199/sim/MockSparkMax.java | 22 +++++++------ .../sim/MockedSparkMaxPIDController.java | 23 +++++++++++-- ...ncoderTest.java => MockedEncoderTest.java} | 28 ++++++++-------- .../sim/MockedSparkMaxPIDControllerTest.java | 32 ++++++++----------- 4 files changed, 61 insertions(+), 44 deletions(-) rename src/test/java/org/carlmontrobotics/lib199/sim/{MockedSparkEncoderTest.java => MockedEncoderTest.java} (78%) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 12b33587..01d3e149 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -114,16 +114,20 @@ public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) MotorController controller = null; // Because ExternalFollower does not implement equals, this could result in bugs if the user passes in a custom ExternalFollower object, // but I think that it's unlikely and users should use the builtin definitions anyway - if(leader.equals(ExternalFollower.kFollowerSparkMax)) { - controller = getControllerWithId(deviceID); - } else if(leader.equals(ExternalFollower.kFollowerPhoenix)) { - // controller = MockPhoenixController.getControllerWithId(deviceID); - } - if(controller == null) { - System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); - return REVLibError.kFollowConfigMismatch; + if(leader.equals(ExternalFollower.kFollowerDisabled)) { + pidControllerImpl.stopFollowing(); + } else { + if(leader.equals(ExternalFollower.kFollowerSparkMax)) { + controller = getControllerWithId(deviceID); + } else if(leader.equals(ExternalFollower.kFollowerPhoenix)) { + // controller = MockPhoenixController.getControllerWithId(deviceID); + } + if(controller == null) { + System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); + return REVLibError.kFollowConfigMismatch; + } + pidControllerImpl.follow(controller, invert); } - pidControllerImpl.follow(controller, invert); return REVLibError.kOk; } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index 93b9e092..1813a98a 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -75,7 +75,7 @@ public double calculate(double currentDraw) { public void setDutyCycle(double speed) { setpoint = speed; controlType = CANSparkMax.ControlType.kDutyCycle; - leader = null; + stopFollowing(); motor.setClosedLoopControl(false); } @@ -85,6 +85,10 @@ public void follow(MotorController leader, boolean invert) { motor.setClosedLoopControl(false); } + public void stopFollowing() { + this.leader = null; + } + public boolean isFollower() { return leader != null; } @@ -247,7 +251,8 @@ public REVLibError setFeedbackDevice(MotorFeedbackSensor sensor) { if (sensor instanceof SparkMaxRelativeEncoder || sensor instanceof SparkMaxAlternateEncoder || sensor instanceof SparkMaxAnalogSensor - || sensor instanceof SparkMaxAbsoluteEncoder) { + || sensor instanceof SparkMaxAbsoluteEncoder + || sensor instanceof MockedEncoder) { if (sensor instanceof SparkMaxRelativeEncoder) { SparkMaxRelativeEncoder encoder = (SparkMaxRelativeEncoder) sensor; feedbackDevice = new FeedbackDevice() { @@ -284,7 +289,7 @@ public double getVelocity() { return encoder.getVelocity(); } }; - } else { + } else if (sensor instanceof SparkMaxAbsoluteEncoder) { SparkMaxAbsoluteEncoder encoder = (SparkMaxAbsoluteEncoder) sensor; feedbackDevice = new FeedbackDevice() { @Override @@ -292,6 +297,18 @@ public double getPosition() { return encoder.getPosition(); } + public double getVelocity() { + return encoder.getVelocity(); + } + }; + } else { + MockedEncoder encoder = (MockedEncoder) sensor; + feedbackDevice = new FeedbackDevice() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + public double getVelocity() { return encoder.getVelocity(); } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoderTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java similarity index 78% rename from src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoderTest.java rename to src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java index dff7b958..d91a288e 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoderTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedEncoderTest.java @@ -18,10 +18,11 @@ import org.junit.Rule; import org.junit.Test; +import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.wpilibj.simulation.SimDeviceSim; -public class MockedSparkEncoderTest { +public class MockedEncoderTest { @ClassRule public static SimDeviceTestRule.Class simClassRule = new SimDeviceTestRule.Class(); @@ -36,15 +37,15 @@ public void testDeviceCreation() { } private void assertTestDeviceCreation(int id) { - String deviceName = String.format("RelativeEncoder[%d]", id); + String deviceName = String.format("testDevice[%d]", id); assertFalse(simDeviceExists(deviceName)); try(SafelyClosable closableEncoder = createEncoder(id)) { assertTrue(simDeviceExists(deviceName)); - SimDeviceSim sim = new SimDeviceSim("RelativeEncoder", id); + SimDeviceSim sim = new SimDeviceSim("testDevice", id); assertEquals(1, Stream.of(sim.enumerateValues()) .map(info -> info.name) .distinct() - .filter(name -> name.equals("count")).count()); + .filter(name -> name.equals("Position")).count()); } assertFalse(simDeviceExists(deviceName)); } @@ -61,19 +62,19 @@ public void testFunctionality() { private void testFunctionalityWithPositionConversionFactor(double factor, RelativeEncoder enc, SimDouble count) { assertEquals(REVLibError.kOk, enc.setPositionConversionFactor(factor)); assertEquals(factor, enc.getPositionConversionFactor(), 0.01); - testCount(10, enc, factor, count); - testCount(0, enc, factor, count); - testCount(-10, enc, factor, count); + testPosition(10, enc, factor, count); + testPosition(0, enc, factor, count); + testPosition(-10, enc, factor, count); } - private void testCount(double position, RelativeEncoder enc, double conversionFactor, SimDouble count) { + private void testPosition(double position, RelativeEncoder enc, double conversionFactor, SimDouble positionSim) { // This test fails with a delta of 0.01 assertEquals(REVLibError.kOk, enc.setPosition(position)); assertEquals(position, enc.getPosition(), 0.02); assertEquals(REVLibError.kOk, enc.setPosition(0)); assertEquals(0, enc.getPosition(), 0.02); - count.set(position * 4096); - assertEquals(position * conversionFactor, enc.getPosition(), 0.02); + positionSim.set(position / enc.getPositionConversionFactor() * enc.getCountsPerRevolution() + positionSim.get()); + assertEquals(position, enc.getPosition(), 0.02); } private boolean simDeviceExists(String deviceName) { @@ -85,9 +86,10 @@ private boolean simDeviceExists(String deviceName) { } private SafelyClosable createEncoder(int deviceId) { + SimDevice device = SimDevice.create("testDevice", deviceId); return (SafelyClosable)Mocks.createMock( RelativeEncoder.class, - new MockedSparkEncoder(deviceId), + new MockedEncoder(device, 4096, false), new REVLibErrorAnswer(), SafelyClosable.class); } @@ -100,8 +102,8 @@ private void withEncoders(EncoderTest func) { private void withEncoder(int id, EncoderTest func) { try(SafelyClosable encoder = createEncoder(id)) { - SimDeviceSim sim = new SimDeviceSim("RelativeEncoder", id); - SimDouble count = sim.getDouble("count"); + SimDeviceSim sim = new SimDeviceSim("testDevice", id); + SimDouble count = sim.getDouble("Position"); assertNotNull(count); func.test((RelativeEncoder)encoder, sim, count); } diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java index fc827710..0d3e5606 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDControllerTest.java @@ -8,6 +8,7 @@ import com.revrobotics.REVLibError; import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; @@ -17,37 +18,30 @@ public class MockedSparkMaxPIDControllerTest { @Test public void testResponses() { - SparkMaxPIDController mock = Mocks.createMock(SparkMaxPIDController.class, new MockedSparkMaxPIDController(), new REVLibErrorAnswer()); + MockSparkMax mockSparkMax = new MockSparkMax(0, MotorType.kBrushless); + SparkMaxPIDController mock = Mocks.createMock(SparkMaxPIDController.class, new MockedSparkMaxPIDController(mockSparkMax), new REVLibErrorAnswer()); assertSlotValueUpdate(mock::setP, mock::setP, mock::getP, mock::getP); assertSlotValueUpdate(mock::setI, mock::setI, mock::getI, mock::getI); assertSlotValueUpdate(mock::setD, mock::setD, mock::getD, mock::getD); } private void assertSlotValueUpdate(Function setFunc, BiFunction slotSetFunc, Supplier getFunc, Function slotGetFunc) { - assertSlotValueUpdate(setFunc, getFunc, slotGetFunc); - assertSlotValueUpdate(v -> slotSetFunc.apply(v, 0), getFunc, slotGetFunc); - assertSlotValueUpdate(v -> slotSetFunc.apply(v, 1), getFunc, slotGetFunc); - assertSlotValueUpdate(v -> slotSetFunc.apply(v, 2), getFunc, slotGetFunc); + assertSlotValueUpdate(setFunc, getFunc); + assertSlotValueUpdate(v -> slotSetFunc.apply(v, 0), () -> slotGetFunc.apply(0)); + assertSlotValueUpdate(v -> slotSetFunc.apply(v, 1), () -> slotGetFunc.apply(1)); + assertSlotValueUpdate(v -> slotSetFunc.apply(v, 2), () -> slotGetFunc.apply(2)); } - private void assertSlotValueUpdate(Function setFunc, Supplier getFunc, Function slotGetFunc) { + private void assertSlotValueUpdate(Function setFunc, Supplier getFunc) { assertEquals(REVLibError.kOk, setFunc.apply(0.0)); - assertSlotValueGet(0, getFunc, slotGetFunc); + assertEquals(0.0, getFunc.get(), 0.01); assertEquals(REVLibError.kOk, setFunc.apply(1.0)); - assertSlotValueGet(1, getFunc, slotGetFunc); + assertEquals(1.0, getFunc.get(), 0.01); assertEquals(REVLibError.kOk, setFunc.apply(0.5)); - assertSlotValueGet(0.5, getFunc, slotGetFunc); + assertEquals(0.5, getFunc.get(), 0.01); assertEquals(REVLibError.kOk, setFunc.apply(-0.5)); - assertSlotValueGet(-0.5, getFunc, slotGetFunc); + assertEquals(-0.5, getFunc.get(), 0.01); assertEquals(REVLibError.kOk, setFunc.apply(-1.0)); - assertSlotValueGet(-1, getFunc, slotGetFunc); + assertEquals(-1.0, getFunc.get(), 0.01); } - - private void assertSlotValueGet(double expected, Supplier getFunc, Function slotGetFunc) { - assertEquals(expected, getFunc.get(), 0.01); - assertEquals(expected, slotGetFunc.apply(0), 0.01); - assertEquals(expected, slotGetFunc.apply(1), 0.01); - assertEquals(expected, slotGetFunc.apply(2), 0.01); - } - } From 6241c5cc30d76b173f241bdb5c986f1b083d3f4d Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 08:39:24 -0700 Subject: [PATCH 12/18] Fix param name so javadoc generates without error. --- .../java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 01d3e149..7cb7492a 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -86,8 +86,8 @@ public static MockSparkMax getControllerWithId(int port) { * to follow the inversion state of the motor and its {@code setInverted} method will be disabled. * @return the simulated {@link CANSparkMax} */ - public static CANSparkMax createMockSparkMax(int portPWM, MotorType type) { - return Mocks.createMock(CANSparkMax.class, new MockSparkMax(portPWM, type), new DummySparkMaxAnswer()); + public static CANSparkMax createMockSparkMax(int port, MotorType type) { + return Mocks.createMock(CANSparkMax.class, new MockSparkMax(port, type), new DummySparkMaxAnswer()); } @Override From 9d86151bfc10f6aa412b6a6a1427ae030e687791 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 13:21:24 -0700 Subject: [PATCH 13/18] Fix misspelled method name. --- .../lib199/sim/MockedSparkMaxPIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index 1813a98a..6fc559a5 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -172,7 +172,7 @@ public double getPositionPIDWrappingMinInput() { return positionPIDWrappingMinInput; } - public REVLibError setPositionPIDWrappingEnable(boolean enable) { + public REVLibError setPositionPIDWrappingEnabled(boolean enable) { if(enable == positionPIDWrappingEnabled) return REVLibError.kOk; positionPIDWrappingEnabled = enable; slots.values().forEach(slot -> { From 1d9d08bce6145632c6ae893ff4e6f4a4aae14e7a Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 13:22:41 -0700 Subject: [PATCH 14/18] Ensure getPosition returns a value between 0 and positionConversionFactor even if there is an offset. --- .../java/org/carlmontrobotics/lib199/sim/MockedEncoder.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index dbfe13fa..524c7099 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -88,7 +88,7 @@ public double getRawPosition() { @Override public double getPosition() { if (absolute) { - return MathUtil.inputModulus(getRawPosition(), 0, positionConversionFactor) + positionOffset; + return MathUtil.inputModulus(getRawPosition() + positionOffset, 0, positionConversionFactor); } else { return getRawPosition() + positionOffset; } From 35685924f883d3f47ab597a1955ad363c4e56e77 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 13:53:42 -0700 Subject: [PATCH 15/18] Make getPosition() and getVelocity() return volts and volts/s for a MockedEncoder representing an AnalogSensor. --- .../lib199/sim/MockSparkMax.java | 2 +- .../lib199/sim/MockedEncoder.java | 17 ++++++++--------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 7cb7492a..63f6abbf 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -245,7 +245,7 @@ public synchronized RelativeEncoder getAlternateEncoder(SparkMaxAlternateEncoder */ public synchronized SparkMaxAnalogSensor getAnalog(SparkMaxAnalogSensor.Mode mode) { if(analogSensor == null) { - MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.ANALOG_SENSOR_CPR, true); + MockedEncoder analogSensorImpl = new MockedEncoder(SimDevice.create(device.getName() + "_AnalogSensor"), MockedEncoder.ANALOG_SENSOR_MAX_VOLTAGE, true); analogSensor = Mocks.createMock(SparkMaxAnalogSensor.class, analogSensorImpl, new REVLibErrorAnswer()); } return analogSensor; diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index 524c7099..f8d92f63 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -24,12 +24,11 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl public static final int NEO_BUILTIN_ENCODER_CPR = 42; public static final double ANALOG_SENSOR_MAX_VOLTAGE = 3.3; - public static final int ANALOG_SENSOR_CPR = 8192; public final SimDevice device; protected final SimDouble position; protected final SimDouble velocity; - protected final int countsPerRev; + protected final double countsOrVoltsPerRev; protected final boolean absolute; protected double positionConversionFactor = 1.0; protected double velocityConversionFactor = 1.0; @@ -38,16 +37,16 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl /** * @param device The device to retrieve position and velocity data from - * @param countsPerRev The cpr of the simulated encoder + * @param countsOrVoltsPerRev The cpr of the simulated encoder * @param absolute Whether the encoder is an absolute encoder. * This flag caps the position to one rotation via. {@link MathUtil#inputModulus(double, double, double)}, * disables {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. */ - public MockedEncoder(SimDevice device, int countsPerRev, boolean absolute) { + public MockedEncoder(SimDevice device, double countsOrVoltsPerRev, boolean absolute) { this.device = device; position = device.createDouble("Position", Direction.kInput, 0); velocity = device.createDouble("Velocity", Direction.kInput, 0); - this.countsPerRev = countsPerRev; + this.countsOrVoltsPerRev = countsOrVoltsPerRev; this.absolute = absolute; } @@ -75,14 +74,14 @@ public int getMeasurementPeriod() { @Override public int getCountsPerRevolution() { - return countsPerRev; + return (int)countsOrVoltsPerRev; } /** * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@link #setZeroOffset(double)}) */ public double getRawPosition() { - return position.get() * (inverted ? -1 : 1) * positionConversionFactor / countsPerRev; + return position.get() * (inverted ? -1 : 1) * positionConversionFactor / countsOrVoltsPerRev; } @Override @@ -96,7 +95,7 @@ public double getPosition() { @Override public double getVelocity() { - return velocity.get() * (inverted ? -1 : 1) * velocityConversionFactor / countsPerRev; + return velocity.get() * (inverted ? -1 : 1) * velocityConversionFactor / countsOrVoltsPerRev; } @Override @@ -166,7 +165,7 @@ public void close() { @Override public double getVoltage() { - return MathUtil.inputModulus(position.get() / countsPerRev, 0, 1) * ANALOG_SENSOR_MAX_VOLTAGE; + return MathUtil.inputModulus(position.get() / countsOrVoltsPerRev, 0, 1) * ANALOG_SENSOR_MAX_VOLTAGE; } } From d5d419a7290997131101c77a4e88882c15508e9c Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 13:56:45 -0700 Subject: [PATCH 16/18] Simplify getVoltage(). --- .../java/org/carlmontrobotics/lib199/sim/MockedEncoder.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index f8d92f63..b625d507 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -165,7 +165,9 @@ public void close() { @Override public double getVoltage() { - return MathUtil.inputModulus(position.get() / countsOrVoltsPerRev, 0, 1) * ANALOG_SENSOR_MAX_VOLTAGE; + // This method only makes sense for an analog sensor and for an analog sensor, + // position.get() is supposed to return volts. + return position.get(); } } From 387a059e8df7f4a030bd37c49445ff54bf4866c8 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Sun, 21 Apr 2024 14:07:19 -0700 Subject: [PATCH 17/18] Fix bug where setting wrapping bounds would enable wrapping. --- .../sim/MockedSparkMaxPIDController.java | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java index 6fc559a5..42da77f6 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkMaxPIDController.java @@ -172,11 +172,9 @@ public double getPositionPIDWrappingMinInput() { return positionPIDWrappingMinInput; } - public REVLibError setPositionPIDWrappingEnabled(boolean enable) { - if(enable == positionPIDWrappingEnabled) return REVLibError.kOk; - positionPIDWrappingEnabled = enable; + private void updatePIDWrapping() { slots.values().forEach(slot -> { - if(enable) { + if(positionPIDWrappingEnabled) { slot.pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); slot.profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); } else { @@ -184,26 +182,26 @@ public REVLibError setPositionPIDWrappingEnabled(boolean enable) { slot.profiledPIDController.disableContinuousInput(); } }); + } + + public REVLibError setPositionPIDWrappingEnabled(boolean enable) { + if(enable == positionPIDWrappingEnabled) return REVLibError.kOk; + positionPIDWrappingEnabled = enable; + updatePIDWrapping(); return REVLibError.kOk; } public REVLibError setPositionPIDWrappingMaxInput(double max) { if(max == positionPIDWrappingMaxInput) return REVLibError.kOk; positionPIDWrappingMaxInput = max; - slots.values().forEach(slot -> { - slot.pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); - slot.profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); - }); + updatePIDWrapping(); return REVLibError.kOk; } public REVLibError setPositionPIDWrappingMinInput(double min) { if(min == positionPIDWrappingMinInput) return REVLibError.kOk; positionPIDWrappingMinInput = min; - slots.values().forEach(slot -> { - slot.pidController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); - slot.profiledPIDController.enableContinuousInput(positionPIDWrappingMinInput, positionPIDWrappingMaxInput); - }); + updatePIDWrapping(); return REVLibError.kOk; } From a6cb5754b9d20e2c56bab9818f20fac0ba602376 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Sun, 21 Apr 2024 14:33:33 -0700 Subject: [PATCH 18/18] clarify variable name --- .../java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 2 +- .../java/org/carlmontrobotics/lib199/sim/MockedEncoder.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 63f6abbf..d1b91b09 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -126,7 +126,7 @@ public REVLibError follow(ExternalFollower leader, int deviceID, boolean invert) System.err.println("Error: Attempted to follow unknown motor controller: " + leader + " " + deviceID); return REVLibError.kFollowConfigMismatch; } - pidControllerImpl.follow(controller, invert); + pidControllerImpl.follow(controller, invert); } return REVLibError.kOk; } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index b625d507..23deaf32 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -51,12 +51,12 @@ public MockedEncoder(SimDevice device, double countsOrVoltsPerRev, boolean absol } @Override - public REVLibError setPosition(double position) { + public REVLibError setPosition(double newPosition) { if (absolute) { System.err.println("(MockedEncoder) setPosition cannot be called on an absolute encoder"); return REVLibError.kParamAccessMode; } - positionOffset = position - getRawPosition(); + positionOffset = newPosition - getRawPosition(); return REVLibError.kOk; }