Arm Brake & 2-Note Auto with Simulated Note

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<WarmUpShooterCommand> warmUpShooterCommandProvider,
+            Provider<SwerveSimpleTrajectoryCommand> swerveProvider,
+            Provider<FireWhenReadyCommand> 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<XbotSwervePoint> 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<XbotSwervePoint> 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);
+            })
+        );
+    }
+}

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) {
 

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()));
         }
     }
 }

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){

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -327,20 +327,33 @@ 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 {
             armBrakeSolenoid.setReverse();
         }
     }
 
+    public boolean getBrakeEngaged() {
+        return brakeEngaged;
+    }
+
     double previousPower;
 
     @Override

src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java

@@ -25,6 +25,8 @@ public class ArmMaintainerCommand extends BaseMaintainerCommand<Double> {
     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();