Initial Commit

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

@@ -0,0 +1,207 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+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.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+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.DifferentialDriveConstants;
+import frc.robot.interfaces.IDrivetrain;
+
+public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain {
+    private VictorSP frontLeft;
+    private VictorSP rearLeft;
+    private VictorSP frontRight;
+    private VictorSP rearRight;
+
+    private DifferentialDrive drive;
+
+    private Encoder leftEncoder;
+    private Encoder rightEncoder;
+
+    private ADXRS450_Gyro gyro;
+
+    private DifferentialDrivePoseEstimator poseEstimator;
+
+    private boolean slowMode;
+    private boolean fieldOriented;
+
+    public DifferentialDrivetrain(Pose2d initialPose, boolean slowModeAtStartup, boolean fieldOrientedAtStartup) {
+        super();
+
+        frontLeft = new VictorSP(DifferentialDriveConstants.kFrontLeftPWM);
+        rearLeft = new VictorSP(DifferentialDriveConstants.kRearLeftPWM);
+        frontRight = new VictorSP(DifferentialDriveConstants.kFrontRightPWM);
+        rearRight = new VictorSP(DifferentialDriveConstants.kRearRightPWM);
+
+        frontLeft.addFollower(rearLeft);
+        frontRight.addFollower(rearRight);
+        frontRight.setInverted(true);
+
+        drive = new DifferentialDrive(frontLeft, frontRight);
+
+        leftEncoder = new Encoder(
+            DifferentialDriveConstants.kLeftEncoderChannelA, 
+            DifferentialDriveConstants.kLeftEncoderChannelB
+        );
+        leftEncoder.setDistancePerPulse(DifferentialDriveConstants.kDistancePerPulseMeters);
+
+        rightEncoder = new Encoder(
+            DifferentialDriveConstants.kRightEncoderChannelA,
+            DifferentialDriveConstants.kRightEncoderChannelB
+        );
+        rightEncoder.setDistancePerPulse(DifferentialDriveConstants.kDistancePerPulseMeters);
+
+        gyro = new ADXRS450_Gyro();
+
+        poseEstimator = new DifferentialDrivePoseEstimator(
+            DifferentialDriveConstants.kKinematics, 
+            Rotation2d.fromDegrees(getGyroAngle()), 
+            getLeftEncoderMeters(), 
+            getRightEncoderMeters(),
+            initialPose 
+        );
+
+        slowMode = slowModeAtStartup;
+        fieldOriented = fieldOrientedAtStartup;
+    }
+
+    @Override
+    public void periodic() {
+        poseEstimator.update(
+            Rotation2d.fromDegrees(getGyroAngle()), 
+            getLeftEncoderMeters(), 
+            getRightEncoderMeters()
+        );
+    }
+
+    @Override
+    public void resetPose(Pose2d pose) {
+        poseEstimator.resetPosition(
+            Rotation2d.fromDegrees(getGyroAngle()), 
+            getLeftEncoderMeters(),
+            getRightEncoderMeters(), 
+            pose
+        );
+    }
+
+    @Override
+    public Pose2d getPose() {
+        return poseEstimator.getEstimatedPosition();
+    }
+
+    @Override
+    public boolean isSlowMode() {
+        return slowMode;
+    }
+
+    @Override
+    public boolean isFieldOriented() {
+        return fieldOriented;
+    }
+
+    @Override
+    public ChassisSpeeds getChassisSpeeds() {
+        return DifferentialDriveConstants.kKinematics.toChassisSpeeds(
+            new DifferentialDriveWheelSpeeds(
+                getLeftEncoderMetersPerSecond(),
+                getRightEncoderMetersPerSecond() 
+            )
+        );
+    }
+
+    @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 ? DifferentialDriveConstants.kSlowModeMultiplier : 1)
+                * (DifferentialDriveConstants.kInvertForwardBackward ? -1 : 1)
+                * DifferentialDriveConstants.kMaxSpeedMetersPerSecond;
+
+            double rotDelivered = 
+                controller.getLeftX()
+                * (slowMode ? DifferentialDriveConstants.kSlowModeMultiplier : 1)
+                * (DifferentialDriveConstants.kInvertLeftRight ? -1 : 1)
+                * DifferentialDriveConstants.kMaxAngularSpeedRadiansPerSecond;
+
+            DifferentialDriveWheelSpeeds wheelSpeeds = DifferentialDriveConstants.kKinematics.toWheelSpeeds(
+                fieldOriented ?
+                    ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, 0, rotDelivered, Rotation2d.fromDegrees(getGyroAngle())) :
+                    new ChassisSpeeds(xSpeedDelivered, 0, rotDelivered)
+            );
+
+            drive.tankDrive(
+                wheelSpeeds.leftMetersPerSecond / DifferentialDriveConstants.kMaxSpeedMetersPerSecond, 
+                wheelSpeeds.rightMetersPerSecond / DifferentialDriveConstants.kMaxSpeedMetersPerSecond
+            );
+        });
+    }
+
+    @Override
+    public Command drive(ChassisSpeeds speeds) {
+        return run(() -> {
+            DifferentialDriveWheelSpeeds wheelSpeeds = 
+                DifferentialDriveConstants.kKinematics.toWheelSpeeds(speeds);
+
+            drive.tankDrive(
+                wheelSpeeds.leftMetersPerSecond / DifferentialDriveConstants.kMaxSpeedMetersPerSecond, 
+                wheelSpeeds.rightMetersPerSecond / DifferentialDriveConstants.kMaxSpeedMetersPerSecond
+            );
+        });
+    }
+
+    @Override
+    public Command drive(Pose2d targetPose) {
+        return new PrintCommand("Differential Drivetrain Drive to Pose Not Implemented!");
+    }
+
+    @Override
+    public Command stop() {
+        return run(() -> {
+            drive.tankDrive(0, 0);
+        });
+    }
+
+    public double getGyroAngle() {
+        return gyro.getAngle() * (DifferentialDriveConstants.kInvertGyro ? -1 : 1);
+    }
+
+    public double getLeftEncoderMeters() {
+        return leftEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    public double getRightEncoderMeters() {
+        return rightEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    public double getLeftEncoderMetersPerSecond() {
+        return leftEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+
+    public double getRightEncoderMetersPerSecond() {
+        return rightEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
+    }
+}