diff --git a/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java b/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java new file mode 100644 index 00000000..0200dba2 --- /dev/null +++ b/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java @@ -0,0 +1,105 @@ +package competition.auto_programs; + +import competition.subsystems.collector.commands.EjectCollectorCommand; +import competition.subsystems.collector.commands.IntakeCollectorCommand; +import competition.subsystems.collector.commands.StopCollectorCommand; +import competition.subsystems.drive.commands.StopDriveCommand; +import competition.subsystems.pose.PoseSubsystem; +import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.shooter.commands.FireWhenReadyCommand; +import competition.subsystems.shooter.commands.WarmUpShooterCommand; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import xbot.common.subsystems.autonomous.AutonomousCommandSelector; +import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; +import xbot.common.subsystems.pose.BasePoseSubsystem; +import xbot.common.trajectory.XbotSwervePoint; + +import javax.inject.Inject; +import javax.inject.Provider; +import java.util.ArrayList; + +public class FromMidShootCollectShoot extends SequentialCommandGroup { + + final AutonomousCommandSelector autoSelector; + + @Inject + public FromMidShootCollectShoot( + PoseSubsystem pose, + Provider warmUpShooterCommandProvider, + Provider swerveProvider, + Provider fireWhenReadyCommandProvider, + IntakeCollectorCommand intake, + EjectCollectorCommand eject, + StopCollectorCommand stopCollector, + StopDriveCommand stopDrive, + AutonomousCommandSelector autoSelector) { + this.autoSelector = autoSelector; + // Force our location + var startInFrontOfSpeaker = pose.createSetPositionCommand( + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation)); + this.addCommands(startInFrontOfSpeaker); + + // Shoot the pre-loaded note from the subwoofer + queueMessageToAutoSelector("Shoot pre-loaded note from subwoofer"); + var warmUpForFirstSubwooferShot = warmUpShooterCommandProvider.get(); + warmUpForFirstSubwooferShot.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER); + var fireFirstShot = fireWhenReadyCommandProvider.get(); + + this.addCommands(Commands.deadline(fireFirstShot, + warmUpForFirstSubwooferShot)); + + // Drive to the middle note while collecting + queueMessageToAutoSelector("Drive to middle note"); + var driveToMiddleSpike = swerveProvider.get(); + driveToMiddleSpike.logic.setEnableConstantVelocity(true); + driveToMiddleSpike.logic.setConstantVelocity(1); + driveToMiddleSpike.logic.setAimAtGoalDuringFinalLeg(true); + driveToMiddleSpike.logic.setKeyPointsProvider(() -> { + ArrayList points = new ArrayList<>(); + var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SpikeMiddle); + points.add(new XbotSwervePoint(target.getTranslation(), target.getRotation(), 10)); + return points; + }); + var stopShooter = warmUpShooterCommandProvider.get(); + stopShooter.setTargetRpm(ShooterWheelSubsystem.TargetRPM.STOP); + + this.addCommands(Commands.deadline(driveToMiddleSpike, + intake, stopShooter)); + + // Return to subwoofer, and also (eject for 0.1 seconds, warm up shooter) + queueMessageToAutoSelector("Return to subwoofer"); + var driveToSubwoofer = swerveProvider.get(); + driveToSubwoofer.logic.setEnableConstantVelocity(true); + driveToSubwoofer.logic.setConstantVelocity(1); + driveToSubwoofer.logic.setKeyPointsProvider(() -> { + ArrayList points = new ArrayList<>(); + var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation); + points.add(new XbotSwervePoint(target.getTranslation(), target.getRotation(), 10)); + return points; + }); + + var moveNoteAwayFromShooterWheels = + eject.withTimeout(0.1).andThen(stopCollector.withTimeout(0.05)); + var warmUpForSecondSubwooferShot = warmUpShooterCommandProvider.get(); + warmUpForSecondSubwooferShot.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER); + + this.addCommands(Commands.deadline(driveToSubwoofer, + moveNoteAwayFromShooterWheels.andThen(warmUpForSecondSubwooferShot))); + + // Fire note + queueMessageToAutoSelector("Fire second note"); + var fireSecondShot = fireWhenReadyCommandProvider.get(); + this.addCommands(Commands.deadline(fireSecondShot, + stopDrive)); + } + + private void queueMessageToAutoSelector(String message) { + this.addCommands( + new InstantCommand(() -> { + autoSelector.setAutonomousState(message); + }) + ); + } +} diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 1bca90e1..5713063a 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -4,12 +4,14 @@ import javax.inject.Provider; import javax.inject.Singleton; +import competition.auto_programs.FromMidShootCollectShoot; import competition.commandgroups.PrepareToFireAtAmpCommandGroup; import competition.commandgroups.PrepareToFireAtSpeakerCommandGroup; 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.ManipulateArmBrakeCommand; import competition.subsystems.arm.commands.SetArmAngleCommand; import competition.subsystems.arm.commands.SetArmExtensionCommand; import competition.subsystems.collector.commands.EjectCollectorCommand; @@ -63,7 +65,9 @@ public void setupFundamentalCommands( FireCollectorCommand fireCollectorCommand, SetArmAngleCommand armAngle, ArmMaintainerCommand armMaintainer, - WarmUpShooterRPMCommand warmUpShooterDifferentialRPM + WarmUpShooterRPMCommand warmUpShooterDifferentialRPM, + ManipulateArmBrakeCommand engageBrake, + ManipulateArmBrakeCommand disengageBrake ) { // Scooch oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get()); @@ -94,6 +98,12 @@ public void setupFundamentalCommands( warmUpShooterDifferentialRPM.setTargetRpm(new ShooterWheelTargetSpeeds(1000, 2000)); //oi.operatorFundamentalsGamepad.getPovIfAvailable(0).whileTrue(warmUpShooterDifferentialRPM); + + engageBrake.setBrakeMode(true); + disengageBrake.setBrakeMode(false); + + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightJoystickYAxisPositive).onTrue(engageBrake); + oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightJoystickYAxisNegative).onTrue(disengageBrake); } // Example for setting up a command to fire when a button is pressed: @@ -291,6 +301,12 @@ public void setupAdvancedOperatorCommands( oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightTrigger).whileTrue(fireWhenReady); } + @Inject + public void setupAutonomous(OperatorInterface oi, + FromMidShootCollectShoot fromMidShootCollectShoot) { + oi.operatorGamepadAdvanced.getPovIfAvailable(0).whileTrue(fromMidShootCollectShoot); + } + private SwerveSimpleTrajectoryCommand createAndConfigureTypicalSwerveCommand( SwerveSimpleTrajectoryCommand command, Pose2d target, double targetVelocity, LowResField fieldWithObstacles) { diff --git a/src/main/java/competition/simulation/Simulator2024.java b/src/main/java/competition/simulation/Simulator2024.java index 1c378a67..e610dde8 100644 --- a/src/main/java/competition/simulation/Simulator2024.java +++ b/src/main/java/competition/simulation/Simulator2024.java @@ -2,13 +2,19 @@ import com.revrobotics.CANSparkBase; import competition.subsystems.arm.ArmSubsystem; +import competition.subsystems.collector.CollectorSubsystem; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.shooter.ShooterWheelSubsystem; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import org.littletonrobotics.junction.Logger; import xbot.common.controls.actuators.mock_adapters.MockCANSparkMax; +import xbot.common.controls.sensors.XTimer; import xbot.common.math.MovingAverageForDouble; import xbot.common.math.MovingAverageForTranslation2d; @@ -31,18 +37,23 @@ public class Simulator2024 { private final DriveSubsystem drive; private final ArmSubsystem arm; private final ShooterWheelSubsystem shooter; + private final CollectorSubsystem collector; @Inject public Simulator2024(PoseSubsystem pose, DriveSubsystem drive, - ArmSubsystem arm, ShooterWheelSubsystem shooter) { + ArmSubsystem arm, ShooterWheelSubsystem shooter, + CollectorSubsystem collector) { this.pose = pose; this.drive = drive; this.arm = arm; this.shooter = shooter; + this.collector = collector; } + int loop = 0; + public void update() { double robotTopSpeedInMetersPerSecond = 3.0; double robotLoopPeriod = 0.02; @@ -106,10 +117,61 @@ public void update() { } // The shooter wheel should pretty much always be in velocity mode. - var shooterMockMotor = (MockCANSparkMax)shooter.upperWheelMotor; - shooterVelocityCalculator.add(shooterMockMotor.getReference()); - if (shooterMockMotor.getControlType() == CANSparkBase.ControlType.kVelocity) { - shooterMockMotor.setVelocity(shooterVelocityCalculator.getAverage()); + var shooterUpperMockMotor = (MockCANSparkMax)shooter.upperWheelMotor; + var shooterLowerMockMotor = (MockCANSparkMax)shooter.lowerWheelMotor; + + if (shooterUpperMockMotor.getControlType() == CANSparkBase.ControlType.kVelocity) { + shooterVelocityCalculator.add(shooterUpperMockMotor.getReference()); + } else { + shooterVelocityCalculator.add(0.0); + } + + shooterUpperMockMotor.setVelocity(shooterVelocityCalculator.getAverage()); + shooterLowerMockMotor.setVelocity(shooterVelocityCalculator.getAverage()); + + double currentCollectorPower = collector.collectorMotor.getAppliedOutput(); + if (currentCollectorPower == 1 + && lastCollectorPower == 0) { + initializeNewFiredNote(); + } + lastCollectorPower = currentCollectorPower; + moveNote(); + } + + double timeNoteFired = 0; + Translation3d noteStartPoint; + Translation3d noteFinishPoint; + double lastCollectorPower = 0; + + private void initializeNewFiredNote() { + timeNoteFired = XTimer.getFPGATimestamp(); + + // get current position and angle + var currentPose = pose.getCurrentPose2d(); + // create a translation3d representing note start point. + noteStartPoint = new Translation3d( + currentPose.getTranslation().getX() + + Math.cos(currentPose.getRotation().getRadians() + Math.PI) * 0.33, + currentPose.getTranslation().getY() + + Math.sin(currentPose.getRotation().getRadians() + Math.PI) * 0.33, + 0.33 + ); + + // create a translation3d representing note finish point. + noteFinishPoint = new Translation3d( + currentPose.getTranslation().getX() + + Math.cos(currentPose.getRotation().getRadians() + Math.PI) * 3, + currentPose.getTranslation().getY() + + Math.sin(currentPose.getRotation().getRadians() + Math.PI) * 3, + 3 + ); + } + + private void moveNote() { + double timeSinceNoteFired = XTimer.getFPGATimestamp() - timeNoteFired; + if (noteStartPoint != null && noteFinishPoint != null) { + var interpolatedPosition = noteStartPoint.interpolate(noteFinishPoint, timeSinceNoteFired); + Logger.recordOutput("VirtualNote", new Pose3d(interpolatedPosition, new Rotation3d())); } } } diff --git a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java index e5b19abd..27ada08d 100644 --- a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java +++ b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java @@ -5,7 +5,7 @@ import competition.subsystems.arm.ArmSubsystem; import competition.subsystems.arm.commands.ArmMaintainerCommand; -import competition.subsystems.arm.commands.StopArmCommand; +import competition.subsystems.arm.commands.SetArmTargetToCurrentPositionCommand; import competition.subsystems.collector.CollectorSubsystem; import competition.subsystems.collector.commands.StopCollectorCommand; import competition.subsystems.schoocher.ScoocherSubsystem; @@ -14,11 +14,6 @@ import competition.subsystems.shooter.ShooterWheelTargetSpeeds; import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand; import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand; -import xbot.common.injection.swerve.FrontLeftDrive; -import xbot.common.injection.swerve.FrontRightDrive; -import xbot.common.injection.swerve.RearLeftDrive; -import xbot.common.injection.swerve.RearRightDrive; -import xbot.common.injection.swerve.SwerveComponent; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; @@ -37,8 +32,11 @@ public void setupDriveSubsystem(DriveSubsystem driveSubsystem, SwerveDriveWithJo } @Inject - public void setupArmSubsystem(ArmSubsystem armSubsystem, ArmMaintainerCommand command) { + public void setupArmSubsystem(ArmSubsystem armSubsystem, + ArmMaintainerCommand command, + SetArmTargetToCurrentPositionCommand setArmTargetToCurrentPositionCommand) { armSubsystem.setDefaultCommand(command); + armSubsystem.getSetpointLock().setDefaultCommand(setArmTargetToCurrentPositionCommand); } @Inject public void setupScoocherSubsystem(ScoocherSubsystem scoocherSubsystem, StopScoocherCommand command){ diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index d11a09c4..b6686fe4 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -327,13 +327,22 @@ else if (distanceLeftAhead < 0) { } } + // finally, if the brake is engaged, just stop the motors. + if (getBrakeEngaged()) { + leftPower = 0; + rightPower = 0; + } + if (contract.isArmReady()) { armMotorLeft.set(leftPower); armMotorRight.set(rightPower); } } + + boolean brakeEngaged = false; //brake solenoid public void setBrakeEnabled(boolean enabled) { + brakeEngaged = enabled; if (enabled) { armBrakeSolenoid.setForward(); } else { @@ -341,6 +350,10 @@ public void setBrakeEnabled(boolean enabled) { } } + public boolean getBrakeEngaged() { + return brakeEngaged; + } + double previousPower; @Override diff --git a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java index e9fb9eff..a62fab6e 100644 --- a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java +++ b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java @@ -25,6 +25,8 @@ public class ArmMaintainerCommand extends BaseMaintainerCommand { TimeStableValidator calibrationValidator; final double calibrationStallDurationSec = 0.5; + private boolean dynamicBrakingEnabled = false; + @Inject public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf, HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory, @@ -52,10 +54,17 @@ protected void coastAction() { @Override protected void calibratedMachineControlAction() { // 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. + // then we need to kill power. if (isMaintainerAtGoal()) { + if (dynamicBrakingEnabled) { + // Engage the brake so we don't backdrive away from this point + // (vibrations from the shooter can cause that) + arm.setBrakeEnabled(true); + } arm.setPower(0.0); } else { + // If we need to move, disengage the brake + arm.setBrakeEnabled(false); double power = positionPid.calculate(arm.getTargetValue(), arm.getCurrentValue()); arm.setPower(power); } @@ -68,6 +77,8 @@ protected void uncalibratedMachineControlAction() { aKitLog.record("Started Calibration", startedCalibration); aKitLog.record("Given Up On Calibration", givenUpOnCalibration); + arm.setBrakeEnabled(false); + // Try to auto-calibrate. if (!startedCalibration) { calibrationStartTime = XTimer.getFPGATimestamp(); @@ -109,6 +120,15 @@ protected void uncalibratedMachineControlAction() { } } + @Override + protected void humanControlAction() { + // Disable the brake before letting the human take over. + if (dynamicBrakingEnabled) { + arm.setBrakeEnabled(false); + } + super.humanControlAction(); + } + @Override protected double getErrorMagnitude() { double current = arm.getCurrentValue(); diff --git a/src/main/java/competition/subsystems/arm/commands/ManipulateArmBrakeCommand.java b/src/main/java/competition/subsystems/arm/commands/ManipulateArmBrakeCommand.java new file mode 100644 index 00000000..97bf61a0 --- /dev/null +++ b/src/main/java/competition/subsystems/arm/commands/ManipulateArmBrakeCommand.java @@ -0,0 +1,35 @@ +package competition.subsystems.arm.commands; + +import competition.subsystems.arm.ArmSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; +import java.lang.reflect.Executable; + +public class ManipulateArmBrakeCommand extends BaseCommand { + + ArmSubsystem arm; + boolean brakeEngaged; + + @Inject + public ManipulateArmBrakeCommand(ArmSubsystem arm) { + this.arm = arm; + addRequirements(arm); + } + + public void setBrakeMode(boolean brakeEngaged) { + this.brakeEngaged = brakeEngaged; + } + + @Override + public void initialize() { + log.info("Initializing"); + log.info("Setting arm brake to " + brakeEngaged); + arm.setBrakeEnabled(brakeEngaged); + } + + @Override + public void execute() { + // No-op. + } +} diff --git a/src/main/java/competition/subsystems/arm/commands/SetArmTargetToCurrentPositionCommand.java b/src/main/java/competition/subsystems/arm/commands/SetArmTargetToCurrentPositionCommand.java new file mode 100644 index 00000000..bf077e4e --- /dev/null +++ b/src/main/java/competition/subsystems/arm/commands/SetArmTargetToCurrentPositionCommand.java @@ -0,0 +1,28 @@ +package competition.subsystems.arm.commands; + +import competition.subsystems.arm.ArmSubsystem; +import xbot.common.command.BaseSetpointCommand; + +import javax.inject.Inject; + +public class SetArmTargetToCurrentPositionCommand extends BaseSetpointCommand { + + ArmSubsystem arm; + + @Inject + public SetArmTargetToCurrentPositionCommand(ArmSubsystem arm) { + super(arm); + this.arm = arm; + } + + @Override + public void initialize() { + log.info("Initializing"); + arm.setTargetValue(arm.getCurrentValue()); + } + + @Override + public void execute() { + // No-op. + } +} diff --git a/src/main/java/competition/subsystems/drive/commands/StopDriveCommand.java b/src/main/java/competition/subsystems/drive/commands/StopDriveCommand.java new file mode 100644 index 00000000..6276e100 --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/StopDriveCommand.java @@ -0,0 +1,27 @@ +package competition.subsystems.drive.commands; + +import competition.subsystems.drive.DriveSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; + +public class StopDriveCommand extends BaseCommand { + + final DriveSubsystem drive; + + @Inject + public StopDriveCommand(DriveSubsystem drive) { + this.drive = drive; + this.addRequirements(drive); + } + + @Override + public void initialize() { + log.info("Initializing"); + } + + @Override + public void execute() { + drive.stop(); + } +} diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index eb7f4313..977976fb 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -30,6 +30,7 @@ import java.util.Optional; +import java.util.function.Supplier; @Singleton @@ -220,6 +221,10 @@ public Command createSetPositionCommand(Pose2d pose) { return Commands.runOnce(() -> setCurrentPosition(pose)); } + public Command createSetPositionCommand(Supplier poseSupplier) { + return Commands.runOnce(() -> setCurrentPosition(poseSupplier.get())); + } + private void improveFusedOdometryUsingPhotonLib(Pose2d recentPosition) { var photonEstimatedPoses = vision.getPhotonVisionEstimatedPoses(recentPosition); diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java index 67e0c249..89c22288 100644 --- a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java +++ b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java @@ -19,6 +19,7 @@ @Singleton public class ShooterWheelSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { public enum TargetRPM { + STOP, SUBWOOFER, NEARSHOT, DISTANCESHOT, @@ -112,6 +113,7 @@ public ShooterWheelSubsystem(XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, P public void setTargetRPM(TargetRPM target) { switch (target) { + case STOP -> setTargetValue(0.0); case SUBWOOFER -> setTargetValue(safeRpm.get()); case NEARSHOT -> setTargetValue(nearShotRpm.get()); case DISTANCESHOT -> setTargetValue(distanceShotRpm.get());