diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 8dc3091f..784bcc28 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 8dc3091f362d14feb4034efeb7eab4cf5e8e8686 +Subproject commit 784bcc2875c3e20b5244d70bc5b4b06d9fda5817 diff --git a/src/main/java/competition/commandgroups/PrepareToFireAtAmpCommandGroup.java b/src/main/java/competition/commandgroups/PrepareToFireAtAmpCommandGroup.java index 7280f07d..0b2b8251 100644 --- a/src/main/java/competition/commandgroups/PrepareToFireAtAmpCommandGroup.java +++ b/src/main/java/competition/commandgroups/PrepareToFireAtAmpCommandGroup.java @@ -14,7 +14,7 @@ public class PrepareToFireAtAmpCommandGroup extends ParallelCommandGroup { public PrepareToFireAtAmpCommandGroup(SetArmAngleCommand armAngle, WarmUpShooterCommand shooter) { // Move arm to preset position - armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.FIRING_IN_AMP); + armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.FIRING_FROM_AMP); this.addCommands(armAngle); // Set shooter wheels to target RPM shooter.setTargetRpm(ShooterWheelSubsystem.TargetRPM.AMP_SHOT); diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 100da8a8..1bca90e1 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -9,6 +9,7 @@ import competition.subsystems.arm.ArmSubsystem; import competition.subsystems.arm.commands.ArmMaintainerCommand; import competition.subsystems.arm.commands.CalibrateArmsManuallyCommand; +import competition.subsystems.arm.commands.ContinuouslyPointArmAtSpeakerCommand; import competition.subsystems.arm.commands.SetArmAngleCommand; import competition.subsystems.arm.commands.SetArmExtensionCommand; import competition.subsystems.collector.commands.EjectCollectorCommand; @@ -23,6 +24,8 @@ import competition.subsystems.schoocher.commands.IntakeScoocherCommand; import competition.subsystems.shooter.ShooterWheelSubsystem; import competition.subsystems.shooter.ShooterWheelTargetSpeeds; +import competition.subsystems.shooter.commands.ContinuouslyWarmUpForSpeakerCommand; +import competition.subsystems.shooter.commands.FireWhenReadyCommand; import competition.subsystems.shooter.commands.WarmUpShooterCommand; import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand; import edu.wpi.first.math.geometry.Pose2d; @@ -53,26 +56,35 @@ public void setupFundamentalCommands( EjectScoocherCommand scoocherEject, IntakeCollectorCommand collectorIntake, EjectCollectorCommand collectorEject, - WarmUpShooterCommand shooterWarmUp, + WarmUpShooterCommand shooterWarmUpSafe, + WarmUpShooterCommand shooterWarmUpNear, + WarmUpShooterCommand shooterWarmUpFar, + WarmUpShooterCommand shooterWarmUpAmp, FireCollectorCommand fireCollectorCommand, SetArmAngleCommand armAngle, ArmMaintainerCommand armMaintainer, - WarmUpShooterCommand warmUpShooterSlow, - WarmUpShooterCommand warmUpShooterFast, WarmUpShooterRPMCommand warmUpShooterDifferentialRPM ) { // Scooch - oi.operatorGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get()); - oi.operatorGamepad.getXboxButton(XboxButton.LeftBumper).whileTrue(scoocherEject); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get()); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.LeftBumper).whileTrue(scoocherEject); // Collect - oi.operatorGamepad.getXboxButton(XboxButton.RightTrigger).whileTrue(collectorIntake); - oi.operatorGamepad.getXboxButton(XboxButton.LeftTrigger).whileTrue(collectorEject); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightTrigger).whileTrue(collectorIntake); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.LeftTrigger).whileTrue(collectorEject); // Fire - shooterWarmUp.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT); - oi.operatorGamepad.getXboxButton(XboxButton.X).whileTrue(shooterWarmUp); - oi.operatorGamepad.getXboxButton(XboxButton.A).whileTrue(fireCollectorCommand); + shooterWarmUpSafe.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER); + shooterWarmUpNear.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT); + shooterWarmUpFar.setTargetRpm(ShooterWheelSubsystem.TargetRPM.DISTANCESHOT); + shooterWarmUpAmp.setTargetRpm(ShooterWheelSubsystem.TargetRPM.AMP_SHOT); + + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.A).whileTrue(shooterWarmUpSafe); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.X).whileTrue(shooterWarmUpNear); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.Y).whileTrue(shooterWarmUpFar); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.Back).whileTrue(shooterWarmUpAmp); + + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.B).whileTrue(fireCollectorCommand); // Arms are taken care of via their maintainer & human overrides. armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.SCOOCH_NOTE); @@ -80,14 +92,8 @@ public void setupFundamentalCommands( 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); + //oi.operatorFundamentalsGamepad.getPovIfAvailable(0).whileTrue(warmUpShooterDifferentialRPM); } // Example for setting up a command to fire when a button is pressed: @@ -199,21 +205,90 @@ public void setupArmPIDCommands( var highArm = commandProvider.get(); highArm.setTargetExtension(150); - var increaseArm = commandProvider.get(); - increaseArm.setTargetExtension(20); - increaseArm.setRelative(true); + var increaseArmLarge = commandProvider.get(); + increaseArmLarge.setTargetExtension(20); + increaseArmLarge.setRelative(true); + + var increaseArmSmall = commandProvider.get(); + increaseArmSmall.setTargetExtension(2); + increaseArmSmall.setRelative(true); + + var decreaseArmLarge = commandProvider.get(); + decreaseArmLarge.setTargetExtension(-20); + decreaseArmLarge.setRelative(true); + + var decreaseArmSmall = commandProvider.get(); + decreaseArmSmall.setTargetExtension(-2); + decreaseArmSmall.setRelative(true); + + oi.operatorFundamentalsGamepad.getPovIfAvailable(0).whileTrue(increaseArmLarge); + oi.operatorFundamentalsGamepad.getPovIfAvailable(180).whileTrue(decreaseArmLarge); + oi.operatorFundamentalsGamepad.getPovIfAvailable(90).whileTrue(increaseArmSmall); + oi.operatorFundamentalsGamepad.getPovIfAvailable(270).whileTrue(decreaseArmSmall); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand); + } + + @Inject + public void setupAdvancedOperatorCommands( + OperatorInterface oi, + ArmSubsystem arm, + Provider setArmExtensionCommandProvider, + IntakeCollectorCommand intakeCollector, + EjectCollectorCommand ejectCollector, + IntakeScoocherCommand intakeScoocher, + EjectScoocherCommand ejectScoocher, + Provider warmUpShooterCommandProvider, + ContinuouslyPointArmAtSpeakerCommand continuouslyPointArmAtSpeaker, + ContinuouslyWarmUpForSpeakerCommand continuouslyWarmUpForSpeaker, + FireWhenReadyCommand fireWhenReady + ) { + //Useful arm positions + var armToCollection = setArmExtensionCommandProvider.get(); + armToCollection.setTargetExtension(arm.getUsefulArmPositionExtensionInMm( + ArmSubsystem.UsefulArmPosition.COLLECTING_FROM_GROUND)); + + var armToScooch = setArmExtensionCommandProvider.get(); + armToScooch.setTargetExtension(arm.getUsefulArmPositionExtensionInMm( + ArmSubsystem.UsefulArmPosition.SCOOCH_NOTE)); + + var armToSubwoofer = setArmExtensionCommandProvider.get(); + armToSubwoofer.setTargetExtension(arm.getUsefulArmPositionExtensionInMm( + ArmSubsystem.UsefulArmPosition.FIRING_FROM_SUBWOOFER)); + + var armToAmp = setArmExtensionCommandProvider.get(); + armToAmp.setTargetExtension(arm.getUsefulArmPositionExtensionInMm( + ArmSubsystem.UsefulArmPosition.FIRING_FROM_AMP)); + + // Useful wheel speeds + var warmUpShooterSubwoofer = warmUpShooterCommandProvider.get(); + warmUpShooterSubwoofer.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER); + + var warmUpShooterAmp = warmUpShooterCommandProvider.get(); + warmUpShooterAmp.setTargetRpm(ShooterWheelSubsystem.TargetRPM.AMP_SHOT); + + // Combine into useful actions + // Note manipulation: + var collectNote = intakeCollector.alongWith(armToCollection); + var scoochNote = intakeScoocher.alongWith(armToScooch); + + // Preparing to score: + var prepareToFireAtSubwoofer = warmUpShooterSubwoofer.alongWith(armToSubwoofer); + var prepareToFireAtAmp = warmUpShooterAmp.alongWith(armToAmp); + var continuouslyPrepareToFireAtSpeaker = + continuouslyWarmUpForSpeaker.alongWith(continuouslyPointArmAtSpeaker); - var decreaseArm = commandProvider.get(); - decreaseArm.setTargetExtension(-20); - decreaseArm.setRelative(true); + // Bind to buttons + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.LeftTrigger).whileTrue(collectNote); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Back).whileTrue(ejectCollector); - oi.operatorGamepadSecond.getXboxButton(XboxButton.A).onTrue(decreaseArm); - oi.operatorGamepadSecond.getXboxButton(XboxButton.Y).onTrue(increaseArm); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.LeftBumper).whileTrue(scoochNote); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Start).whileTrue(ejectScoocher); - oi.operatorGamepadSecond.getXboxButton(XboxButton.X).onTrue(homeArm); - oi.operatorGamepadSecond.getXboxButton(XboxButton.B).onTrue(highArm); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.X).whileTrue(prepareToFireAtSubwoofer); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Y).whileTrue(prepareToFireAtAmp); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.A).whileTrue(continuouslyPrepareToFireAtSpeaker); - oi.operatorGamepad.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand); + oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightTrigger).whileTrue(fireWhenReady); } private SwerveSimpleTrajectoryCommand createAndConfigureTypicalSwerveCommand( diff --git a/src/main/java/competition/operator_interface/OperatorInterface.java b/src/main/java/competition/operator_interface/OperatorInterface.java index af08710b..265e6a95 100644 --- a/src/main/java/competition/operator_interface/OperatorInterface.java +++ b/src/main/java/competition/operator_interface/OperatorInterface.java @@ -20,8 +20,8 @@ public class OperatorInterface { // ONE GAMEPAD IS FOR COMPETITION, SECOND GAMEPAD IS USED DURING PRACTICE public XXboxController driverGamepad; - public XXboxController operatorGamepad; - public XXboxController operatorGamepadSecond; + public XXboxController operatorFundamentalsGamepad; + public XXboxController operatorGamepadAdvanced; public XXboxController autoGamepad; public XJoystick neoTrellis; @@ -41,13 +41,13 @@ public OperatorInterface(XXboxControllerFactory controllerFactory, driverGamepad.setLeftInversion(false, true); driverGamepad.setRightInversion(true, true); - operatorGamepad = controllerFactory.create(1); - operatorGamepad.setLeftInversion(false, true); - operatorGamepad.setRightInversion(false, true); + operatorFundamentalsGamepad = controllerFactory.create(1); + operatorFundamentalsGamepad.setLeftInversion(false, true); + operatorFundamentalsGamepad.setRightInversion(false, true); - operatorGamepadSecond = controllerFactory.create(4); - operatorGamepadSecond.setLeftInversion(false, true); - operatorGamepadSecond.setRightInversion(false, true); + operatorGamepadAdvanced = controllerFactory.create(4); + operatorGamepadAdvanced.setLeftInversion(false, true); + operatorGamepadAdvanced.setRightInversion(false, true); autoGamepad = controllerFactory.create(3); diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 4edef26f..faf49504 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -16,6 +16,7 @@ import xbot.common.controls.actuators.XSolenoid; import xbot.common.controls.sensors.XSparkAbsoluteEncoder; import xbot.common.controls.sensors.XTimer; +import xbot.common.math.DoubleInterpolator; import xbot.common.math.MathUtils; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; @@ -88,11 +89,13 @@ public enum LimitState { public enum UsefulArmPosition { STARTING_POSITION, COLLECTING_FROM_GROUND, - FIRING_FROM_SPEAKER_FRONT, - FIRING_IN_AMP, + FIRING_FROM_SUBWOOFER, + FIRING_FROM_AMP, SCOOCH_NOTE } + private DoubleInterpolator speakerDistanceToExtensionInterpolator; + @Inject public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, XSolenoid.XSolenoidFactory xSolenoidFactory, @@ -166,8 +169,8 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa armMotorRight.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true); armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, false); - armMotorLeft.setIdleMode(CANSparkBase.IdleMode.kBrake); - armMotorRight.setIdleMode(CANSparkBase.IdleMode.kBrake); + armMotorLeft.setIdleMode(CANSparkBase.IdleMode.kCoast); + armMotorRight.setIdleMode(CANSparkBase.IdleMode.kCoast); } this.armState = ArmState.STOPPED; @@ -180,6 +183,10 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa .append(new MechanismLigament2d("box-top", 2, 90)) .append(new MechanismLigament2d("box-left", 1, 90)); + speakerDistanceToExtensionInterpolator = + new DoubleInterpolator( + new double[]{0, 36, 49.5, 63, 80, 111, 136}, + new double[]{0, 0, 20.0, 26, 41, 57, 64}); } public double constrainPowerIfNearLimit(double power, double actualPosition) { @@ -374,14 +381,34 @@ public double getUsefulArmPositionAngle(UsefulArmPosition usefulArmPosition) { // THESE ARE ALL PLACEHOLDER VALUES!!! case STARTING_POSITION -> angle = 40; case COLLECTING_FROM_GROUND -> angle = 0; - case FIRING_FROM_SPEAKER_FRONT -> angle = 30; - case FIRING_IN_AMP -> angle = 80; + case FIRING_FROM_SUBWOOFER -> angle = 30; + case FIRING_FROM_AMP -> angle = 80; case SCOOCH_NOTE -> angle = 60; // placeholder value, safe angle to let note through while still low default -> angle = 40; } return angle; } + public double getUsefulArmPositionExtensionInMm(UsefulArmPosition usefulArmPosition) { + double extension = 0; + switch (usefulArmPosition) { + case STARTING_POSITION: + case COLLECTING_FROM_GROUND: + case FIRING_FROM_SUBWOOFER: + extension = 0; + break; + case FIRING_FROM_AMP: + extension = upperLegalLimitMm.get(); + break; + case SCOOCH_NOTE: + extension = 15; + break; + default: + return 0; + } + return extension; + } + public LimitState getLimitState(XCANSparkMax motor) { boolean upperHit = false; @@ -510,6 +537,21 @@ public void setRampingPowerEnabled(boolean enabled) { powerRampingEnabled = enabled; } + public double getAngleFromRange() { + return getArmAngleFromDistance(pose.getDistanceFromSpeaker()); + } + + public void markArmsAsCalibratedAgainstLowerPhyscalLimit() { + hasCalibratedLeft = true; + armMotorLeftRevolutionOffset = -armMotorLeft.getPosition(); + hasCalibratedRight = true; + armMotorRightRevolutionOffset = -armMotorRight.getPosition(); + } + + public double getRecommendedExtension(double distanceFromSpeaker) { + return speakerDistanceToExtensionInterpolator.getInterpolatedOutputVariable(distanceFromSpeaker); + } + public void periodic() { if (contract.isArmReady()) { recordArmEncoderValues(); @@ -542,15 +584,4 @@ public void refreshDataFrame() { armAbsoluteEncoder.refreshDataFrame(); } } - - public double getAngleFromRange() { - return getArmAngleFromDistance(pose.getDistanceFromSpeaker()); - } - - public void markArmsAsCalibratedAgainstLowerPhyscalLimit() { - hasCalibratedLeft = true; - armMotorLeftRevolutionOffset = -armMotorLeft.getPosition(); - hasCalibratedRight = true; - armMotorRightRevolutionOffset = -armMotorRight.getPosition(); - } } \ No newline at end of file diff --git a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java index f370a331..e9fb9eff 100644 --- a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java @@ -30,7 +30,7 @@ public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf, HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory, PIDManager.PIDManagerFactory pidf, CalibrationDecider.CalibrationDeciderFactory calf, OperatorInterface oi){ - super(arm, pf, hvmFactory, 1, .001); + super(arm, pf, hvmFactory, 1, 0.5); this.arm = arm; this.oi = oi; pf.setPrefix(this); @@ -120,7 +120,7 @@ protected double getErrorMagnitude() { @Override protected Double getHumanInput() { return MathUtils.deadband( - oi.operatorGamepad.getLeftVector().y, + oi.operatorFundamentalsGamepad.getLeftVector().y, oi.getOperatorGamepadTypicalDeadband(), (x) -> x); } diff --git a/src/main/java/competition/subsystems/arm/commands/ContinuouslyPointArmAtSpeakerCommand.java b/src/main/java/competition/subsystems/arm/commands/ContinuouslyPointArmAtSpeakerCommand.java new file mode 100644 index 00000000..5d150a6f --- /dev/null +++ b/src/main/java/competition/subsystems/arm/commands/ContinuouslyPointArmAtSpeakerCommand.java @@ -0,0 +1,31 @@ +package competition.subsystems.arm.commands; + +import competition.subsystems.arm.ArmSubsystem; +import competition.subsystems.pose.PoseSubsystem; +import xbot.common.command.BaseCommand; +import xbot.common.command.BaseSetpointCommand; + +import javax.inject.Inject; + +public class ContinuouslyPointArmAtSpeakerCommand extends BaseSetpointCommand { + + ArmSubsystem arm; + PoseSubsystem pose; + + @Inject + public ContinuouslyPointArmAtSpeakerCommand(ArmSubsystem arm, PoseSubsystem pose) { + super(arm); + this.arm = arm; + this.pose = pose; + } + + @Override + public void initialize() { + log.info("Initializing"); + } + + @Override + public void execute() { + arm.setTargetValue(arm.getRecommendedExtension(pose.getDistanceFromSpeaker())); + } +} diff --git a/src/main/java/competition/subsystems/arm/commands/EmergencyManualArmControlCommand.java b/src/main/java/competition/subsystems/arm/commands/EmergencyManualArmControlCommand.java index 4ba78431..94cd321b 100644 --- a/src/main/java/competition/subsystems/arm/commands/EmergencyManualArmControlCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/EmergencyManualArmControlCommand.java @@ -29,7 +29,7 @@ public void initialize() { @Override public void execute() { - arm.dangerousManualSetPowerToBothArms(MathUtils.deadband(oi.operatorGamepad.getLeftStickY() * armPowerFactor.get(), 0.15)); + arm.dangerousManualSetPowerToBothArms(MathUtils.deadband(oi.operatorFundamentalsGamepad.getLeftStickY() * armPowerFactor.get(), 0.15)); } } diff --git a/src/main/java/competition/subsystems/arm/commands/SetArmExtensionCommand.java b/src/main/java/competition/subsystems/arm/commands/SetArmExtensionCommand.java index 84d817f9..eb3f17cc 100644 --- a/src/main/java/competition/subsystems/arm/commands/SetArmExtensionCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/SetArmExtensionCommand.java @@ -28,11 +28,14 @@ public void setRelative(boolean isRelative) { @Override public void initialize() { + log.info("Initializing"); + double targetValue = targetExtension; if(isRelative) { - armSubsystem.setTargetValue(armSubsystem.getCurrentValue() + targetExtension); - } else { - armSubsystem.setTargetValue(targetExtension); + targetValue = armSubsystem.getCurrentValue() + targetExtension; } + log.info("Setting arm target extension to " + targetValue); + armSubsystem.setTargetValue(targetValue); + armSubsystem.initializeRampingPowerTarget(); } @Override diff --git a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java index 635b3964..f1572f4b 100644 --- a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java +++ b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java @@ -1,5 +1,6 @@ package competition.subsystems.collector; +import com.revrobotics.CANSparkBase; import competition.electrical_contract.ElectricalContract; import org.littletonrobotics.junction.Logger; import xbot.common.advantage.DataFrameRefreshable; @@ -41,6 +42,7 @@ public CollectorSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory s if (contract.isCollectorReady()) { this.collectorMotor = sparkMaxFactory.createWithoutProperties(contract.getCollectorMotor(), getPrefix(), "CollectorMotor"); collectorMotor.setSmartCurrentLimit(40); + collectorMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); } else { this.collectorMotor = null; } diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java index 2d2ad252..67e0c249 100644 --- a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java +++ b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java @@ -8,6 +8,7 @@ import competition.electrical_contract.ElectricalContract; import xbot.common.controls.actuators.XCANSparkMax; import xbot.common.controls.actuators.XCANSparkMaxPIDProperties; +import xbot.common.math.DoubleInterpolator; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; @@ -18,7 +19,7 @@ @Singleton public class ShooterWheelSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { public enum TargetRPM { - SAFE, + SUBWOOFER, NEARSHOT, DISTANCESHOT, AMP_SHOT @@ -28,6 +29,9 @@ public enum TargetRPM { PoseSubsystem pose; ShooterDistanceToRpmConverter converter; + DoubleInterpolator upperWheelDistanceToRpmInterpolator; + DoubleInterpolator lowerWheelDistanceToRpmInterpolator; + // IMPORTANT PROPERTIES private ShooterWheelTargetSpeeds targetRpms = new ShooterWheelTargetSpeeds(0.0); @@ -41,12 +45,6 @@ public enum TargetRPM { private final DoubleProperty iMaxAccumValueForShooter; private final DoubleProperty acceptableToleranceRPM; - - - - - - //DEFINING MOTORS public XCANSparkMax upperWheelMotor; public XCANSparkMax lowerWheelMotor; @@ -103,11 +101,18 @@ public ShooterWheelSubsystem(XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, P upperWheelMotor.setSmartCurrentLimit(75); lowerWheelMotor.setSmartCurrentLimit(75); } + + var distanceArray = new double[]{0, 36, 49.5, 63, 80, 111, 136}; + var upperWheelRPMArray = new double[]{4000, 4000, 4000, 4000, 4000, 4000, 4500}; + var lowerWheelRPMArray = new double[]{4000, 4000, 4000, 4000, 4000, 4000, 4500}; + + upperWheelDistanceToRpmInterpolator = new DoubleInterpolator(distanceArray, upperWheelRPMArray); + lowerWheelDistanceToRpmInterpolator = new DoubleInterpolator(distanceArray, lowerWheelRPMArray); } public void setTargetRPM(TargetRPM target) { switch (target) { - case SAFE -> setTargetValue(safeRpm.get()); + case SUBWOOFER -> setTargetValue(safeRpm.get()); case NEARSHOT -> setTargetValue(nearShotRpm.get()); case DISTANCESHOT -> setTargetValue(distanceShotRpm.get()); case AMP_SHOT -> setTargetValue(ampShotRpm.get()); @@ -215,6 +220,16 @@ public void configurePID() { upperWheelMotor.setIMaxAccum(iMaxAccumValueForShooter.get(), 0); } } + + //returns the RPM based on the distance from the speaker + public ShooterWheelTargetSpeeds getSpeedForRange(){ + double distanceFromSpeaker = pose.getDistanceFromSpeaker(); + return new ShooterWheelTargetSpeeds( + upperWheelDistanceToRpmInterpolator.getInterpolatedOutputVariable(distanceFromSpeaker), + lowerWheelDistanceToRpmInterpolator.getInterpolatedOutputVariable(distanceFromSpeaker) + ); + } + public void periodic() { upperWheelMotor.periodic(); lowerWheelMotor.periodic(); @@ -230,10 +245,5 @@ public void refreshDataFrame() { lowerWheelMotor.refreshDataFrame(); } } - - //returns the RPM based on the distance from the speaker - public ShooterWheelTargetSpeeds getSpeedForRange(){ - return converter.getRPMForDistance(pose.getDistanceFromSpeaker()); - } } diff --git a/src/main/java/competition/subsystems/shooter/commands/ContinuouslyWarmUpForSpeakerCommand.java b/src/main/java/competition/subsystems/shooter/commands/ContinuouslyWarmUpForSpeakerCommand.java new file mode 100644 index 00000000..e4635876 --- /dev/null +++ b/src/main/java/competition/subsystems/shooter/commands/ContinuouslyWarmUpForSpeakerCommand.java @@ -0,0 +1,27 @@ +package competition.subsystems.shooter.commands; + +import competition.subsystems.shooter.ShooterWheelSubsystem; +import xbot.common.command.BaseSetpointCommand; + +import javax.inject.Inject; + +public class ContinuouslyWarmUpForSpeakerCommand extends BaseSetpointCommand { + + ShooterWheelSubsystem shooter; + + @Inject + public ContinuouslyWarmUpForSpeakerCommand(ShooterWheelSubsystem shooter){ + super(shooter); + this.shooter = shooter; + } + + @Override + public void initialize() { + log.info("Initializing"); + } + + @Override + public void execute() { + shooter.setTargetValue(shooter.getSpeedForRange()); + } +} \ No newline at end of file diff --git a/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java b/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java index 0a6d2868..96cdd9f3 100644 --- a/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java +++ b/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java @@ -22,11 +22,12 @@ public class FireWhenReadyCommand extends BaseCommand { @Inject public FireWhenReadyCommand(ShooterWheelSubsystem wheel, ArmSubsystem arm, CollectorSubsystem collector, PropertyFactory pf) { + addRequirements(collector); this.wheel = wheel; this.arm = arm; this.collector = collector; - - this.waitTimeAfterFiring = pf.createPersistentProperty("WaitTimeAfterFiring", 0.5); + pf.setPrefix(this); + this.waitTimeAfterFiring = pf.createPersistentProperty("WaitTimeAfterFiring", 1.5); this.hasFired = false; pf.setPrefix(this); @@ -57,6 +58,12 @@ public void execute() { @Override public boolean isFinished() { - return hasFired && XTimer.getFPGATimestamp() + waitTimeAfterFiring.get() >= timeWhenFired; + return hasFired && timeWhenFired + waitTimeAfterFiring.get() <= XTimer.getFPGATimestamp(); + } + + @Override + public void end(boolean interrupted) { + log.info("Ending"); + super.end(interrupted); } } diff --git a/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java b/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java index 22218ec1..84f64f68 100644 --- a/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java +++ b/src/test/java/competition/subsystems/shooter/WarmUpShooterCommandTest.java @@ -24,7 +24,7 @@ public void setUp() { @Test public void testSetTargetRPM() { - Supplier safeRPMSupplier = ()->ShooterWheelSubsystem.TargetRPM.SAFE; + Supplier safeRPMSupplier = ()->ShooterWheelSubsystem.TargetRPM.SUBWOOFER; warmUpShooterCommand.setTargetRpm(safeRPMSupplier); warmUpShooterCommand.initialize(); warmUpShooterCommand.execute();