mm

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index db296538790070cd5029587ca947c9f9465afac1..1d222583b429523101a58262541bbc0bb388848e 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -20,6 +20,7 @@ import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.hood.HoodConstants;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.turret.ShotInterpolation;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.turret.TurretConstants;
@@ -32,6 +33,7 @@ public class AutoShootCommand extends Command {
     private Drivetrain drivetrain;
     private Hood hood;
     private Shooter shooter;
+    private Spindexer spindexer;
 
     //TODO: find maximum interpolation
     private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
@@ -56,11 +58,12 @@ public class AutoShootCommand extends Command {
 
     private final double phaseDelay = 0.03;
 
-    public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter) {
+    public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
         this.turret = turret;
         this.drivetrain = drivetrain;
         this.hood = hood;
         this.shooter = shooter;
+        this.spindexer = spindexer;
         drivepose  = drivetrain.getPose();
 
         goalState = ShooterPhysics.getShotParams(
@@ -135,7 +138,7 @@ public class AutoShootCommand extends Command {
 
         turretSetpoint = potentialSetpoint;
 
-        // Hood stuff
+        /** Hood Stuff!! */
         //hoodAngle = ShotInterpolation.hoodAngleMap.get(lookaheadTurretToTargetDistance);
         // Pitch is in radians
         hoodAngle = goalState.pitch();
@@ -160,6 +163,11 @@ public class AutoShootCommand extends Command {
         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
         hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
         shooter.setShooter(Units.radiansToRotations(goalState.exitVel() / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)));
+
+        /** Spindexer Stuff!! */
+        if(spindexer != null){
+            spindexer.turnOnSpindexer();
+        }
     }
 
     @Override
@@ -167,6 +175,9 @@ public class AutoShootCommand extends Command {
         // Set the turret to a safe position when the command ends
         turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
         hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
+        if(spindexer != null){
+            spindexer.stopSpindexer();
+        }
     }
 
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index ba896246553e5ddafe827795074dd4129dfb24aa..7ae55d124552f2b3782896aff913297893c17548 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -27,6 +27,7 @@ import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.hood.HoodConstants;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeConstants;
 import lib.controllers.PS5Controller;
 import lib.controllers.PS5Controller.PS5Axis;
 import lib.controllers.PS5Controller.PS5Button;
@@ -48,6 +49,8 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Command simpleTurretAutoShoot;
     private TurretJoyStickAim turretJoyStickAim;
 
+    private boolean intakeBoolean = true;
+
     public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood) {
         super(drive);
         this.shooter = shooter;
@@ -75,8 +78,19 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             interrupted->getDrivetrain().setStateDeadband(true),
             ()->false, getDrivetrain()).withTimeout(2));
 
+        // Intake
         if(intake != null){
-            
+            driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
+                if(intakeBoolean){
+                    intake.setSetpoint(IntakeConstants.INTAKE_ANGLE);
+                    intake.setFlyWheel();
+                    intakeBoolean = false;
+                }
+                else{
+                    intake.setSetpoint(IntakeConstants.STOW_ANGLE);
+                    intake.stopFlyWheel();
+                }
+            }));
         }
 
         // driver.get(PS5Button.LB).onTrue(