Retry for SE2

2024-12-18 02:06:22 +00:00 · 2024-12-18 02:06:22 +00:00 · 31ef6bb688
commit 31ef6bb688
parent 496891cce4
5 changed files with 139 additions and 109 deletions
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@ -19,109 +19,111 @@ import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;

 public class RobotContainer {
-  private Drivetrain drivetrain;
-  private Indexer indexer;
-  private Intake intake;
-  private Shooter shooter;
+    private Drivetrain drivetrain;
+    private Indexer indexer;
+    private Intake intake;
+    private Shooter shooter;

-  private CommandXboxController driver;
+    private CommandXboxController driver;

-  //constructor//
-  public RobotContainer() {
-    drivetrain = new Drivetrain();
-    indexer = new Indexer();
-    intake = new Intake();
-    shooter = new Shooter();
+    //constructor//
+    public RobotContainer() {
+        drivetrain = new Drivetrain();
+        indexer = new Indexer();
+        intake = new Intake();
+        shooter = new Shooter();

-    driver = new CommandXboxController(OIConstants.kDriverUSB);
+        driver = new CommandXboxController(OIConstants.kDriverUSB);

-    configureBindings();
-    configureShuffleboard();
-  }
+        configureBindings();
+        configureShuffleboard();
+    }

-  private void configureBindings() {
-    //default commands//
-    drivetrain.setDefaultCommand(
-      drivetrain.driveArcade(
-        null, 
-        null
-      )
-    );
+    private void configureBindings() {
+        //default commands//
+        drivetrain.setDefaultCommand(
+            drivetrain.driveArcade(
+                driver::getLeftX, 
+                driver::getLeftY
+            )
+        );

-    indexer.setDefaultCommand(indexer.stopIndexer());
-    intake.setDefaultCommand(intake.stopIntake());
-    shooter.setDefaultCommand(shooter.stopShooter());
+        indexer.setDefaultCommand(indexer.stopIndexer());
+        intake.setDefaultCommand(intake.stopIntake());
+        shooter.setDefaultCommand(shooter.runShooter(driver::getRightTriggerAxis));

-    //driver inputs//
-    driver.rightTrigger().whileTrue(
-      shooter.runShooter(driver.getRightTriggerAxis())
-    );
-    
-    driver.a().whileTrue(
-      intake.runIntake(1)
-      .alongWith(indexer.runIndexer(1))
-    );
+        //driver inputs//
+        driver.a().whileTrue(
+            intake.runIntake(1)
+                .alongWith(indexer.runIndexer(() -> 1))
+        );

-    driver.b().whileTrue(
-      intake.runIntake(-1)
-      .alongWith(indexer.runIndexer(-1))
-    );
+        driver.b().whileTrue(
+            intake.runIntake(-1)
+                .alongWith(indexer.runIndexer(() -> -1))
+        );

-    driver.x().onTrue(smartShooter());
-  }
+        driver.x().onTrue(smartShooter());
+    }

-  //configures shuffleboard//
-  private void configureShuffleboard() {
-    ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName);
+    //configures shuffleboard//
+    private void configureShuffleboard() {
+        ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName);

-    robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoder)
-      .withPosition(0, 0)
-      .withSize(2, 1)
-      .withWidget(BuiltInWidgets.kTextView);
+        robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoderPos)
+            .withPosition(0, 0)
+            .withSize(2, 1)
+            .withWidget(BuiltInWidgets.kTextView);

-      robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoder)
-      .withPosition(2, 0)
-      .withSize(2, 1)
-      .withWidget(BuiltInWidgets.kTextView);
+        robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoderPos)
+            .withPosition(2, 0)
+            .withSize(2, 1)
+            .withWidget(BuiltInWidgets.kTextView);

-      robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle)
-      .withPosition(0, 1)
-      .withSize(2, 1)
-      .withWidget(BuiltInWidgets.kGyro);
+        robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle)
+            .withPosition(0, 1)
+            .withSize(2, 1)
+            .withWidget(BuiltInWidgets.kGyro);

-      robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoder)
-      .withPosition(4, 0)
-      .withSize(2, 2)
-      .withWidget(BuiltInWidgets.kDial)
-      .withProperties(Map.of(
-        "Max", ShooterConstants.kShooterFireSpeed)
-      );
+        robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoderRate)
+            .withPosition(4, 0)
+            .withSize(2, 2)
+            .withWidget(BuiltInWidgets.kDial)
+            .withProperties(Map.of(
+                "Max", ShooterConstants.kShooterFireSpeed)
+            );

-      robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoder)
-      .withPosition(6, 0)
-      .withSize(2, 2)
-      .withWidget(BuiltInWidgets.kDial)
-      .withProperties(Map.of(
-        "Max", ShooterConstants.kShooterFireSpeed)
-      );
-  }
+        robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoderRate)
+            .withPosition(6, 0)
+            .withSize(2, 2)
+            .withWidget(BuiltInWidgets.kDial)
+            .withProperties(Map.of(
+                "Max", ShooterConstants.kShooterFireSpeed)
+            );
+    }

-  public Command getAutonomousCommand() {
-    return drivetrain.moveDistance(1.5, 5)
-    .andThen(drivetrain.goToAngle(45, 2))
-    .andThen(shooter.runAutoShooter())
-      .until(shooter::shooterAtSpeed)
-    .andThen(shooter.runAutoShooter())
-      .alongWith(indexer.runIndexer(1))
-        .withTimeout(2);
-  }
+    //moves, rotates, runs auto shooter//
+    public Command getAutonomousCommand() {
+        drivetrain.resetGyro();
+        drivetrain.resetDriveEncoders();

-  //smart commands//
-  private Command smartShooter() {
-    return shooter.runAutoShooter()
-      .until(shooter::shooterAtSpeed)
-    .andThen(shooter.runAutoShooter())
-      .alongWith(indexer.runIndexer(1))
-        .withTimeout(2);
-  }
+        return drivetrain.moveDistance(1.5, 5)
+            .andThen(
+                drivetrain.goToAngle(
+                    45,
+                    2
+                ), 
+                smartShooter()
+            );
+    }
+
+    //smart commands//
+    private Command smartShooter() {
+        return shooter.runAutoShooter()
+            .until(shooter::shooterAtSpeed)
+                .andThen(
+                    shooter.runAutoShooter(), 
+                    indexer.runIndexer(() -> 1)
+                ).withTimeout(2);
+    }
 }
--- a/src/main/java/frc/robot/constants/ShooterConstants.java
+++ b/src/main/java/frc/robot/constants/ShooterConstants.java
@ -25,4 +25,5 @@ public class ShooterConstants {
    public static final int kFFG = 0;
    public static final int kFFV = 0;

+    public static final int kBangBangScale = 8;
 }
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.DrivetrainConstants;

 public class Drivetrain extends SubsystemBase {
-    
    //creates instance variables//
    private CANSparkMax leftFront;
    private CANSparkMax leftRear;
@ -27,12 +26,12 @@ public class Drivetrain extends SubsystemBase {
    private ADXRS450_Gyro gyro;

    private PIDController turnController;
-    private PIDController distanceController;
+    private PIDController rightDistanceController;
+    private PIDController leftDistanceController;
    
    private DifferentialDrive drive;

    public Drivetrain() {
-
        //creates instances of each motor//
        leftFront = new CANSparkMax(DrivetrainConstants.kLeftFrontCAN, MotorType.kBrushless);
        leftRear = new CANSparkMax(DrivetrainConstants.kLeftRearCAN, MotorType.kBrushless);
@ -57,28 +56,47 @@ public class Drivetrain extends SubsystemBase {
        );
        turnController.setTolerance(DrivetrainConstants.kTurnControllerTolerance);

-        distanceController = new PIDController(
+        rightDistanceController = new PIDController(
            DrivetrainConstants.kDistanceControllerP, 
            DrivetrainConstants.kDistanceControllerI, 
            DrivetrainConstants.kDistanceControllerD
        );
-        distanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance);
+        rightDistanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance);
+
+        leftDistanceController = new PIDController(
+            DrivetrainConstants.kDistanceControllerP,
+            DrivetrainConstants.kDistanceControllerI,
+            DrivetrainConstants.kDistanceControllerD
+        );
+        leftDistanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance);

        drive = new DifferentialDrive(leftFront, rightFront);
    }

-    //getters/
+    //gyro methods//
    public double getGyroAngle() {
        return gyro.getAngle() * (DrivetrainConstants.kInvertGyro ? -1 : 1);
    }

-    public double getLeftEncoder() {
+    public void resetGyro() {
+        gyro.reset();
+        return;
+    }
+
+    //encoder methods//
+    public double getLeftEncoderPos() {
        return leftFrontEncoder.getPosition();
    }

-    public double getRightEncoder() {
+    public double getRightEncoderPos() {
        return rightFrontEncoder.getPosition();
    }
+
+    public void resetDriveEncoders() {
+        leftFrontEncoder.setPosition(0);
+        rightFrontEncoder.setPosition(0);
+        return;
+    }
    
    //method to turn drivetrain a certain angle//
    public Command goToAngle(double angle, double timeout) {
@ -94,12 +112,16 @@ public class Drivetrain extends SubsystemBase {
    //method to move drivetrain a certain distance//
    public Command moveDistance(double distance, double timeout) {
        return run(() -> {
-            double output = distanceController.calculate(
+            double outputL = rightDistanceController.calculate(
                (leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2, 
                distance
            );
-            drive.arcadeDrive(output, 0.0);
-        }).until(distanceController::atSetpoint).withTimeout(timeout);
+            double outputR = rightDistanceController.calculate(
+                (leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2, 
+                distance
+            );
+            drive.tankDrive(outputL, outputR);
+        }).until(rightDistanceController::atSetpoint).withTimeout(timeout);
    }

    //methods that take 2 doubles and convert them to motor commands//
--- a/src/main/java/frc/robot/subsystems/Indexer.java
+++ b/src/main/java/frc/robot/subsystems/Indexer.java
@ -1,5 +1,7 @@
 package frc.robot.subsystems;

+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;

 import edu.wpi.first.wpilibj2.command.Command;
@ -15,9 +17,9 @@ public class Indexer extends SubsystemBase {
    }

    //indexer commands//
-    public Command runIndexer(double speed) {
+    public Command runIndexer(DoubleSupplier speed) {
        return run(() -> {
-            motor1.set(speed);
+            motor1.set(speed.getAsDouble());
        });
    }

--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@ -1,5 +1,7 @@
 package frc.robot.subsystems;

+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;

 import edu.wpi.first.math.controller.BangBangController;
@ -14,6 +16,7 @@ public class Shooter extends SubsystemBase {
    private WPI_VictorSPX rearMotor;

    private BangBangController bangBangController;
+
    private SimpleMotorFeedforward motorFeedforward;

    private Encoder frontMotorEncoder;
@ -50,33 +53,33 @@ public class Shooter extends SubsystemBase {
    }

    //getters//
-    public double getFrontEncoder() {
+    public double getFrontEncoderRate() {
        return frontMotorEncoder.getRate();
    }

-    public double getRearEncoder() {
+    public double getRearEncoderRate() {
        return rearMotorEncoder.getRate();
    }

    public boolean shooterAtSpeed() {
-        return (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2 >= ShooterConstants.kShooterFireSpeed;
+        return bangBangController.atSetpoint();
    }

-    //shooter commands//
+    //shooter run/stop commands//
    public Command runAutoShooter() {
        return run(() -> {
-            double output = bangBangController.calculate(
+            double output = (bangBangController.calculate(
                (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2,
-                ShooterConstants.kShooterFireSpeed
+                ShooterConstants.kShooterFireSpeed) * ShooterConstants.kBangBangScale
            ) + motorFeedforward.calculate(ShooterConstants.kShooterFireSpeed);

            frontMotor.setVoltage(output);
        });
    }

-    public Command runShooter(double speed) {
+    public Command runShooter(DoubleSupplier speed) {
        return run(() -> {
-            frontMotor.set(speed);
+            frontMotor.set(speed.getAsDouble());
        });
    }