Portland #1

src/main/java/competition/Robot.java

@@ -20,6 +20,7 @@
 import xbot.common.math.FieldPose;
 import xbot.common.math.MovingAverageForDouble;
 import xbot.common.math.MovingAverageForTranslation2d;
+import xbot.common.subsystems.drive.swerve.SwerveDriveSubsystem;
 import xbot.common.subsystems.pose.BasePoseSubsystem;
 
 public class Robot extends BaseRobot {
@@ -31,6 +32,7 @@ public Robot() {
     DynamicOracle oracle;
     PoseSubsystem poseSubsystem;
     OperatorInterface oi;
+    DriveSubsystem drive;
 
     @Override
     protected void initializeSystems() {
@@ -45,8 +47,9 @@ protected void initializeSystems() {
         }
         oracle = getInjectorComponent().dynamicOracle();
         oi = getInjectorComponent().operatorInterface();
+        drive = (DriveSubsystem)getInjectorComponent().driveSubsystem();
 
-        dataFrameRefreshables.add((DriveSubsystem)getInjectorComponent().driveSubsystem());
+        dataFrameRefreshables.add(drive);
         poseSubsystem = (PoseSubsystem) getInjectorComponent().poseSubsystem();
         dataFrameRefreshables.add(poseSubsystem);
         dataFrameRefreshables.add(getInjectorComponent().visionSubsystem());
@@ -105,6 +108,7 @@ public void teleopInit() {
         super.teleopInit();
         oracle.clearNoteMapForTeleop();
         oracle.clearScoringLocationsForTeleop();
+        drive.setDriveModuleCurrentLimits(SwerveDriveSubsystem.CurrentLimitMode.Teleop);
     }
 
     @Override

src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java

@@ -17,6 +17,7 @@
 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.swerve.SwerveDriveSubsystem;
 import xbot.common.trajectory.XbotSwervePoint;
 
 import javax.inject.Inject;
@@ -27,7 +28,13 @@
 //when we have the time to tune a ranged shot will update to that
 public class SubwooferShotFromBotThenTwoCenterline extends SequentialCommandGroup {
     AutonomousCommandSelector autoSelector;
-    double centerlineTimeout = 8;
+    double centerlineTimeout = 999;
+
+    double meterThreshold = 0.3048;
+    double velocityThreshold = 0.05;
+    final PoseSubsystem pose;
+    final DriveSubsystem drive;
+
     @Inject
     public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelector, PoseSubsystem pose,
                                                  DriveSubsystem drive,
@@ -37,8 +44,14 @@ public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelec
                                                  Provider<DriveToListOfPointsCommand> driveToListOfPointsCommandProvider,
                                                  Provider<IntakeCollectorCommand> intakeCollectorCommandProvider
                                                  ){
+        this.pose = pose;
+        this.drive = drive;
+
         this.autoSelector = autoSelector;
 
+        var limitCurrentCommand = drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto);
+        this.addCommands(limitCurrentCommand);
+
         var startInFrontOfSpeaker = pose.createSetPositionCommand(
                 () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation));
         this.addCommands(startInFrontOfSpeaker);
@@ -52,29 +65,32 @@ public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelec
         queueMessageToAutoSelector("Drive to bottom spike note, collect, drive back to sub(middle) and shoot");
         this.addCommands(
                 new InstantCommand(() -> {
-                    drive.setTargetNote(PoseSubsystem.CenterLine5);
+                    drive.setTargetNote(PoseSubsystem.CenterLine4);
                 })
         );
-        var driveToCenterline5 = driveToNoteProvider.get();
+        var driveToCenterline4 = driveToNoteProvider.get();
         this.addCommands(
                 new InstantCommand(()->{
-                    driveToCenterline5.setWaypoints(new Translation2d(
+                    driveToCenterline4.setWaypoints(new Translation2d(
                             PoseSubsystem.BlueSpikeBottom.getX() + 2.06,
                             PoseSubsystem.BlueSpikeBottom.getY() - 2.3951
                     ));
                 })
         );
-        driveToCenterline5.logic.setEnableConstantVelocity(true);
-        driveToCenterline5.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed());
-        driveToCenterline5.setVisionRangeOverride(VisionRange.Far);
+        driveToCenterline4.logic.setEnableConstantVelocity(true);
+        driveToCenterline4.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed());
+        driveToCenterline4.setVisionRangeOverride(VisionRange.Far);
 
         var collect1 = collectSequenceCommandGroupProvider.get();
         //swap collect and drive for testing
-        this.addCommands(Commands.deadline(collect1,driveToCenterline5).withTimeout(centerlineTimeout));
+        this.addCommands(Commands.deadline(collect1,driveToCenterline4).withTimeout(centerlineTimeout));
+
+        addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Teleop));
 
         var driveBackToBottomSubwooferFirst = driveToListOfPointsCommandProvider.get();
         driveBackToBottomSubwooferFirst.addPointsSupplier(this::goBackToBotSubwoofer);
         driveBackToBottomSubwooferFirst.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed());
+        driveBackToBottomSubwooferFirst.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer);
 
         var collect3 = intakeCollectorCommandProvider.get();
         this.addCommands(Commands.deadline(driveBackToBottomSubwooferFirst.withTimeout(centerlineTimeout),collect3));
@@ -85,30 +101,35 @@ public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelec
 
         this.addCommands(
                 new InstantCommand(() -> {
-                    drive.setTargetNote(PoseSubsystem.CenterLine4);
+                    drive.setTargetNote(PoseSubsystem.CenterLine5);
                 })
         );
-        var driveToCenterline4 = driveToNoteProvider.get();
+        var driveToCenterline5 = driveToNoteProvider.get();
         this.addCommands(
                 new InstantCommand(()->{
-                    driveToCenterline4.setWaypoints(new Translation2d(
+                    driveToCenterline5.setWaypoints(new Translation2d(
                             PoseSubsystem.BlueSpikeBottom.getX() + 2.06,
                             PoseSubsystem.BlueSpikeBottom.getY() - 2.3951
                     ));
                 })
         );
 
-        driveToCenterline4.logic.setEnableConstantVelocity(true);
-        driveToCenterline4.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed());
-        driveToCenterline4.setVisionRangeOverride(VisionRange.Far);
+        addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto));
+
+        driveToCenterline5.logic.setEnableConstantVelocity(true);
+        driveToCenterline5.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed());
+        driveToCenterline5.setVisionRangeOverride(VisionRange.Far);
 
         var collect2 = collectSequenceCommandGroupProvider.get();
         //swap collect and drive for testing
-        this.addCommands(Commands.deadline(collect2,driveToCenterline4).withTimeout(centerlineTimeout));
+        this.addCommands(Commands.deadline(collect2,driveToCenterline5).withTimeout(centerlineTimeout));
+
+        addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto));
 
         var driveBackToBottomSubwooferSecond = driveToListOfPointsCommandProvider.get();
         driveBackToBottomSubwooferSecond.addPointsSupplier(this::goBackToBotSubwoofer);
         driveBackToBottomSubwooferSecond.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed());
+        driveBackToBottomSubwooferSecond.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer);
 
         var collect4 = intakeCollectorCommandProvider.get();
         this.addCommands(Commands.deadline(driveBackToBottomSubwooferSecond.withTimeout(centerlineTimeout),collect4));
@@ -130,4 +151,19 @@ private ArrayList<XbotSwervePoint> goBackToBotSubwoofer(){
         points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(PoseSubsystem.BlueSubwooferBottomScoringLocation,10));
         return points;
     }
+
+    private boolean alternativeIsFinishedForSubwoofer() {
+        double speed = pose.getRobotCurrentSpeed();
+
+        Translation2d robotLocation = pose.getCurrentPose2d().getTranslation();
+
+        // Returns finished if both position and velocity are under threshold
+        boolean nearPositionThreshold = PoseSubsystem.convertBlueToRedIfNeeded(
+                PoseSubsystem.BlueSubwooferBottomScoringLocation)
+                .getTranslation().getDistance(robotLocation) < meterThreshold;
+
+        boolean nearVelocityThreshold = speed < velocityThreshold;
+
+        return (nearPositionThreshold && nearVelocityThreshold);
+    }
 }

src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java

@@ -66,7 +66,7 @@ public void execute() {
                     }
                 }
                 break;
-            case VisionApproach:
+            case CenterCameraVisionApproach:
                 if (super.isFinished()) {
                     // we've hit our target, but no note! Back away and try again.
                     log.info("Switching to back away mode");
@@ -80,7 +80,7 @@ public void execute() {
                     if (getNearestVisionNote() != null) {
                         log.info("Switching to vision mode");
                         assignClosestVisionNoteToDriveSubsystemIfYouSeeANoteAndReplanPath();
-                        noteAcquisitionMode = NoteAcquisitionMode.VisionApproach;
+                        noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach;
                     } else {
                         log.info("Switching to give up mode");
                         noteAcquisitionMode = NoteAcquisitionMode.GiveUp;
@@ -152,7 +152,7 @@ private void assignClosestVisionNoteToDriveSubsystemIfYouSeeANoteAndReplanPath()
         log.info("Found note at " + noteLocation.getTranslation());
         log.info("Assigning note to drive subsystem");
         drive.setTargetNote(noteLocation);
-        noteAcquisitionMode = NoteAcquisitionMode.VisionApproach;
+        noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach;
         prepareToDriveAtGivenNote();
     }
 

src/main/java/competition/subsystems/collector/commands/WaitForNoteCollectedCommand.java

@@ -25,9 +25,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return collector.confidentlyHasControlOfNote()
-                || collector.getGamePieceInControl()
-                || collector.getGamePieceReady();
+        return collector.confidentlyHasControlOfNote();
     }
 
 

src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java

@@ -92,9 +92,9 @@ public void execute() {
             case BackAwayToTryAgain:
                 super.execute();
                 break;
-            // If we are using
-            case VisionApproach:
-            case VisionTerminalApproach:
+            // If we are using the cameras, listen to the suggested powers
+            case CenterCameraVisionApproach:
+            case CenterCameraTerminalApproach:
             case SearchViaRotation:
                 if (currentAdvice.suggestedDrivePercentages.isPresent()) {
                     var driveValues = currentAdvice.suggestedDrivePercentages.get();

src/main/java/competition/subsystems/drive/commands/TryDriveToBearingNote.java

@@ -26,7 +26,6 @@ public TryDriveToBearingNote(
     @Override
     public void initialize() {
         log.info("Initializing");
-        noteSeekLogic.setInitialMode(NoteAcquisitionMode.SearchViaRotation);
         noteSeekLogic.setAllowRotationSearch(true);
         super.initialize();
     }