Updated pathplanner and phoenix to the latest beta

Updated drive telemetry and created a new drive subsystem
Simbotics · Jan 4, 2024 · e1cdb52 · e1cdb52
1 parent d44bb89
commit e1cdb52
Show file tree

Hide file tree

Showing 12 changed files with 563 additions and 440 deletions.
diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/CommandSwerveDrivetrain.java
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,8 +4,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.SignalLogger;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
@@ -24,8 +22,6 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
 
     m_robotContainer.drivetrain.getDaqThread().setThreadPriority(99);
-
-    SignalLogger.start();
   }
 
   @Override

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,92 +4,90 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.led.LEDSubsystem;
 
 public class RobotContainer {
-  final double MaxSpeed = 6; // 6 meters per second desired top speed
-  final double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity
+    private double MaxSpeed = 6; // 6 meters per second desired top speed
+    private double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity
 
-  /* Setting up bindings for necessary control of the swerve drive platform */
-  CommandController driver = new CommandController(0); // Driver Controller
-  CommandController operator = new CommandController(1); // Operator Controller
-  CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
-  private final LEDSubsystem ledSubsystem;
-  SwerveRequest.FieldCentric drive =
-      new SwerveRequest.FieldCentric()
-          .withIsOpenLoop(true)
-          .withDeadband(MaxSpeed * 0.1)
-          .withRotationalDeadband(MaxAngularRate * 0.1); // I want field-centric
-  // driving in open loop
-  SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
-  SwerveRequest.RobotCentric forwardStraight =
-      new SwerveRequest.RobotCentric().withIsOpenLoop(true);
-  SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+    /* Setting up bindings for necessary control of the swerve drive platform */
+    private final CommandController driver = new CommandController(0); // Driver Controller
+    private final CommandController operator = new CommandController(1); // Operator Controller
+    final DriveSubsystem drivetrain = DriveConstants.DriveTrain; // My drivetrain
+    private final LEDSubsystem ledSubsystem;
+    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric
+    // driving in open loop
 
-  /* Path follower */
-  // Command runAuto = drivetrain.getAutoPath("Tests");
+    private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+    private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric()
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+    private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
-  Telemetry logger = new Telemetry(MaxSpeed);
+    /* Path follower */
+    private Command runAuto = drivetrain.getAutoPath("Tests");
 
-  Pose2d odomStart = new Pose2d(0, 0, new Rotation2d(0, 0));
+    private final Telemetry logger = new Telemetry(MaxSpeed);
 
-  private void configureBindings() {
-    ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
+    private void configureBindings() {
+        ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
 
-    drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
-        drivetrain
-            .applyRequest(
-                () ->
-                    drive
-                        .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with
-                        // negative Y (forward)
-                        .withVelocityY(
-                            -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left)
-                        .withRotationalRate(
-                            -driver.getRightX()
-                                * MaxAngularRate) // Drive counterclockwise with negative X (left)
-                )
-            .ignoringDisable(true));
+        drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
+                drivetrain
+                        .applyRequest(
+                                () -> drive
+                                        .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with
+                                        // negative Y (forward)
+                                        .withVelocityY(
+                                                -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left)
+                                        .withRotationalRate(
+                                                -driver.getRightX()
+                                                        * MaxAngularRate) // Drive counterclockwise with negative X
+                                                                          // (left)
+                        )
+                        .ignoringDisable(true));
 
-    driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake));
-    driver
-        .yellowButton()
-        .whileTrue(
-            drivetrain.applyRequest(
-                () ->
-                    point.withModuleDirection(
-                        new Rotation2d(-driver.getLeftY(), -driver.getLeftX()))));
+        driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake));
+        driver
+                .yellowButton()
+                .whileTrue(
+                        drivetrain.applyRequest(
+                                () -> point.withModuleDirection(
+                                        new Rotation2d(-driver.getLeftY(), -driver.getLeftX()))));
 
-    // if (Utils.isSimulation()) {
-    //   drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
-    // }
-    drivetrain.registerTelemetry(logger::telemeterize);
-    driver.POVUp()
-        .whileTrue(
-            drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
-    driver.POVDown()
-        .whileTrue(
-            drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
-  }
+        // if (Utils.isSimulation()) {
+        // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(),
+        // Rotation2d.fromDegrees(90)));
+        // }
+        drivetrain.registerTelemetry(logger::telemeterize);
+        driver.POVUp()
+                .whileTrue(
+                        drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
+        driver.POVDown()
+                .whileTrue(
+                        drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
+    }
 
-  public RobotContainer() {
-    configureBindings();
-    ledSubsystem = new LEDSubsystem();
-  }
+    public RobotContainer() {
+        configureBindings();
+        ledSubsystem = new LEDSubsystem();
+    }
 
-  public Command getAutonomousCommand() {
-    /* First put the drivetrain into auto run mode, then run the auto */
-    return new InstantCommand(() -> {});
-  }
+    public Command getAutonomousCommand() {
+        /* First put the drivetrain into auto run mode, then run the auto */
+        return runAuto;
+    }
 
-  public boolean seedPoseButtonDown() {
-    return driver.leftBumper().getAsBoolean();
-  }
+    public boolean seedPoseButtonDown() {
+        return driver.leftBumper().getAsBoolean();
+    }
 }