Merge pull request #17 from StuyPulse/se/7-19

Se/7 19

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -4,9 +4,11 @@
 import com.stuypulse.robot.commands.BuzzController;
 import com.stuypulse.robot.commands.arm.ArmToAmp;
 import com.stuypulse.robot.commands.arm.ArmToFeed;
-import com.stuypulse.robot.commands.arm.ArmToFerry;
+import com.stuypulse.robot.commands.arm.ArmToLobFerry;
+import com.stuypulse.robot.commands.arm.ArmToLowFerry;
 import com.stuypulse.robot.commands.arm.ArmToPreClimb;
-import com.stuypulse.robot.commands.arm.ArmToSpeaker;
+import com.stuypulse.robot.commands.arm.ArmToSpeakerHigh;
+import com.stuypulse.robot.commands.arm.ArmToSpeakerLow;
 import com.stuypulse.robot.commands.arm.ArmToStow;
 import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
@@ -15,12 +17,11 @@
 import com.stuypulse.robot.commands.intake.IntakeStop;
 import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake;
 import com.stuypulse.robot.commands.shooter.ShooterAutoShoot;
-import com.stuypulse.robot.commands.swerve.SwerveDriveAlignedFerryAndShoot;
-import com.stuypulse.robot.commands.swerve.SwerveDriveAlignedSpeakerAndShoot;
 import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
-import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedFerry;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedLowFerry;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerHigh;
 import com.stuypulse.robot.commands.swerve.SwerveDriveXMode;
-import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeaker;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedSpeakerLow;
 import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
@@ -41,6 +42,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 
 public class RobotContainer {
@@ -126,15 +128,22 @@ private void configureButtonBindings() {
             );
 
         driver.getRightBumper()
-            .onTrue(new SwerveDriveDriveAlignedSpeaker(driver));
+            .onTrue(new SwerveDriveDriveAlignedSpeakerLow(driver));
 
         driver.getTopButton()
-            .onTrue(new ArmToSpeaker());
-            // .onTrue(new SwerveDriveDriveAlignedSpeaker(driver));
+            .onTrue(new ConditionalCommand(
+                new ArmToSpeakerHigh(), 
+                new ArmToSpeakerLow(), 
+                () -> Arm.getInstance().getState() == Arm.State.SPEAKER_LOW));
+
         driver.getLeftButton().onTrue(new ArmToAmp());
+
         driver.getRightButton()
-            .onTrue(new ArmToFerry());
-            // .onTrue(new SwerveDriveAlignedFerry(driver));
+            .onTrue(new ConditionalCommand(
+                new ArmToLobFerry(), 
+                new ArmToLowFerry(), 
+                () -> Arm.getInstance().getState() == Arm.State.LOW_FERRY));
+
         driver.getBottomButton().onTrue(new ArmToFeed());
 
         driver.getDPadUp().onTrue(new ArmToPreClimb());

src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerry.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+public class ArmToLobFerry extends ArmSetState{
+
+    public ArmToLobFerry(){
+        super(Arm.State.LOB_FERRY);
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerry.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+public class ArmToLowFerry extends ArmSetState{
+
+    public ArmToLowFerry(){
+        super(Arm.State.LOW_FERRY);
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerHigh.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+public class ArmToSpeakerHigh extends ArmSetState{
+
+    public ArmToSpeakerHigh(){
+        super(Arm.State.SPEAKER_HIGH);
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/ArmToSpeakerLow.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.arm;
+
+import com.stuypulse.robot.subsystems.arm.Arm;
+
+public class ArmToSpeakerLow extends ArmSetState{
+
+    public ArmToSpeakerLow(){
+        super(Arm.State.SPEAKER_LOW);
+    }
+}

src/main/java/com/stuypulse/robot/commands/shooter/ShooterAutoShoot.java

@@ -16,10 +16,16 @@ public void initialize() {
             case AMP:
                 CommandScheduler.getInstance().schedule(new ShooterScoreAmp());
                 break;
-            case SPEAKER:
+            case SPEAKER_HIGH:
                 CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker());
                 break;
-            case FERRY:
+            case SPEAKER_LOW:
+                CommandScheduler.getInstance().schedule(new ShooterScoreSpeaker());
+                break;
+            case LOW_FERRY:
+                CommandScheduler.getInstance().schedule(new ShooterFerry());
+                break;
+            case LOB_FERRY:
                 CommandScheduler.getInstance().schedule(new ShooterFerry());
                 break;
             default:

src/main/java/com/stuypulse/robot/commands/shooter/ShooterFerry.java

@@ -8,9 +8,8 @@ public class ShooterFerry extends SequentialCommandGroup {
 
     public ShooterFerry() {
         addCommands(
-            new ShooterSetRPM(Settings.Shooter.FERRY),
-            new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))
-                .andThen(new ShooterFeederShoot())
+            new ShooterWaitForTarget().withTimeout((Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)),
+            new ShooterFeederShoot()
         );
     }
 

src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreAmp.java

@@ -1,5 +1,6 @@
 package com.stuypulse.robot.commands.shooter;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -12,7 +13,7 @@ public ShooterScoreAmp() {
         addCommands(
             new ShooterFeederDeacquire(),
             new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()),
-            new WaitUntilCommand(1),
+            new WaitUntilCommand(Settings.Arm.SHOULD_RETURN_TO_FEED_TIME.get()),
             new InstantCommand(() -> Shooter.getInstance().feederStop())
         );
     }

src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java

@@ -8,9 +8,8 @@ public class ShooterScoreSpeaker extends SequentialCommandGroup {
 
     public ShooterScoreSpeaker() {
         addCommands(
-            new ShooterSetRPM(Settings.Shooter.SPEAKER),
-            new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET)
-                .andThen(new ShooterFeederShoot())
+            new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET),
+            new ShooterFeederShoot()
         );
     }
 }

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -57,7 +57,7 @@ public SwerveDriveDrive(Gamepad driver) {
     @Override
     public void execute() {
         swerve.setControl(drive.withVelocityX(speed.get().y)
-                .withVelocityY(speed.get().x)
+                .withVelocityY(-speed.get().x)
                 .withRotationalRate(turn.get())         
             );
     }

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAligned.java

@@ -76,8 +76,8 @@ protected double getAngleError() {
     @Override
     public void execute() {
         swerve.setControl(
-            drive.withVelocityX(velocity.get().x)
-                .withVelocityY(velocity.get().y)
+            drive.withVelocityX(velocity.get().y)
+                .withVelocityY(-velocity.get().x)
                 .withRotationalRate(
                     angleVelocity.get() 
                     + controller.update(

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLobFerry.java

@@ -0,0 +1,35 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.input.Gamepad;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+
+public class SwerveDriveDriveAlignedLobFerry extends SwerveDriveDriveAligned {
+
+    public SwerveDriveDriveAlignedLobFerry(Gamepad driver) {
+        super(driver);
+    }
+
+    // returns pose of close amp corner
+    private Translation2d getAmpCornerPose() {
+        Translation2d targetPose = Robot.isBlue()
+            ? new Translation2d(0.0, Field.WIDTH - 1.5)
+            : new Translation2d(0.0, 1.5);
+
+        return targetPose;
+    }
+
+    @Override
+    protected Rotation2d getTargetAngle() {
+        return SwerveDrive.getInstance().getPose().getTranslation().minus(getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180));
+    }
+
+    @Override
+    protected double getDistanceToTarget() {
+        return SwerveDrive.getInstance().getPose().getTranslation().getDistance(getAmpCornerPose());
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedFerry.java → src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveAlignedLowFerry.java

@@ -8,9 +8,9 @@
 import edu.wpi.first.math.geometry.Translation2d;
 
 
-public class SwerveDriveDriveAlignedFerry extends SwerveDriveDriveAligned {
+public class SwerveDriveDriveAlignedLowFerry extends SwerveDriveDriveAligned {
 
-    public SwerveDriveDriveAlignedFerry(Gamepad driver) {
+    public SwerveDriveDriveAlignedLowFerry(Gamepad driver) {
         super(driver);
     }