diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 715a8022..8dc3091f 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 715a802215408f612343150f63288da967f16835 +Subproject commit 8dc3091f362d14feb4034efeb7eab4cf5e8e8686 diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 2e9c7d39..100da8a8 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -22,7 +22,9 @@ import competition.subsystems.schoocher.commands.EjectScoocherCommand; import competition.subsystems.schoocher.commands.IntakeScoocherCommand; import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.shooter.ShooterWheelTargetSpeeds; import competition.subsystems.shooter.commands.WarmUpShooterCommand; +import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -54,7 +56,10 @@ public void setupFundamentalCommands( WarmUpShooterCommand shooterWarmUp, FireCollectorCommand fireCollectorCommand, SetArmAngleCommand armAngle, - ArmMaintainerCommand armMaintainer + ArmMaintainerCommand armMaintainer, + WarmUpShooterCommand warmUpShooterSlow, + WarmUpShooterCommand warmUpShooterFast, + WarmUpShooterRPMCommand warmUpShooterDifferentialRPM ) { // Scooch oi.operatorGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get()); @@ -74,6 +79,15 @@ public void setupFundamentalCommands( var scoochNote = scoocherIntakeProvider.get(); scoochNote.alongWith(armAngle); // TODO: bind scoochNote action to a button in operatorGamepad + + warmUpShooterSlow.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SAFE); + warmUpShooterFast.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT); + + warmUpShooterDifferentialRPM.setTargetRpm(new ShooterWheelTargetSpeeds(1000, 2000)); + + oi.operatorGamepad.getPovIfAvailable(270).whileTrue(warmUpShooterSlow); + oi.operatorGamepad.getPovIfAvailable(90).whileTrue(warmUpShooterFast); + oi.operatorGamepad.getPovIfAvailable(0).whileTrue(warmUpShooterDifferentialRPM); } // Example for setting up a command to fire when a button is pressed: @@ -199,7 +213,7 @@ public void setupArmPIDCommands( oi.operatorGamepadSecond.getXboxButton(XboxButton.X).onTrue(homeArm); oi.operatorGamepadSecond.getXboxButton(XboxButton.B).onTrue(highArm); - oi.operatorGamepadSecond.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand); + oi.operatorGamepad.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand); } private SwerveSimpleTrajectoryCommand createAndConfigureTypicalSwerveCommand( diff --git a/src/main/java/competition/simulation/Simulator2024.java b/src/main/java/competition/simulation/Simulator2024.java index f770c74d..1c378a67 100644 --- a/src/main/java/competition/simulation/Simulator2024.java +++ b/src/main/java/competition/simulation/Simulator2024.java @@ -106,7 +106,7 @@ public void update() { } // The shooter wheel should pretty much always be in velocity mode. - var shooterMockMotor = (MockCANSparkMax)shooter.leader; + var shooterMockMotor = (MockCANSparkMax)shooter.upperWheelMotor; shooterVelocityCalculator.add(shooterMockMotor.getReference()); if (shooterMockMotor.getControlType() == CANSparkBase.ControlType.kVelocity) { shooterMockMotor.setVelocity(shooterVelocityCalculator.getAverage()); diff --git a/src/main/java/competition/subsystems/NeoTrellisGamepadSubsystem.java b/src/main/java/competition/subsystems/NeoTrellisGamepadSubsystem.java index 5557214d..4c22624b 100644 --- a/src/main/java/competition/subsystems/NeoTrellisGamepadSubsystem.java +++ b/src/main/java/competition/subsystems/NeoTrellisGamepadSubsystem.java @@ -61,8 +61,9 @@ public void fillButtonColors(Color8Bit color) { @Override public void refreshDataFrame() { - aKitLog.record("ButtonColors", - buttonColors.stream().map(Color8Bit::toHexString).toArray(String[]::new)); + // Temporarily commenting out, as we are running into issues with too many properties on NetworkTables + //aKitLog.record("ButtonColors", + // buttonColors.stream().map(Color8Bit::toHexString).toArray(String[]::new)); } /** diff --git a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java index 5cb0db6f..e5b19abd 100644 --- a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java +++ b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java @@ -11,6 +11,7 @@ import competition.subsystems.schoocher.ScoocherSubsystem; import competition.subsystems.schoocher.commands.StopScoocherCommand; import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.shooter.ShooterWheelTargetSpeeds; import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand; import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand; import xbot.common.injection.swerve.FrontLeftDrive; @@ -53,7 +54,7 @@ public void setupShooterWheelSubsystem(ShooterWheelSubsystem shooterWheelSubsyst ShooterWheelMaintainerCommand command, WarmUpShooterRPMCommand setToZero) { shooterWheelSubsystem.setDefaultCommand(command); - setToZero.setTargetRpm(0.0); + setToZero.setTargetRpm(new ShooterWheelTargetSpeeds(0.0, 0.0)); shooterWheelSubsystem.getSetpointLock().setDefaultCommand(setToZero); } } diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 57789990..4edef26f 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -15,6 +15,7 @@ import xbot.common.controls.actuators.XCANSparkMax; import xbot.common.controls.actuators.XSolenoid; import xbot.common.controls.sensors.XSparkAbsoluteEncoder; +import xbot.common.controls.sensors.XTimer; import xbot.common.math.MathUtils; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; @@ -33,28 +34,30 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF public ArmState armState; - public final DoubleProperty extendPower; - public final DoubleProperty retractPower; + public double extendPower; + public double retractPower; - private final DoubleProperty powerMax; - private final DoubleProperty powerMin; + public final DoubleProperty powerMax; + public final DoubleProperty powerMin; public final DoubleProperty extensionMmPerRevolution; // Millimeters private double armMotorLeftRevolutionOffset; // # of revolutions private double armMotorRightRevolutionOffset; - public final DoubleProperty upperLimitInMm; + public final DoubleProperty upperLegalLimitMm; public final DoubleProperty absoluteEncoderOffset; public final DoubleProperty absoluteEncoderRevolutionsPerArmDegree; - public final DoubleProperty softUpperLimitInMm; - public final DoubleProperty softLowerLimitInMm; - public final DoubleProperty softUpperLimitSpeed; - public final DoubleProperty softLowerLimitSpeed; - public final DoubleProperty speedLimitForNotCalibrated; + public final DoubleProperty upperSlowZoneThresholdMm; + public final DoubleProperty lowerSlowZoneThresholdMm; + public final DoubleProperty lowerExtremelySlowZoneThresholdMm; + public final DoubleProperty upperSlowZonePowerLimit; + public final DoubleProperty lowerSlowZonePowerLimit; + public final DoubleProperty lowerExtremelySlowZonePowerLimit; + public final DoubleProperty powerLimitForNotCalibrated; public final DoubleProperty angleTrim; boolean hasCalibratedLeft; boolean hasCalibratedRight; - private final DoubleProperty maximumExtensionDesyncInMm; + private final DoubleProperty maximumExtensionDesyncMm; private double targetExtension; private final DoubleProperty overallPowerClampForTesting; @@ -65,6 +68,10 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF // what angle does the arm make with the pivot when it's at our concept of zero? public final double armPivotAngleAtArmAngleZero = 45; + private double timeSinceNewTarget = -Double.MAX_VALUE; + private final DoubleProperty powerRampDurationSec; + private boolean powerRampingEnabled = true; + public enum ArmState { EXTENDING, RETRACTING, @@ -97,14 +104,14 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa pf.setPrefix(this); this.contract = contract; setArmBrakeSolenoid(false); - extendPower = pf.createPersistentProperty("ExtendPower", 0.1); - retractPower = pf.createPersistentProperty("RetractPower", 0.1); + extendPower = 0.1; + retractPower = -0.1; - powerMax = pf.createPersistentProperty("PowerMax", 0.5); - powerMin = pf.createPersistentProperty("PowerMin", -0.3); + powerMax = pf.createPersistentProperty("PowerMax", 0.45); + powerMin = pf.createPersistentProperty("PowerMin", -0.25); extensionMmPerRevolution = pf.createPersistentProperty("ExtensionMmPerRevolution", 5.715352326); - upperLimitInMm = pf.createPersistentProperty("UpperLimitInMm", 250); + upperLegalLimitMm = pf.createPersistentProperty("UpperLegalLimitMm", 238); angleTrim = pf.createPersistentProperty("AngleTrim", 0); @@ -113,18 +120,24 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa absoluteEncoderRevolutionsPerArmDegree = pf.createPersistentProperty( "AbsoluteEncoderRevolutionPerArmDegree", 1); - softLowerLimitInMm = pf.createPersistentProperty( - "SoftLowerLimit", upperLimitInMm.get() * 0.15); - softUpperLimitInMm = pf.createPersistentProperty( - "SoftUpperLimit", upperLimitInMm.get() * 0.85); - softLowerLimitSpeed = pf.createPersistentProperty("SoftLowerLimitSpeed", -0.05); - softUpperLimitSpeed = pf.createPersistentProperty("SoftUpperLimitSpeed", 0.10); + upperSlowZoneThresholdMm = pf.createPersistentProperty( + "UpperSlowZoneThresholdMm", upperLegalLimitMm.get() * 0.85); + lowerSlowZoneThresholdMm = pf.createPersistentProperty( + "LowerSlowZoneThresholdMm", upperLegalLimitMm.get() * 0.15); + lowerExtremelySlowZoneThresholdMm = pf.createPersistentProperty( + "LowerExtremelySlowZoneThresholdMm", upperLegalLimitMm.get() * 0.05); + + upperSlowZonePowerLimit = pf.createPersistentProperty("UpperSlowZonePowerLimit", 0.10); + lowerSlowZonePowerLimit = pf.createPersistentProperty("LowerSlowZonePowerLimit", -0.05); + lowerExtremelySlowZonePowerLimit = pf.createPersistentProperty("LowerExtremelySlowZonePowerLimit", -0.02); - speedLimitForNotCalibrated = pf.createPersistentProperty( - "SpeedLimitForNotCalibrated", -0.1); + powerLimitForNotCalibrated = pf.createPersistentProperty( + "PowerLimitForNotCalibrated", -0.02); - overallPowerClampForTesting = pf.createPersistentProperty("overallTestingPowerClamp", 0.3); - maximumExtensionDesyncInMm = pf.createPersistentProperty("MaximumExtensionDesyncInMm", 5); + overallPowerClampForTesting = pf.createPersistentProperty("overallTestingPowerClamp", 0.45); + maximumExtensionDesyncMm = pf.createPersistentProperty("MaximumExtensionDesyncMm", 0.5); + + powerRampDurationSec = pf.createPersistentProperty("PowerRampDurationSec", 0.5); hasCalibratedLeft = false; hasCalibratedRight = false; @@ -149,9 +162,9 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa // Enable hardware limits armMotorLeft.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true); - armMotorLeft.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true); + armMotorLeft.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, false); armMotorRight.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true); - armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true); + armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, false); armMotorLeft.setIdleMode(CANSparkBase.IdleMode.kBrake); armMotorRight.setIdleMode(CANSparkBase.IdleMode.kBrake); @@ -170,10 +183,14 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa } public double constrainPowerIfNearLimit(double power, double actualPosition) { - if (actualPosition >= softUpperLimitInMm.get()) { - power = MathUtils.constrainDouble(power, powerMin.get(), softUpperLimitSpeed.get()); - } else if (actualPosition <= softLowerLimitInMm.get()) { - power = MathUtils.constrainDouble(power, softLowerLimitSpeed.get(), powerMax.get()); + if (actualPosition >= upperLegalLimitMm.get()) { + power = MathUtils.constrainDouble(power, powerMin.get(), 0); + } else if (actualPosition >= upperSlowZoneThresholdMm.get()) { + power = MathUtils.constrainDouble(power, powerMin.get(), upperSlowZonePowerLimit.get()); + } else if (actualPosition <= lowerSlowZoneThresholdMm.get() && actualPosition > lowerExtremelySlowZoneThresholdMm.get()) { + power = MathUtils.constrainDouble(power, lowerSlowZonePowerLimit.get(), powerMax.get()); + } else if (actualPosition <= lowerExtremelySlowZoneThresholdMm.get()) { + power = MathUtils.constrainDouble(power, lowerExtremelySlowZonePowerLimit.get(), powerMax.get()); } return power; } @@ -213,7 +230,7 @@ public void setPowerToLeftAndRightArms(double leftPower, double rightPower) { // If we have to make any changes to power, do so by a factor proportional to the maximum // allowed desync in mm. At 50% of the desync, it would restrict power by 50%. - double potentialReductionFactor = Math.max(0, 1 - Math.abs(distanceLeftAhead) / maximumExtensionDesyncInMm.get()); + double potentialReductionFactor = Math.max(0, 1 - Math.abs(distanceLeftAhead) / maximumExtensionDesyncMm.get()); aKitLog.record("PotentialReductionFactor", potentialReductionFactor); // If left arm is ahead @@ -249,7 +266,7 @@ else if (distanceLeftAhead < 0) { // Next, a sanity check; if we have been grossly misconfigured to where the // max/min powers are out of bounds (e.g. a max smaller than min), freeze the arm entirely. - if (powerMax.get() < 0 || powerMin.get() > 0 || speedLimitForNotCalibrated.get() > 0) { + if (powerMax.get() < 0 || powerMin.get() > 0 || powerLimitForNotCalibrated.get() > 0) { armMotorLeft.set(0); armMotorRight.set(0); if (!unsafeMinOrMax) { @@ -262,8 +279,8 @@ else if (distanceLeftAhead < 0) { // If not calibrated, motor can only go down at slow rate since we don't know where we are. if (!(hasCalibratedLeft && hasCalibratedRight)) { - leftPower = MathUtils.constrainDouble(leftPower, speedLimitForNotCalibrated.get(), 0); - rightPower = MathUtils.constrainDouble(rightPower, speedLimitForNotCalibrated.get(), 0); + leftPower = MathUtils.constrainDouble(leftPower, powerLimitForNotCalibrated.get(), 0); + rightPower = MathUtils.constrainDouble(rightPower, powerLimitForNotCalibrated.get(), 0); } // If calibrated, but near limits, slow the system down a bit so we @@ -286,6 +303,17 @@ else if (distanceLeftAhead < 0) { leftPower = MathUtils.constrainDouble(leftPower, powerMin.get(), powerMax.get()); rightPower = MathUtils.constrainDouble(rightPower, powerMin.get(), powerMax.get()); + // Try to ramp power - a smoother start will definitely help reduce high current shock loads, + // and may reduce instability if both sides can get up to "cruise speed" together + if (powerRampingEnabled && powerRampDurationSec.get() > 0) { + double timeSince = XTimer.getFPGATimestamp() - timeSinceNewTarget; + if (timeSince < powerRampDurationSec.get()) { + double rampFactor = timeSince / powerRampDurationSec.get(); + leftPower *= rampFactor; + rightPower *= rampFactor; + } + } + if (contract.isArmReady()) { armMotorLeft.set(leftPower); armMotorRight.set(rightPower); @@ -294,10 +322,18 @@ else if (distanceLeftAhead < 0) { //brake solenoid public void setArmBrakeSolenoid(boolean on){armBrakeSolenoid.setOn(on);} + double previousPower; + @Override public void setPower(Double power) { + + if (previousPower == 0 && power != 0) { + initializeRampingPowerTarget(); + } + aKitLog.record("RequestedArmPower", power); setPowerToLeftAndRightArms(power, power); + previousPower = power; } public void dangerousManualSetPowerToBothArms(double power) { @@ -306,12 +342,12 @@ public void dangerousManualSetPowerToBothArms(double power) { } public void extend() { - setPower(extendPower.get()); + setPower(extendPower); armState = ArmState.EXTENDING; } public void retract() { - setPower(retractPower.get()); + setPower(retractPower); armState = ArmState.RETRACTING; } @@ -466,6 +502,14 @@ public void setClampLimit(double clampPower) { overallPowerClampForTesting.set(Math.abs(clampPower)); } + public void initializeRampingPowerTarget() { + timeSinceNewTarget = XTimer.getFPGATimestamp(); + } + + public void setRampingPowerEnabled(boolean enabled) { + powerRampingEnabled = enabled; + } + public void periodic() { if (contract.isArmReady()) { recordArmEncoderValues(); @@ -503,7 +547,7 @@ public double getAngleFromRange() { return getArmAngleFromDistance(pose.getDistanceFromSpeaker()); } - public void calibrateArmsManually() { + public void markArmsAsCalibratedAgainstLowerPhyscalLimit() { hasCalibratedLeft = true; armMotorLeftRevolutionOffset = -armMotorLeft.getPosition(); hasCalibratedRight = true; diff --git a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java index c06f86ed..f370a331 100644 --- a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java @@ -3,8 +3,10 @@ import competition.operator_interface.OperatorInterface; import competition.subsystems.arm.ArmSubsystem; import xbot.common.command.BaseMaintainerCommand; +import xbot.common.controls.sensors.XTimer; import xbot.common.logic.CalibrationDecider; import xbot.common.logic.HumanVsMachineDecider; +import xbot.common.logic.TimeStableValidator; import xbot.common.math.MathUtils; import xbot.common.math.PIDManager; import xbot.common.properties.PropertyFactory; @@ -16,6 +18,13 @@ public class ArmMaintainerCommand extends BaseMaintainerCommand { // oi used for human input private final OperatorInterface oi; + boolean startedCalibration = false; + boolean givenUpOnCalibration = false; + double calibrationStartTime = 0; + double calibrationMaxDuration = 5; + TimeStableValidator calibrationValidator; + final double calibrationStallDurationSec = 0.5; + @Inject public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf, HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory, @@ -27,7 +36,7 @@ public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf, pf.setPrefix(this); positionPid = pidf.create(getPrefix() + "PoisitionPID", 0.0125, 0.0, 0); - + calibrationValidator = new TimeStableValidator(() -> calibrationStallDurationSec); } @Override public void initialize() { @@ -42,8 +51,62 @@ protected void coastAction() { @Override protected void calibratedMachineControlAction() { - double power = positionPid.calculate(arm.getTargetValue(), arm.getCurrentValue()); - arm.setPower(power); + // The arms can draw huge currents when trying to move small values, so if we are on target + // then we need to kill power. In the future, we could potentially engage the brake here. + if (isMaintainerAtGoal()) { + arm.setPower(0.0); + } else { + double power = positionPid.calculate(arm.getTargetValue(), arm.getCurrentValue()); + arm.setPower(power); + } + } + + + + @Override + protected void uncalibratedMachineControlAction() { + aKitLog.record("Started Calibration", startedCalibration); + aKitLog.record("Given Up On Calibration", givenUpOnCalibration); + + // Try to auto-calibrate. + if (!startedCalibration) { + calibrationStartTime = XTimer.getFPGATimestamp(); + startedCalibration = true; + } + + if (calibrationStartTime + calibrationMaxDuration < XTimer.getFPGATimestamp()) { + givenUpOnCalibration = true; + } + + if (!givenUpOnCalibration) { + // Set some tiny small power to get the arm moving down + arm.setPower(arm.lowerExtremelySlowZonePowerLimit.get()); + + // Are we above 5A usage? + boolean stalledCurrent = arm.armMotorLeft.getOutputCurrent() > 5 + && arm.armMotorRight.getOutputCurrent() > 5; + + aKitLog.record("StalledCurrent", stalledCurrent); + + // Are the arms still? + boolean stillArms = arm.armMotorLeft.getVelocity() < 0.1 + && arm.armMotorRight.getVelocity() < 0.1; + + aKitLog.record("StillArms", stillArms); + + boolean stableAtBottom = calibrationValidator.checkStable(stalledCurrent && stillArms); + + if (stableAtBottom) { + arm.markArmsAsCalibratedAgainstLowerPhyscalLimit(); + // If nobody is currently commanding a setpoint, this will clear the setpoint + // so the arms don't move from the 0 position they just calibrated to. + // If there is an active setpoint, it will override this quite quickly (example; autonomous trying + // to move the arm to a position but has to wait for calibration). + arm.setTargetValue(arm.getCurrentValue()); + } + } else { + humanControlAction(); + } } @Override diff --git a/src/main/java/competition/subsystems/arm/commands/CalibrateArmsManuallyCommand.java b/src/main/java/competition/subsystems/arm/commands/CalibrateArmsManuallyCommand.java index d1c1b093..c56e66c4 100644 --- a/src/main/java/competition/subsystems/arm/commands/CalibrateArmsManuallyCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/CalibrateArmsManuallyCommand.java @@ -17,7 +17,7 @@ public CalibrateArmsManuallyCommand(ArmSubsystem armSubsystem) { @Override public void initialize() { - armSubsystem.calibrateArmsManually(); + armSubsystem.markArmsAsCalibratedAgainstLowerPhyscalLimit(); } @Override diff --git a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java index 819856fc..635b3964 100644 --- a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java +++ b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java @@ -45,8 +45,8 @@ public CollectorSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory s this.collectorMotor = null; } - this.inControlNoteSensor = xDigitalInputFactory.create(contract.getInControlNoteSensorDio()); - this.readyToFireNoteSensor = xDigitalInputFactory.create(contract.getReadyToFireNoteSensorDio()); + this.inControlNoteSensor = xDigitalInputFactory.create(contract.getInControlNoteSensorDio(), this.getPrefix()); + this.readyToFireNoteSensor = xDigitalInputFactory.create(contract.getReadyToFireNoteSensorDio(), this.getPrefix()); pf.setPrefix(this); intakePower = pf.createPersistentProperty("intakePower",0.8); diff --git a/src/main/java/competition/subsystems/shooter/ShooterDistanceToRpmConverter.java b/src/main/java/competition/subsystems/shooter/ShooterDistanceToRpmConverter.java index 47794e15..1b4797cb 100644 --- a/src/main/java/competition/subsystems/shooter/ShooterDistanceToRpmConverter.java +++ b/src/main/java/competition/subsystems/shooter/ShooterDistanceToRpmConverter.java @@ -27,29 +27,29 @@ public double getYIntercept(double slope, double x1, double y1){ //estimates the RPM we need to fire at our distance based on prerecorded data - public double getRPMForDistance(double distanceFromSpeaker) { + public ShooterWheelTargetSpeeds getRPMForDistance(double distanceFromSpeaker) { double secantLineSlope; double yIntercept; for (int i = 0; i < distancesFromSpeaker.length - 1; i++) { //logic to find where currentPosition lies in the array if (distancesFromSpeaker[i] == distanceFromSpeaker){ - return rpmForDistance[i]; + return new ShooterWheelTargetSpeeds(rpmForDistance[i]); } //bandage case if(distanceFromSpeaker == distancesFromSpeaker[distancesFromSpeaker.length-1]){ - return rpmForDistance[distancesFromSpeaker.length - 1]; + return new ShooterWheelTargetSpeeds(rpmForDistance[distancesFromSpeaker.length - 1]); } else if (distancesFromSpeaker[i] < distanceFromSpeaker && distanceFromSpeaker < distancesFromSpeaker[i + 1]) { //secant line calculator secantLineSlope = (rpmForDistance[i + 1] - rpmForDistance[i]) / (distancesFromSpeaker[i + 1] - distancesFromSpeaker[i]); yIntercept = getYIntercept(secantLineSlope,distancesFromSpeaker[i],rpmForDistance[i]); - return secantLineSlope * distanceFromSpeaker + yIntercept; + return new ShooterWheelTargetSpeeds(secantLineSlope * distanceFromSpeaker + yIntercept); } } //returns ZERO if our current distance is further than the greatest range tested on the robot - return 0; + return new ShooterWheelTargetSpeeds(0); } public double[] getRpmForDistanceArray() { diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java index 5e870e61..2d2ad252 100644 --- a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java +++ b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java @@ -1,8 +1,6 @@ package competition.subsystems.shooter; import com.revrobotics.CANSparkBase; -import edu.wpi.first.wpilibj.DriverStation; -import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; import competition.subsystems.pose.PoseSubsystem; @@ -18,7 +16,7 @@ @Singleton -public class ShooterWheelSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { +public class ShooterWheelSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { public enum TargetRPM { SAFE, NEARSHOT, @@ -32,7 +30,7 @@ public enum TargetRPM { // IMPORTANT PROPERTIES - private double targetRpm; + private ShooterWheelTargetSpeeds targetRpms = new ShooterWheelTargetSpeeds(0.0); private double trimRpm; private final DoubleProperty safeRpm; private final DoubleProperty nearShotRpm; @@ -50,8 +48,8 @@ public enum TargetRPM { //DEFINING MOTORS - public XCANSparkMax leader; - public XCANSparkMax follower; + public XCANSparkMax upperWheelMotor; + public XCANSparkMax lowerWheelMotor; // DEFINING CONTRACT final ElectricalContract contract; @@ -84,27 +82,26 @@ public ShooterWheelSubsystem(XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, P acceptableToleranceRPM = pf.createPersistentProperty("AcceptableToleranceRPM", 200); XCANSparkMaxPIDProperties defaultShooterPidProperties = new XCANSparkMaxPIDProperties( - 0.0007, - 0.0, - 0.0, + 0.00015, + 0.0000005, 0.0, + 300.0, 0.00019, 1, -1 ); if (contract.isShooterReady()) { - this.leader = sparkMaxFactory.create(contract.getShooterMotorLeader(), this.getPrefix(), + this.upperWheelMotor = sparkMaxFactory.create(contract.getShooterMotorLeader(), this.getPrefix(), "ShooterMaster", "ShooterWheel", defaultShooterPidProperties); - this.follower = sparkMaxFactory.createWithoutProperties(contract.getShooterMotorFollower(), this.getPrefix(), - "ShooterFollower"); - this.follower.follow(this.leader, false); + this.lowerWheelMotor = sparkMaxFactory.create(contract.getShooterMotorFollower(), this.getPrefix(), + "ShooterFollower", "ShooterWheel", defaultShooterPidProperties); - leader.setIdleMode(CANSparkBase.IdleMode.kCoast); - follower.setIdleMode(CANSparkBase.IdleMode.kCoast); + upperWheelMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + lowerWheelMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - this.leader.setP(defaultShooterPidProperties.p()); - this.leader.setFF(defaultShooterPidProperties.feedForward()); + upperWheelMotor.setSmartCurrentLimit(75); + lowerWheelMotor.setSmartCurrentLimit(75); } } @@ -135,30 +132,38 @@ public void resetTrimRPM() { } @Override - public Double getCurrentValue() { + public ShooterWheelTargetSpeeds getCurrentValue() { //We want the actual current value from the motor not from the code if (contract.isShooterReady()) { - return leader.getVelocity(); + return new ShooterWheelTargetSpeeds(upperWheelMotor.getVelocity(), lowerWheelMotor.getVelocity()); } // DON'T RETURN NULL, OR ROBOT COULD POTENTIALLY CRASH, 0.0 IS SAFER - return 0.0; + return new ShooterWheelTargetSpeeds(); } @Override - public Double getTargetValue() { - return targetRpm + getTrimRPM(); + public ShooterWheelTargetSpeeds getTargetValue() { + // Include the trim RPM when reporting out the target RPM + return new ShooterWheelTargetSpeeds( + targetRpms.upperWheelsTargetRPM + getTrimRPM(), + targetRpms.lowerWheelsTargetRPM + getTrimRPM()); } @Override - public void setTargetValue(Double value) { - targetRpm = value; + public void setTargetValue(ShooterWheelTargetSpeeds value) { + targetRpms = value; log.info("Target RPM: " + value); } + public void setTargetValue(double value) { + setTargetValue(new ShooterWheelTargetSpeeds(value)); + } + @Override - public void setPower(Double power) { + public void setPower(ShooterWheelTargetSpeeds power) { if (contract.isShooterReady()) { - leader.set(power); + upperWheelMotor.set(power.upperWheelsTargetRPM); + lowerWheelMotor.set(power.upperWheelsTargetRPM); } } @@ -168,13 +173,13 @@ public boolean isCalibrated() { } public void resetWheel() { - setPower(0.0); - setTargetValue(0.0); + setPower(new ShooterWheelTargetSpeeds(0.0)); + setTargetValue(new ShooterWheelTargetSpeeds(0.0)); resetPID(); } public void stopWheel() { - setPower(0.0); + setPower(new ShooterWheelTargetSpeeds(0.0)); } @@ -193,25 +198,26 @@ public double getAcceptableToleranceRPM() { public void resetPID() { if (contract.isShooterReady()) { - leader.setIAccum(0); + upperWheelMotor.setIAccum(0); } } //WAY TO SET THE ACTUAL PID - public void setPidSetpoint(double speed) { + public void setPidSetpoints(ShooterWheelTargetSpeeds speeds) { if (contract.isShooterReady()) { - leader.setReference(speed, CANSparkBase.ControlType.kVelocity); + upperWheelMotor.setReference(speeds.upperWheelsTargetRPM, CANSparkBase.ControlType.kVelocity); + lowerWheelMotor.setReference(speeds.lowerWheelsTargetRPM, CANSparkBase.ControlType.kVelocity); } } public void configurePID() { if (contract.isShooterReady()) { - leader.setIMaxAccum(iMaxAccumValueForShooter.get(), 0); + upperWheelMotor.setIMaxAccum(iMaxAccumValueForShooter.get(), 0); } } public void periodic() { - leader.periodic(); - follower.periodic(); + upperWheelMotor.periodic(); + lowerWheelMotor.periodic(); aKitLog.record("TargetRPM", getTargetValue()); aKitLog.record("CurrentRPM", getCurrentValue()); @@ -220,13 +226,13 @@ public void periodic() { public void refreshDataFrame() { if (contract.isShooterReady()) { - leader.refreshDataFrame(); - follower.refreshDataFrame(); + upperWheelMotor.refreshDataFrame(); + lowerWheelMotor.refreshDataFrame(); } } //returns the RPM based on the distance from the speaker - public double getSpeedForRange(){ + public ShooterWheelTargetSpeeds getSpeedForRange(){ return converter.getRPMForDistance(pose.getDistanceFromSpeaker()); } } diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelTargetSpeeds.java b/src/main/java/competition/subsystems/shooter/ShooterWheelTargetSpeeds.java new file mode 100644 index 00000000..9d6cf590 --- /dev/null +++ b/src/main/java/competition/subsystems/shooter/ShooterWheelTargetSpeeds.java @@ -0,0 +1,27 @@ +package competition.subsystems.shooter; + +import edu.wpi.first.util.struct.StructSerializable; + +public class ShooterWheelTargetSpeeds implements StructSerializable { + public double upperWheelsTargetRPM; + public double lowerWheelsTargetRPM; + + public ShooterWheelTargetSpeeds(double upperWheelsTargetRPM, double lowerWheelsTargetRPM) { + this.upperWheelsTargetRPM = upperWheelsTargetRPM; + this.lowerWheelsTargetRPM = lowerWheelsTargetRPM; + } + + public ShooterWheelTargetSpeeds(double rpmForUpperAndLower) { + this(rpmForUpperAndLower, rpmForUpperAndLower); + } + + public ShooterWheelTargetSpeeds() { + this(0, 0); + } + + public boolean representsZeroSpeed() { + return Math.abs(upperWheelsTargetRPM) < 0.001 && Math.abs(lowerWheelsTargetRPM) < 0.001; + } + + public static final ShooterWheelTargetSpeedsStruct struct = new ShooterWheelTargetSpeedsStruct(); +} diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelTargetSpeedsStruct.java b/src/main/java/competition/subsystems/shooter/ShooterWheelTargetSpeedsStruct.java new file mode 100644 index 00000000..5722ed3a --- /dev/null +++ b/src/main/java/competition/subsystems/shooter/ShooterWheelTargetSpeedsStruct.java @@ -0,0 +1,40 @@ +package competition.subsystems.shooter; + +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class ShooterWheelTargetSpeedsStruct implements Struct { + + @Override + public Class getTypeClass() { + return ShooterWheelTargetSpeeds.class; + } + + @Override + public String getTypeString() { + return "struct:ShooterWheelTargetSpeeds"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double upperWheelsTargetRPM;double lowerWheelsTargetRPM"; + } + + @Override + public ShooterWheelTargetSpeeds unpack(ByteBuffer bb) { + double upperWheelsTargetRPM = bb.getDouble(); + double lowerWheelsTargetRPM = bb.getDouble(); + return new ShooterWheelTargetSpeeds(upperWheelsTargetRPM, lowerWheelsTargetRPM); + } + + @Override + public void pack(ByteBuffer bb, ShooterWheelTargetSpeeds value) { + bb.putDouble(value.upperWheelsTargetRPM); + bb.putDouble(value.lowerWheelsTargetRPM); + } +} diff --git a/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java b/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java index 9bd5fb11..49b820ea 100644 --- a/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java +++ b/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java @@ -2,13 +2,14 @@ import competition.operator_interface.OperatorInterface; import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.shooter.ShooterWheelTargetSpeeds; import xbot.common.command.BaseMaintainerCommand; import xbot.common.logic.HumanVsMachineDecider; import xbot.common.properties.PropertyFactory; import javax.inject.Inject; -public class ShooterWheelMaintainerCommand extends BaseMaintainerCommand { +public class ShooterWheelMaintainerCommand extends BaseMaintainerCommand { final ShooterWheelSubsystem wheel; final OperatorInterface oi; @@ -16,7 +17,7 @@ public class ShooterWheelMaintainerCommand extends BaseMaintainerCommand @Inject public ShooterWheelMaintainerCommand(OperatorInterface oi, ShooterWheelSubsystem wheel, PropertyFactory pf, HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory) { - super(wheel, pf, hvmFactory, 0, 0); + super(wheel, pf, hvmFactory, 150, 0.5); this.oi = oi; this.wheel = wheel; } @@ -39,23 +40,23 @@ protected void initializeMachineControlAction() { @Override protected void calibratedMachineControlAction() { - double speed = wheel.getTargetValue(); + var speeds = wheel.getTargetValue(); - if (Math.abs(speed) < 0.001) { - wheel.setPower(0.0); + if (speeds.representsZeroSpeed()) { + wheel.setPower(new ShooterWheelTargetSpeeds(0.0)); return; } if (wheel.isCalibrated()) { - wheel.setPidSetpoint(speed); + wheel.setPidSetpoints(speeds); } } @Override - protected Double getHumanInput() { + protected ShooterWheelTargetSpeeds getHumanInput() { // I THINK WE CURRENTLY DON'T HAVE ANY HUMAN INPUT. - return 0.0; + return new ShooterWheelTargetSpeeds(0,0); } protected void end() { @@ -69,10 +70,11 @@ protected double getHumanInputMagnitude() { @Override protected double getErrorMagnitude() { - double current = wheel.getCurrentValue(); - double target = wheel.getTargetValue(); + var currents = wheel.getCurrentValue(); + var targets = wheel.getTargetValue(); - double shooterError = target - current; - return shooterError; + // Take the average of the two errors for now. + return ((targets.upperWheelsTargetRPM - currents.upperWheelsTargetRPM) + + (targets.lowerWheelsTargetRPM - currents.lowerWheelsTargetRPM)) / 2; } } diff --git a/src/main/java/competition/subsystems/shooter/commands/WarmUpShooterRPMCommand.java b/src/main/java/competition/subsystems/shooter/commands/WarmUpShooterRPMCommand.java index 037502e2..2ad48fae 100644 --- a/src/main/java/competition/subsystems/shooter/commands/WarmUpShooterRPMCommand.java +++ b/src/main/java/competition/subsystems/shooter/commands/WarmUpShooterRPMCommand.java @@ -1,5 +1,6 @@ package competition.subsystems.shooter.commands; import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.shooter.ShooterWheelTargetSpeeds; import xbot.common.command.BaseSetpointCommand; import java.util.function.Supplier; import javax.inject.Inject; @@ -7,7 +8,7 @@ public class WarmUpShooterRPMCommand extends BaseSetpointCommand { ShooterWheelSubsystem shooter; - private Supplier customRPM; + private Supplier customRPM; @Inject public WarmUpShooterRPMCommand(ShooterWheelSubsystem shooter) { @@ -15,14 +16,14 @@ public WarmUpShooterRPMCommand(ShooterWheelSubsystem shooter) { this.shooter = shooter; } - public void setTargetRpm(Supplier customRPM) { + public void setTargetRpm(Supplier customRPM) { this.customRPM = customRPM; } - public void setTargetRpm(Double customRPM) { - setTargetRpm(()-> customRPM); + public void setTargetRpm(ShooterWheelTargetSpeeds customRPMs) { + setTargetRpm(()-> customRPMs); } - + public void initialize() { shooter.setTargetValue(this.customRPM.get()); } diff --git a/src/test/java/competition/subsystems/arm/ArmSubsystemTest.java b/src/test/java/competition/subsystems/arm/ArmSubsystemTest.java index da2a21ca..c634b852 100644 --- a/src/test/java/competition/subsystems/arm/ArmSubsystemTest.java +++ b/src/test/java/competition/subsystems/arm/ArmSubsystemTest.java @@ -19,6 +19,7 @@ public void setUp() { super.setUp(); arm = getInjectorComponent().armSubsystem(); arm.setClampLimit(1.0); + arm.setRampingPowerEnabled(false); } private void checkMotorPower(double leftPower, double rightPower) { @@ -39,18 +40,22 @@ private void setMotorPosition(double position) { arm.armMotorLeft.setPosition(position); arm.armMotorRight.setPosition(position); } - + @Test public void testExtend() { // Assuming arm motors has already calibrated arm.hasCalibratedLeft = true; arm.hasCalibratedRight = true; + arm.markArmsAsCalibratedAgainstLowerPhyscalLimit(); + + ((MockCANSparkMax)arm.armMotorLeft).setPosition(20); + ((MockCANSparkMax)arm.armMotorRight).setPosition(20); - assertNotEquals(arm.extendPower.get(), 0, 0.0001); + assertNotEquals(arm.extendPower, 0, 0.0001); checkMotorPower(0); arm.extend(); - checkMotorPower(arm.extendPower.get()); + checkMotorPower(arm.extendPower); } @@ -58,11 +63,15 @@ public void testExtend() { public void testRetract() { arm.hasCalibratedLeft = true; arm.hasCalibratedRight = true; + arm.markArmsAsCalibratedAgainstLowerPhyscalLimit(); - assertNotEquals(arm.retractPower.get(), 0, 0.0001); // Check if retract power == 0 + ((MockCANSparkMax)arm.armMotorLeft).setPosition(20); + ((MockCANSparkMax)arm.armMotorRight).setPosition(20); + + assertNotEquals(arm.retractPower, 0, 0.0001); // Check if retract power == 0 checkMotorPower(0); // Make sure motor not moving arm.retract(); - checkMotorPower(arm.retractPower.get()); + checkMotorPower(arm.retractPower); } @@ -118,7 +127,7 @@ public void testLimits() { // Attempt to move backward but should only be moving at -0.1 arm.setPower(-0.3); - checkMotorPower(-0.1); + checkMotorPower(arm.lowerExtremelySlowZonePowerLimit.get()); // Reverse limit hit (Usually you only get here for calibration) ((MockCANSparkMax)arm.armMotorLeft).setReverseLimitSwitchStateForTesting(true); @@ -153,8 +162,8 @@ public void testLimits() { arm.setPower(0.3); checkMotorPower(0); - arm.setPower(-0.3); - checkMotorPower(-0.3); + arm.setPower(arm.powerMin.get()); + checkMotorPower(arm.powerMin.get()); // Test LimitState: BOTH_LIMITS_HIT (Activates when there is issues...) ((MockCANSparkMax)arm.armMotorLeft).setReverseLimitSwitchStateForTesting(true); diff --git a/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java b/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java index 472cad42..7987d33e 100644 --- a/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java +++ b/src/test/java/competition/subsystems/arm/BaseArmCommandsTest.java @@ -1,7 +1,6 @@ package competition.subsystems.arm; import competition.BaseCompetitionTest; -import competition.operator_interface.OperatorCommandMap; import competition.subsystems.arm.commands.ExtendArmCommand; import competition.subsystems.arm.commands.ReconcileArmAlignmentCommand; import competition.subsystems.arm.commands.RetractArmCommand; @@ -29,6 +28,7 @@ public void setUp() { stop = getInjectorComponent().stopArmCommand(); reconcile = getInjectorComponent().reconcileArmAlignmentCommand(); arm.setClampLimit(1.0); + arm.setRampingPowerEnabled(false); } private void checkMotorPower(double power) { @@ -47,12 +47,12 @@ public void test() { // Extend extend.initialize(); extend.execute(); - checkMotorPower(arm.extendPower.get()); + checkMotorPower(arm.extendPower); // Retract retract.initialize(); retract.execute(); - checkMotorPower(arm.retractPower.get()); + checkMotorPower(arm.lowerExtremelySlowZonePowerLimit.get()); // Stop stop.initialize(); diff --git a/src/test/java/competition/subsystems/shooter/ShooterDistanceToRpmConverterTest.java b/src/test/java/competition/subsystems/shooter/ShooterDistanceToRpmConverterTest.java index 411c82ad..cca4f1b3 100644 --- a/src/test/java/competition/subsystems/shooter/ShooterDistanceToRpmConverterTest.java +++ b/src/test/java/competition/subsystems/shooter/ShooterDistanceToRpmConverterTest.java @@ -24,19 +24,19 @@ public void setUp() { @Test public void testConverter() { //testing when distance is exactly equal to a recorded data point (Also testing when the distance is the first and lastelement in the array) - assertEquals(500, converter.getRPMForDistance(1), 0.00001); - assertEquals(3500, converter.getRPMForDistance(11), 0.00001); - assertEquals(2200, converter.getRPMForDistance(6), 0.00001); + assertEquals(500, converter.getRPMForDistance(1).upperWheelsTargetRPM, 0.00001); + assertEquals(3500, converter.getRPMForDistance(11).upperWheelsTargetRPM, 0.00001); + assertEquals(2200, converter.getRPMForDistance(6).upperWheelsTargetRPM, 0.00001); //testing when distance is in between two points in the recorded data - assertEquals(725,converter.getRPMForDistance(1.5),0.00001); - assertEquals(2850,converter.getRPMForDistance(9),0.00001); + assertEquals(725,converter.getRPMForDistance(1.5).upperWheelsTargetRPM,0.00001); + assertEquals(2850,converter.getRPMForDistance(9).upperWheelsTargetRPM,0.00001); //testing when distance is less than the lowest or // greater than the highest recorded data point(should return zero?? Unless we should return something else) - assertEquals(0,converter.getRPMForDistance(0.5),0.00001); - assertEquals(0,converter.getRPMForDistance(200),0.00001); - assertEquals(0,converter.getRPMForDistance(-192391),0.00001); + assertEquals(0,converter.getRPMForDistance(0.5).upperWheelsTargetRPM,0.00001); + assertEquals(0,converter.getRPMForDistance(200).upperWheelsTargetRPM,0.00001); + assertEquals(0,converter.getRPMForDistance(-192391).upperWheelsTargetRPM,0.00001); } } diff --git a/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java b/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java index 278f258a..22218ec1 100644 --- a/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java +++ b/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java @@ -30,7 +30,7 @@ public void testSetTargetRPM() { warmUpShooterCommand.execute(); double safeRPM = 500; - assertEquals(Optional.ofNullable(shooter.getTargetValue()), Optional.of(safeRPM)); + assertEquals(Optional.of(safeRPM), Optional.ofNullable(shooter.getTargetValue().upperWheelsTargetRPM)); } } \ No newline at end of file diff --git a/src/test/java/competition/subsystems/shooter/WarmUpShooterRPMCommandTest.java b/src/test/java/competition/subsystems/shooter/WarmUpShooterRPMCommandTest.java index a6e2a938..d1b026b1 100644 --- a/src/test/java/competition/subsystems/shooter/WarmUpShooterRPMCommandTest.java +++ b/src/test/java/competition/subsystems/shooter/WarmUpShooterRPMCommandTest.java @@ -24,12 +24,12 @@ public void setUp() { @Test public void testSetTargetRPM() { - Supplier randomRPM = ()->(Double)100.0; + Supplier randomRPM = ()-> new ShooterWheelTargetSpeeds(100.0); warmUpShooterRPMCommand.setTargetRpm(randomRPM); warmUpShooterRPMCommand.initialize(); warmUpShooterRPMCommand.execute(); - assertEquals(Optional.ofNullable(shooter.getTargetValue()), Optional.of(100.0)); + assertEquals(Optional.of(100.0), Optional.ofNullable(shooter.getTargetValue().lowerWheelsTargetRPM)); } } \ No newline at end of file