Adding MecanumDrivetrain and some other bits and bobs

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

@@ -4,15 +4,41 @@
 
 package frc.robot;
 
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.constants.OIConstants;
+import frc.robot.interfaces.IDrivetrain;
+import frc.robot.subsystems.DifferentialDrivetrain;
 
 public class RobotContainer {
+  // TODO Not sure if the polymorphism will work correctly here when logging
+  @Logged(name = "Drivetrain")
+  private IDrivetrain drivetrain;
+
+  private CommandXboxController driver;
+
   public RobotContainer() {
+    drivetrain = new DifferentialDrivetrain(
+      new Pose2d(), 
+      true, 
+      false
+    );
+
+    driver = new CommandXboxController(OIConstants.kDriverUSB);
+
     configureBindings();
   }
 
-  private void configureBindings() {}
+  private void configureBindings() {
+    if (drivetrain instanceof DifferentialDrivetrain) {
+      ((DifferentialDrivetrain) drivetrain).setDefaultCommand(
+        drivetrain.drive(driver)
+      );
+    }
+  }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");

src/main/java/frc/robot/constants/MecanumDriveConstants.java

@@ -0,0 +1,52 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+
+public class MecanumDriveConstants {
+    public static final int kFrontLeftPWM = 0;
+    public static final int kRearLeftPWM = 1;
+    public static final int kFrontRightPWM = 2;
+    public static final int kRearRightPWM = 3;
+
+    // TODO Are these the right values
+    public static final int kFrontLeftEncoderChannelA = 0;
+    public static final int kFrontLeftEncoderChannelB = 1;
+    public static final int kRearLeftEncoderChannelA = 2;
+    public static final int kRearLeftEncoderChannelB = 3;
+    public static final int kFrontRightEncoderChannelA = 4;
+    public static final int kFrontRightEncoderChannelB = 5;
+    public static final int kRearRightEncoderChannelA = 6;
+    public static final int kRearRightEncoderChannelB = 7;
+
+    // TODO Set this to a real value
+    public static final double kDistancePerPulseMeters = 0;
+
+    public static final double kSlowModeMultiplier = .75;
+
+    // TODO Set these to a real value
+    public static final Translation2d kFrontLeftTranslation = 
+        new Translation2d(0, 0);
+    public static final Translation2d kRearLeftTranslation =
+        new Translation2d(0, 0);
+    public static final Translation2d kFrontRightTranslation =
+        new Translation2d(0, 0);
+    public static final Translation2d kRearRightTranslation =
+        new Translation2d(0, 0);
+
+    public static final double kMaxSpeedMetersPerSecond = 0;
+    public static final double kMaxAngularSpeedRadiansPerSecond = 0;
+
+    public static final boolean kInvertForwardBackward = true;
+    public static final boolean kInvertLeftRight = true;
+    public static final boolean kInvertRotation = true;
+    public static final boolean kInvertGyro = true;
+    public static final boolean kInvertEncoders = false;
+
+    public static final MecanumDriveKinematics kKinematics = new MecanumDriveKinematics(
+        kFrontLeftTranslation, 
+        kFrontRightTranslation, 
+        kRearLeftTranslation, 
+        kRearRightTranslation
+    );
+}

src/main/java/frc/robot/constants/OIConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class OIConstants {
+    public static final int kDriverUSB = 0;
+}

src/main/java/frc/robot/subsystems/DifferentialDrivetrain.java

@@ -36,6 +36,9 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
     private boolean slowMode;
     private boolean fieldOriented;
 
+    @Logged(name = "Drivebase Type", importance = Importance.INFO)
+    public static final String kDrivebaseType = "Differential";
+
     public DifferentialDrivetrain(Pose2d initialPose, boolean slowModeAtStartup, boolean fieldOrientedAtStartup) {
         super();
 
@@ -96,7 +99,7 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
     }
 
     @Override
-    @Logged(name = "DD Pose", importance = Importance.INFO)
+    @Logged(name = "Pose", importance = Importance.INFO)
     public Pose2d getPose() {
         return poseEstimator.getEstimatedPosition();
     }
@@ -188,27 +191,27 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
         });
     }
 
-    @Logged(name = "DD Gyro Angle", importance = Importance.DEBUG)
+    @Logged(name = "Gyro Angle Degrees", importance = Importance.DEBUG)
     public double getGyroAngle() {
         return gyro.getAngle() * (DifferentialDriveConstants.kInvertGyro ? -1 : 1);
     }
 
-    @Logged(name = "DD Left Encoder Meters", importance = Importance.DEBUG)
+    @Logged(name = "Left Encoder Meters", importance = Importance.DEBUG)
     public double getLeftEncoderMeters() {
         return leftEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }
 
-    @Logged(name = "DD Right Encoder Meters", importance = Importance.DEBUG)
+    @Logged(name = "Right Encoder Meters", importance = Importance.DEBUG)
     public double getRightEncoderMeters() {
         return rightEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }
 
-    @Logged(name = "DD Left Encoder Meters Per Second", importance = Importance.DEBUG)
+    @Logged(name = "Left Encoder Meters Per Second", importance = Importance.DEBUG)
     public double getLeftEncoderMetersPerSecond() {
         return leftEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }
 
-    @Logged(name = "DD Right Encoder Meters Per Second", importance = Importance.DEBUG)
+    @Logged(name = "Right Encoder Meters Per Second", importance = Importance.DEBUG)
     public double getRightEncoderMetersPerSecond() {
         return rightEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
     }

src/main/java/frc/robot/subsystems/MecanumDrivetrain.java

@@ -0,0 +1,266 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.epilogue.Logged.Importance;
+import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.constants.MecanumDriveConstants;
+import frc.robot.interfaces.IDrivetrain;
+
+public class MecanumDrivetrain extends SubsystemBase implements IDrivetrain {
+    private VictorSP frontLeft;
+    private VictorSP rearLeft;
+    private VictorSP frontRight;
+    private VictorSP rearRight;
+
+    private MecanumDrive drive;
+
+    private Encoder frontLeftEncoder;
+    private Encoder rearLeftEncoder;
+    private Encoder frontRightEncoder;
+    private Encoder rearRightEncoder;
+
+    private ADXRS450_Gyro gyro;
+
+    private MecanumDrivePoseEstimator poseEstimator;
+
+    private boolean slowMode;
+    private boolean fieldOriented;
+
+    @Logged(name = "Drivebase Type", importance = Importance.INFO)
+    public static final String kDrivebaseType = "Mecanum";
+
+    public MecanumDrivetrain(Pose2d initialPose, boolean slowModeAtStartup, boolean fieldOrientedAtStartup) {
+        super();
+
+        frontLeft = new VictorSP(MecanumDriveConstants.kFrontLeftPWM);
+        rearLeft = new VictorSP(MecanumDriveConstants.kRearLeftPWM);
+        frontRight = new VictorSP(MecanumDriveConstants.kFrontRightPWM);
+        rearRight = new VictorSP(MecanumDriveConstants.kRearRightPWM);
+
+        drive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+        frontLeftEncoder = new Encoder(
+            MecanumDriveConstants.kFrontLeftEncoderChannelA, 
+            MecanumDriveConstants.kFrontLeftEncoderChannelB
+        );
+        frontLeftEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
+
+        rearLeftEncoder = new Encoder(
+            MecanumDriveConstants.kRearLeftEncoderChannelA, 
+            MecanumDriveConstants.kRearLeftEncoderChannelB
+        );
+        rearLeftEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
+
+        frontRightEncoder = new Encoder(
+            MecanumDriveConstants.kFrontRightEncoderChannelA, 
+            MecanumDriveConstants.kFrontRightEncoderChannelB
+        );
+        frontRightEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
+
+        rearRightEncoder = new Encoder(
+            MecanumDriveConstants.kRearRightEncoderChannelA,
+            MecanumDriveConstants.kRearRightEncoderChannelB
+        );
+        rearRightEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
+
+        gyro = new ADXRS450_Gyro();
+
+        poseEstimator = new MecanumDrivePoseEstimator(
+            MecanumDriveConstants.kKinematics, 
+            Rotation2d.fromDegrees(getGyroAngle()), 
+            new MecanumDriveWheelPositions(
+                getFrontLeftEncoderMeters(), 
+                getFrontRightEncoderMeters(), 
+                getRearLeftEncoderMeters(), 
+                getRearRightEncoderMeters()
+            ), 
+            initialPose
+        );
+
+        slowMode = slowModeAtStartup;
+        fieldOriented = fieldOrientedAtStartup;
+    }
+
+    @Override
+    public void periodic() {
+        poseEstimator.update(
+            Rotation2d.fromDegrees(getGyroAngle()), 
+            new MecanumDriveWheelPositions(
+                getFrontLeftEncoderMeters(), 
+                getFrontRightEncoderMeters(), 
+                getRearLeftEncoderMeters(), 
+                getRearRightEncoderMeters()
+            )
+        );
+    }
+
+    @Override
+    public void resetPose(Pose2d pose) {
+        poseEstimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroAngle()), 
+            new MecanumDriveWheelPositions(
+                getFrontLeftEncoderMeters(), 
+                getFrontRightEncoderMeters(), 
+                getRearLeftEncoderMeters(), 
+                getRearRightEncoderMeters()
+            ), 
+            pose
+        );
+    }
+
+    @Override
+    @Logged(name = "Pose", importance = Importance.INFO)
+    public Pose2d getPose() {
+        return poseEstimator.getEstimatedPosition();
+    }
+
+    @Override
+    public boolean isSlowMode() {
+        return slowMode;
+    }
+
+    @Override
+    public boolean isFieldOriented() {
+        return fieldOriented;
+    }
+
+    @Override
+    public ChassisSpeeds getChassisSpeeds() {
+        return MecanumDriveConstants.kKinematics.toChassisSpeeds(
+            new MecanumDriveWheelSpeeds(
+                getFrontLeftEncoderMetersPerSecond(), 
+                getFrontRightEncoderMetersPerSecond(), 
+                getRearLeftEncoderMetersPerSecond(), 
+                getFrontRightEncoderMetersPerSecond()
+            )
+        );
+    }
+
+    @Override
+    public Command toggleSlowMode() {
+        return runOnce(() -> {
+            slowMode = !slowMode;
+        });
+    }
+
+    @Override
+    public Command toggleFieldOriented() {
+        return runOnce(() -> {
+            fieldOriented = !fieldOriented;
+        });
+    }
+
+    @Override
+    public Command drive(CommandXboxController controller) {
+        return run(() -> {
+            double xSpeedDelivered = 
+                controller.getLeftY()
+                * (slowMode ? MecanumDriveConstants.kSlowModeMultiplier : 1)
+                * (MecanumDriveConstants.kInvertForwardBackward ? -1 : 1)
+                * MecanumDriveConstants.kMaxSpeedMetersPerSecond;
+
+            double ySpeedDelivered = 
+                controller.getLeftX()
+                * (slowMode ? MecanumDriveConstants.kSlowModeMultiplier : 1)
+                * (MecanumDriveConstants.kInvertLeftRight ? -1 : 1)
+                * MecanumDriveConstants.kMaxSpeedMetersPerSecond;
+
+            double rotDelivered =
+                controller.getRightX()
+                * (slowMode ? MecanumDriveConstants.kSlowModeMultiplier : 1)
+                * (MecanumDriveConstants.kInvertRotation ? -1 : 1)
+                * MecanumDriveConstants.kMaxAngularSpeedRadiansPerSecond;
+
+            ChassisSpeeds speeds = fieldOriented ?
+                ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle())) :
+                new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered);
+
+            drive.driveCartesian(
+                speeds.vxMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond, 
+                speeds.vyMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond, 
+                speeds.omegaRadiansPerSecond / MecanumDriveConstants.kMaxAngularSpeedRadiansPerSecond
+            );
+        });
+    }
+
+    @Override
+    public Command drive(ChassisSpeeds speeds) {
+        return run(() -> {
+            drive.driveCartesian(
+                speeds.vxMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond, 
+                speeds.vyMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond, 
+                speeds.omegaRadiansPerSecond / MecanumDriveConstants.kMaxAngularSpeedRadiansPerSecond
+            );
+        });
+    }
+
+    @Override
+    public Command drive(Pose2d targetPose) {
+        return new PrintCommand("Mecanum Drivetrain Drive to Pose Not Implemented");
+    }
+
+    @Override
+    public Command stop() {
+        return run(() -> {
+            drive.driveCartesian(0, 0, 0);
+        });
+    }
+
+    @Logged(name = "Gyro Angle Degrees", importance = Importance.DEBUG)
+    public double getGyroAngle() {
+        return gyro.getAngle() * (MecanumDriveConstants.kInvertGyro ? -1 : 1);
+    }
+
+    @Logged(name = "Front Left Encoder Meters", importance = Importance.DEBUG)
+    public double getFrontLeftEncoderMeters() {
+        return frontLeftEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    @Logged(name = "Rear Left Encoder Meters", importance = Importance.DEBUG)
+    public double getRearLeftEncoderMeters() {
+        return rearLeftEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    @Logged(name = "Front Right Encoder Meters", importance = Importance.DEBUG)
+    public double getFrontRightEncoderMeters() {
+        return frontRightEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    @Logged(name = "Rear Right Encoder Meters", importance = Importance.DEBUG)
+    public double getRearRightEncoderMeters() {
+        return rearRightEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    @Logged(name = "Front Left Encoder Meters Per Second", importance = Importance.DEBUG)
+    public double getFrontLeftEncoderMetersPerSecond() {
+        return frontLeftEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    @Logged(name = "Rear Left Encoder Meters Per Second", importance = Importance.DEBUG)
+    public double getRearLeftEncoderMetersPerSecond() {
+        return rearLeftEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1: 1);
+    }
+
+    @Logged(name = "Front Right Encoder Meters Per Second", importance = Importance.DEBUG)
+    public double getFrontRightEncoderMetersPerSecond() {
+        return frontRightEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    @Logged(name = "Rear Right Encoder Meters Per Second", importance = Importance.DEBUG)
+    public double getRearRightEncoderMetersPerSecond() {
+        return rearRightEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+}