Sotm

src/main/java/frc/robot/RobotContainer.java

@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 
 import frc.lib.SpikeController;
-import frc.robot.commands.EmptyHopper;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
 import frc.robot.subsystems.findexer.Findexer;
@@ -41,6 +40,12 @@ public class RobotContainer {
     private final SwerveRequest.FieldCentric driveCmd = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
+
+    private final SwerveRequest.FieldCentricFacingAngle snapCmd = new SwerveRequest.FieldCentricFacingAngle()
+            .withDeadband(MaxSpeed * 0.1)
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage)
+            .withHeadingPID(6, 0, 0.2);
+
     private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
     private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
@@ -52,10 +57,10 @@ public class RobotContainer {
     public static CommandSwerveDrivetrain drive;
     private final Vision vision;
     private final Turret turret;
-     private final Intake intake;
+    private final Intake intake;
     private final Trigger trigger;
     private final Shooter shooter;
-     private final Targeting targeting;
+    private final Targeting targeting;
     private final Findexer findexer;
 
     public RobotContainer() {
@@ -72,8 +77,6 @@ public RobotContainer() {
         SmartDashboard.putData("Auto Path", autoChooser);
 
         configureBindings();
-
-        NamedCommands.registerCommand("emptyHopper", new EmptyHopper(shooter, targeting, 15));
     }
 
     public static CommandSwerveDrivetrain getDrive() {
@@ -91,24 +94,55 @@ private void setupSwerveBindings() {
         // Note that X is defined as forward according to WPILib convention,
         // and Y is defined as to the left according to WPILib convention.
         drive.setDefaultCommand(
-                // Drivetrain will execute this command periodically
-                drive.applyRequest(() -> driveCmd.withVelocityX(-driverController.getLeftY() * MaxSpeed) // Drive
-                                                                                                         // forward with
-                                                                                                         // negative Y
-                                                                                                         // (forward)
-                        .withVelocityY(-driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left)
-                        .withRotationalRate(-driverController.getRightX() * MaxAngularRate) // Drive counterclockwise
-                                                                                           // with negative X (left)
-                ));
-
+            drive.applyRequest(() -> {
+
+                double speedMultiplier = driverController.rightBumper().getAsBoolean() ? 0.3 : 1.0;
+
+                double vx = -driverController.getLeftY() * MaxSpeed * speedMultiplier;
+                double vy = -driverController.getLeftX() * MaxSpeed * speedMultiplier;
+
+                // Snap angles 
+                if (driverController.y().getAsBoolean()) { // Up
+                    return snapCmd
+                        .withVelocityX(vx)
+                        .withVelocityY(vy)
+                        .withTargetDirection(Rotation2d.fromDegrees(0));
+                } 
+                else if (driverController.b().getAsBoolean()) { // Right
+                    return snapCmd
+                        .withVelocityX(vx)
+                        .withVelocityY(vy)
+                        .withTargetDirection(Rotation2d.fromDegrees(270));
+                } 
+                else if (driverController.a().getAsBoolean()) { // Down
+                    return snapCmd
+                        .withVelocityX(vx)
+                        .withVelocityY(vy)
+                        .withTargetDirection(Rotation2d.fromDegrees(180));
+                } 
+                else if (driverController.x().getAsBoolean()) { // Left
+                    return snapCmd
+                        .withVelocityX(vx)
+                        .withVelocityY(vy)
+                        .withTargetDirection(Rotation2d.fromDegrees(90));
+                }
+
+                double angularMultiplier = driverController.rightBumper().getAsBoolean() ? 0.3 : 1.0;
+
+                return driveCmd
+                    .withVelocityX(vx)
+                    .withVelocityY(vy)
+                    .withRotationalRate(-driverController.getRightX() * MaxAngularRate * angularMultiplier);
+            })
+        );
         // Idle while the robot is disabled. This ensures the configured
         // neutral mode is applied to the drive motors while disabled.
         final var idle = new SwerveRequest.Idle();
         RobotModeTriggers.disabled().whileTrue(
                 drive.applyRequest(() -> idle).ignoringDisable(true));
 
-        driverController.a().whileTrue(drive.applyRequest(() -> brake));
-        driverController.b().whileTrue(drive.applyRequest(() -> point
+        driverController.leftTrigger().whileTrue(drive.applyRequest(() -> brake));
+        driverController.rightTrigger().whileTrue(drive.applyRequest(() -> point
                 .withModuleDirection(new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX()))));
 
         // Run SysId routines when holding back/start and X/Y.
@@ -133,9 +167,8 @@ private void setupSwerveBindings() {
 
     private void setupIntakeBindings() {
         // toggle intake on A press
-        operatorController.rightBumper().onTrue(intake.runOnce(intake::toggleIntake));
-
-        operatorController.a().onTrue(intake.runOnce(intake::switchDirection));
+        driverController.leftBumper().onTrue(intake.runOnce(intake::toggle));
+        driverController.povUp().onTrue(intake.runOnce(intake::switchDirection));
     }
 
     private void setupTargetingBindings() {
@@ -146,10 +179,10 @@ private void setupTargetingBindings() {
 
     private void setupShooterBindings() {
         // toggle shooter on right trigger hold
-        driverController.rightTrigger()
+        operatorController.rightTrigger()
                 .onTrue(shooter.runOnce(() -> shooter.setDriverRequestingShooting(true)))
                 .onFalse(shooter.runOnce(() -> shooter.setDriverRequestingShooting(false)));
-        driverController.leftTrigger()
+        operatorController.leftTrigger()
                 .onTrue(shooter.runOnce(() -> shooter.setRequestingWithForce(true)))
                 .onFalse(shooter.runOnce(() -> shooter.setRequestingWithForce(false)));
 
@@ -162,8 +195,8 @@ private void setupShooterBindings() {
 
         operatorController.povUp().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(0.1)));
         operatorController.povDown().onTrue(shooter.runOnce(() -> shooter.changeDistanceTrim(-0.1)));
-        operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(2)));
-        operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-2)));
+        operatorController.povLeft().onTrue(turret.runOnce(() -> turret.changeTrim(1)));
+        operatorController.povRight().onTrue(turret.runOnce(() -> turret.changeTrim(-1)));
     }
 
     public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/findexer/Findexer.java

@@ -4,7 +4,8 @@
 import frc.robot.subsystems.trigger.Trigger;
 
 public class Findexer extends SpikeSystem<FindexerIO.FindexerIOInputs> {
-    private static final double FEEDING_RPS = -30.0 * 9; // feeding velocity in rotations per second
+    // multiplied by the gear ratio of the findexer (36:1)
+    private static final double FEEDING_RPS = -3*36; // feeding velocity in rotations per second
 
     private final Trigger trigger;
 

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -91,6 +91,10 @@ private void doDeployedState() {
         // }
 
         // Update the intakeIO on speed
+        if (forward == false) {
+            intakeIO.setIntakeSpeed(-70);
+            return;
+        }
         intakeIO.setIntakeSpeed(70);
     }
 
@@ -128,6 +132,10 @@ public void switchDirection() {
         forward = !forward;
     }
 
+    public void toggle() {
+        running = !running;
+    }
+
     // Disables the intake
     public void disable() {
         running = false;

src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java

@@ -70,6 +70,11 @@ public void updateInputs(IntakeIOInputs inputs) {
     // double speed - Speed to set the motor to in Rotations Per Second
     @Override
     public void setIntakeSpeed(double speed) {
+        if (speed == 0) {
+            intakeMotor.stopMotor();
+            return;
+        }
+
         // intakeMotor.set(speed);
         this.velocityControl.withVelocity(speed);
         intakeMotor.setControl(this.velocityControl);

src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java

@@ -93,10 +93,10 @@ public ShooterIOTalonFX() {
 
         // flywheel configs (units in AMPS)
         var flywheelSlot0 = new Slot0Configs();
-        flywheelSlot0.kP = 20; // amps / rps of error
+        flywheelSlot0.kP = 14; // amps / rps of error
         flywheelSlot0.kI = 0.0;
         flywheelSlot0.kD = 0.0;
-        flywheelSlot0.kS = 0.0; // amps needed to overcome static friction
+        flywheelSlot0.kS = 15.0; // amps needed to overcome static friction
         flywheelSlot0.kV = 0.0; // not used for torque control
 
         this.flywheelMotor.getConfigurator().apply(flywheelSlot0);
@@ -174,16 +174,13 @@ public void updateInputs(ShooterIOInputs inputs) {
     public void setFlywheelVelocity(double rps) {
         this.flywheelRPSSetPoint = rps;
 
-        // boolean firstCommand = Double.isNaN(lastAppliedFlywheelRPSSetPoint);
-        // boolean meaningfulChange = firstCommand
-        //         || (Math.abs(rps - lastAppliedFlywheelRPSSetPoint) >= FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS);
-
-        // if (!meaningfulChange) {
-        //     return;
-        // }
+        if (rps == 0) {
+            flywheelMotor.stopMotor();
+        } else {
+            this.flywheelControl.withVelocity(rps);
+            flywheelMotor.setControl(this.flywheelControl);
+        }
 
-        this.flywheelControl.withVelocity(rps);
-        flywheelMotor.setControl(this.flywheelControl);
         this.lastAppliedFlywheelRPSSetPoint = rps;
     }
     /**

src/main/java/frc/robot/subsystems/targeting/ShotData.java

@@ -5,39 +5,27 @@
 public class ShotData {
     public static final InterpolatingDoubleTreeMap distanceToRPM = new InterpolatingDoubleTreeMap();
     public static final InterpolatingDoubleTreeMap distanceToHoodAngle = new InterpolatingDoubleTreeMap();
-
+    public static final InterpolatingDoubleTreeMap distanceToTOFConstant = new InterpolatingDoubleTreeMap(); //TOF = Time of Flight
+
     static {
-        distanceToRPM.put(3.14, 2200.0);
-        distanceToHoodAngle.put(3.14, 30.0);
-
-        distanceToRPM.put(4.7, 2550.0);
-        distanceToHoodAngle.put(4.7, 36.0);
-
-        distanceToRPM.put(1.5, 1900.0);
-        distanceToHoodAngle.put(1.5, 15.0);
-
-        distanceToRPM.put(2.5, 2300.0);
-        distanceToHoodAngle.put(2.5, 25.0);
-
-        // distanceToRPM.put(2.96, 2300.0);
-        // distanceToHoodAngle.put(2.96, 30.0);
-
-        // distanceToRPM.put(2.7, 1900.0);
-        // distanceToHoodAngle.put(2.7, 30.0);
+        distanceToRPM.put(1.0, 1800.0);
+        distanceToHoodAngle.put(1.0, 15.0);
+        distanceToTOFConstant.put(1.0, 0.8);
+        distanceToTOFConstant.put(4.0, 0.9  );
 
-        // distanceToRPM.put(3.0, 2000.0);
-        // distanceToHoodAngle.put(3.0, 30.0);
+        distanceToRPM.put(2.0, 2050.0);
+        distanceToHoodAngle.put(2.0, 23.0);
 
-        // distanceToRPM.put(3.6, 2100.0);
-        // distanceToHoodAngle.put(3.6, 30.0);
+        distanceToRPM.put(3.0, 2200.0);
+        distanceToHoodAngle.put(3.0, 30.0);
 
-        // distanceToRPM.put(4.0, 2250.0);
-        // distanceToHoodAngle.put(4.0, 30.0);
+        distanceToRPM.put(4.0, 2500.0);
+        distanceToHoodAngle.put(4.0, 32.0);
 
-        // distanceToRPM.put(5.5, 2400.0);
-        // distanceToHoodAngle.put(5.5, 40.0);
+        distanceToRPM.put(5.3, 2650.0);
+        distanceToHoodAngle.put(5.3, 35.0);
 
-        distanceToRPM.put(7.5, 3100.0);
-        distanceToHoodAngle.put(7.5, 40.0);
+        distanceToRPM.put(17.069, 5000.0);
+        distanceToHoodAngle.put(17.069, 45.0);
     }
 }