From 43ac9ce84c039cdbc5dc4a92eaf3b6d5ecc3d2c7 Mon Sep 17 00:00:00 2001
From: superpenguin612 <74030080+superpenguin612@users.noreply.github.com>
Date: Mon, 19 Feb 2024 10:10:20 -0500
Subject: [PATCH] Add climbing commands, add shootertilt to shoot command, fix
 speaker targetting when on red alliance

---
 src/main/java/frc/robot/RobotCommands.java | 48 ++++++++++++++++++----
 1 file changed, 40 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java
index 842f8b6..76cf346 100644
--- a/src/main/java/frc/robot/RobotCommands.java
+++ b/src/main/java/frc/robot/RobotCommands.java
@@ -15,9 +15,14 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.Climber.ClimberPosition;
+import frc.robot.Constants.Intake.IntakePosition;
+import frc.robot.Constants.NoteLift.NoteLiftPosition;
 import frc.robot.Constants.ShooterTilt.ShooterTiltPosition;
+import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.NoteLift;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.ShooterTilt;
 
@@ -32,26 +37,53 @@ public static Command targetSpeakerCommand(Drivetrain drivetrain, Shooter shoote
                                     : FieldConstants.TARGET.toPose2d();
                     Transform2d diff = drivetrain.getPose().minus(target);
                     Rotation2d rot = new Rotation2d(diff.getX(), diff.getY());
-                    if (DriverStation.getAlliance().isPresent() &&
-                            DriverStation.getAlliance().get() == Alliance.Blue)
-                        rot = rot.plus(new Rotation2d(Math.PI));
+                    rot = rot.plus(new Rotation2d(Math.PI));
                     return rot.getRadians();
                 }, DriveMode.FIELD_ORIENTED),
-                shooterTilt.targetSpeakerCommand(drivetrain::getPose).asProxy(),
-                shooter.targetSpeakerCommand(drivetrain::getPose).asProxy());
+                shooterTilt.targetSpeakerCommand(drivetrain::getPose),
+                shooter.targetSpeakerCommand(drivetrain::getPose));
     }
 
     public static Command targetFromSubwooferCommand(Drivetrain drivetrain, Shooter shooter, ShooterTilt shooterTilt) {
         return Commands.parallel(
-                shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.SUBWOOFER).asProxy(),
-                shooter.spinAtVelocityCommand(() -> SHOOTING_RPS).asProxy());
+                shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.SUBWOOFER),
+                shooter.spinAtVelocityCommand(() -> SHOOTING_RPS));
     }
 
     public static Command shootCommand(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) {
         return Commands.sequence(
-                shooter.targetSpeakerCommand(drivetrain::getPose).asProxy()
+                Commands.parallel(
+                        shooterTilt.targetSpeakerCommand(drivetrain::getPose).asProxy(),
+                        shooter.targetSpeakerCommand(drivetrain::getPose).asProxy())
                         .until(() -> shooter.atGoal() && shooterTilt.atGoal()),
                 shooter.targetSpeakerCommand(drivetrain::getPose).asProxy().alongWith(intake.runRollersCommand())
                         .withTimeout(1));
     }
+
+    public static Command intakeToNoteLift(Shooter shooter, ShooterTilt shooterTilt, NoteLift noteLift) {
+        return Commands.parallel(
+                Commands.sequence(
+                        shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.CLIMB).asProxy(),
+                        noteLift.moveToPositionCommand(() -> NoteLiftPosition.INTAKE).asProxy()),
+                shooter.stopCommand());
+    }
+
+    public static Command prepareClimb(Intake intake, Shooter shooter, ShooterTilt shooterTilt, Climber climber,
+            NoteLift noteLift) {
+        return Commands.parallel(
+                Commands.sequence(
+                        shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.CLIMB).asProxy(),
+                        intake.moveToPositionCommand(() -> IntakePosition.GROUND).asProxy(),
+                        noteLift.moveToPositionCommand(() -> NoteLiftPosition.CLIMB_PREP).asProxy(),
+                        climber.moveToPositionCommand(() -> ClimberPosition.CLIMB_PREP).asProxy()),
+                shooter.stopCommand());
+    }
+
+    public static Command resetClimb(Intake intake, Shooter shooter, ShooterTilt shooterTilt, Climber climber,
+            NoteLift noteLift) {
+        return Commands.parallel(
+                Commands.sequence(
+                        shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.CLIMB).asProxy(),
+                        climber.moveToPositionCommand(() -> ClimberPosition.BOTTOM).asProxy()));
+    }
 }