florida comp

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.1"
 }
 
 java {

src/main/java/frc4388/robot/Constants.java

@@ -25,21 +25,23 @@ import frc4388.utility.Gains;
 public final class Constants {
     public static final class SwerveDriveConstants {
 
-        public static final double MAX_ROT_SPEED        = 1.5;
+        public static final double MAX_ROT_SPEED        = 3.5;
+        public static final double AUTO_MAX_ROT_SPEED = 1.5;
         public static final double MIN_ROT_SPEED        = 0.8;
         public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
+        public static       double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
         public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
     
         public static final double CORRECTION_MIN = 10;
         public static final double CORRECTION_MAX = 50;
     
-        public static final double SLOW_SPEED = 0.4;
+        public static final double SLOW_SPEED = 0.2;
         public static final double FAST_SPEED = 1.0;
         public static final double TURBO_SPEED = 4.0;
     
         public static final class DefaultSwerveRotOffsets {
           public static final double FRONT_LEFT_ROT_OFFSET = 130.957 + 90;
-          public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 90;//-202.588;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 77.783 + 45 + 90 ;//-202.588;
           public static final double BACK_LEFT_ROT_OFFSET = 347.871 + 90;
           public static final double BACK_RIGHT_ROT_OFFSET = 52.646 + 90;
         }
@@ -65,7 +67,7 @@ public final class Constants {
         public static final class PIDConstants {
             public static final int SWERVE_SLOT_IDX = 0;
             public static final int SWERVE_PID_LOOP_IDX = 1;
-            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0, 0.0, 0, 1.0);
+            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.1, 0.0, 0, 1.0);
         }
     
         public static final class AutoConstants {
@@ -168,7 +170,7 @@ public final class Constants {
     public static final class ShooterConstants {
         public static final int LEFT_SHOOTER_ID = 15; // final
         public static final int RIGHT_SHOOTER_ID = 16; // final
-        public static final double SHOOTER_SPEED = 0.40; // final
+        public static final double SHOOTER_SPEED = 0.50; // final
         public static final double SHOOTER_IDLE = 0.35; // final
         public static final double SHOOTER_IDLE_LIMELIGHT = 0.8;
     }

src/main/java/frc4388/robot/RobotContainer.java

@@ -32,6 +32,7 @@ import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Intake;
+import frc4388.utility.DeferredBlock;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.VirtualController;
 import frc4388.utility.controller.XboxController;
@@ -76,6 +77,7 @@ public class RobotContainer {
     // private final VirtualController m_virtualOperator = new VirtualController(1);
 
     private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
+    private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
 
     private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotIntake.talonPIDIn())
@@ -94,6 +96,8 @@ public class RobotContainer {
     // );
 
 
+    // ! Teleop Commands
+
     private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
 
     private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
@@ -124,22 +128,33 @@ public class RobotContainer {
         new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
     );
 
+    private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
+        interrupt,
+        new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
+    );
+
+    private SequentialCommandGroup ampShoot = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.ampPosition()),
+        new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed
+    );
+
     // ! /*  Autos */
     private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand();
     private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
     private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
-    private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
+    
 
     //Help Simplify Shooting
-    private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup(
+    // private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
+    //     new InstantCommand(() -> m_robotIntake.talonPIDIn()),
-        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+    //     new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
-        new WaitCommand(1.4).asProxy(),
+    //     new WaitCommand(1.4).asProxy(),
-        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+    //     new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
-        new WaitCommand(0.5),
+    //     new WaitCommand(0.5),
-        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+    //     new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
-        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
+    //     new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
-    );
+    // );
 
     private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
@@ -170,11 +185,6 @@ public class RobotContainer {
         taxi.asProxy()
     );
 
-    private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
-        interrupt,
-        new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
-        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
-    );
 
     private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
@@ -184,7 +194,13 @@ public class RobotContainer {
         new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
         new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
         new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
-        pullInArmtoShoot,
+        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        new WaitCommand(1.4).asProxy(),
+        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+        new WaitCommand(0.5),
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
         new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
         // new WaitCommand(1).asProxy(),
         // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
@@ -197,6 +213,23 @@ public class RobotContainer {
         // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
     );
 
+    private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        new WaitCommand(1).asProxy(),
+        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+        new WaitCommand(1).asProxy(),
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
+        new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
+        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        new WaitCommand(1.4).asProxy(),
+        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+        new WaitCommand(0.5),
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
+    );
+
     private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup(
         new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
         new WaitCommand(1).asProxy(),
@@ -205,19 +238,32 @@ public class RobotContainer {
         new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
         new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
         new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
-        pullInArmtoShoot,
+        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        new WaitCommand(1.4).asProxy(),
+        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+        new WaitCommand(0.5),
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
         //? Create Another Parallel Command Group :(
-        pullInArmtoShoot,
+        new InstantCommand(() -> m_robotIntake.talonPIDIn()),
+        new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+        new WaitCommand(1.4).asProxy(),
+        new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
+        new WaitCommand(0.5),
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
         new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
     );
 
     private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
             .addOption("Taxi Auto", taxi.asProxy())
-            .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker)
+            .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy())
-            .addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary)
+            .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy())
-            // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
+            // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
-            // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
+            // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
-            .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker)
+            .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
+            .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
             .buildDisplay();
     
     
@@ -228,6 +274,7 @@ public class RobotContainer {
     public RobotContainer() {
         configureButtonBindings();        
        // configureVirtualButtonBindings();
+       new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
 
 
         DriverStation.silenceJoystickConnectionWarning(true);

src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java

@@ -26,6 +26,6 @@ public class ArmIntakeInAuto extends ParallelCommandGroup {
     this.intake = intake;
     this.shooter = shooter;
     this.swerve = swerve;
-    addCommands(new ArmIntakeIn(intake, shooter), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
+    addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
   }
 }

src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java

@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Autos;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class ArmIntakeInTimeout extends Command {
+  /** Creates a new ArmIntakeIn. */
+  private Intake robotIntake;
+  private Shooter robotShooter;
+
+  public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.robotIntake = robotIntake;
+    this.robotShooter = robotShooter;
+
+    addRequirements(this.robotIntake, this.robotShooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    robotIntake.talonPIDOut();
+    robotIntake.talonSpinIntakeMotor();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    if(interrupted) {
+      robotIntake.talonPIDIn();
+      robotIntake.talonStopIntakeMotors();
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return robotIntake.getTalonIntakeLimitSwitchState();
+    // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) 
+    // {
+    //   return !true==true;
+    // } 
+    // else 
+    // {
+    //   return !false==!(!(true));
+    // }
+  }
+}

src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java

@@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase {
     //                              new Translation2d(out.rightX, out.rightY),
     //                              true);
     
-    this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
+    // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
+    //                             new Translation2d(lerpRX, lerpRY),
+    //                             true);
+
+    this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
                                 new Translation2d(lerpRX, lerpRY),
                                 true);
                              

src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java

@@ -64,7 +64,7 @@ public class JoystickRecorder extends CommandBase {
 
     outputs.add(inputs);
 
-    swerve.driveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
+    swerve.playbackDriveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
                           new Translation2d(inputs.rightX, inputs.rightY),
                           true);
     

src/main/java/frc4388/robot/subsystems/Intake.java

@@ -145,6 +145,10 @@ public class Intake extends SubsystemBase {
     talonIntake.set(IntakeConstants.INTAKE_SPEED);
   }
 
+  public void talonSpinIntakeMotor(double speed) {
+    talonIntake.set(speed);
+  }
+
   public boolean getTalonIntakeLimitSwitchState() {
     if(r = talonIntake.getForwardLimit().getValue().value == 0) {
       return true;
@@ -174,6 +178,15 @@ public class Intake extends SubsystemBase {
     }
   }
 
+  public void ampPosition() {
+    PositionVoltage request = new PositionVoltage(-0);
+    talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
+  } 
+
+  public void ampShoot(double speed) {
+    talonSpinIntakeMotor(speed);
+  }
+
 
   // ! NEO Methods
   //hanoff

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -33,7 +33,8 @@ public class  SwerveDrive extends SubsystemBase {
 
   private RobotGyro gyro;
 
-  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
+  public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   
   public double rotTarget = 0.0;
   public Rotation2d orientRotTarget = new Rotation2d();
@@ -87,6 +88,41 @@ public class  SwerveDrive extends SubsystemBase {
     setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
+  public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
+
+      double rot = 0;
+      
+      // ! drift correction
+      if (rightStick.getNorm() > 0.05) {
+        rotTarget = gyro.getAngle();
+        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
+      //  SmartDashboard.putBoolean("drift correction", false);
+        stopped = false;
+      } else if(leftStick.getNorm() > 0.05) {
+        if (!stopped) {
+          stopModules();
+          stopped = true;
+        }
+
+     //   SmartDashboard.putBoolean("drift correction", true);
+        
+        rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+
+      }
+
+      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
+      Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
+      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+
+      // Convert field-relative speeds to robot-relative speeds.
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
+    } else {      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
   public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
 
     Translation2d rightStick = new Translation2d(rightX, rightY);
@@ -157,6 +193,11 @@ public class  SwerveDrive extends SubsystemBase {
     return this.kinematics;
   }
 
+  public boolean getSpeedState() {
+    
+    return false;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run\
@@ -170,6 +211,7 @@ public class  SwerveDrive extends SubsystemBase {
       this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
     } else {
       this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+
     }
   }
 
@@ -220,4 +262,6 @@ public class  SwerveDrive extends SubsystemBase {
     }
   }
 
+  
+
 }

src/main/java/frc4388/utility/RobotGyro.java

@@ -103,6 +103,7 @@ public class RobotGyro implements Gyro {
         } else {
             m_navX.reset();
         }
+
     }
 
     public void resetFlip() {
@@ -113,6 +114,29 @@ public class RobotGyro implements Gyro {
         } else {
             m_navX.reset();
         }
+
+    }
+
+    public void resetNinety() {
+        resetZeroValues();
+        
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(90);
+        } else {
+            m_navX.reset();
+        }
+        
+    }
+
+    public void resetTwoSeventy() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(270);
+        } else {
+            m_navX.reset();
+        }
+
     }
 
     /**