Initial Commit

src/main/java/frc/robot/Main.java

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public final class Main {
+  private Main() {}
+
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

src/main/java/frc/robot/Robot.java

@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final RobotContainer m_robotContainer;
+
+  public Robot() {
+    m_robotContainer = new RobotContainer();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    CommandScheduler.getInstance().run();
+  }
+
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  @Override
+  public void disabledExit() {}
+
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void autonomousExit() {}
+
+  @Override
+  public void teleopInit() {
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void teleopExit() {}
+
+  @Override
+  public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  @Override
+  public void testPeriodic() {}
+
+  @Override
+  public void testExit() {}
+}

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

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+public class RobotContainer {
+  public RobotContainer() {
+    configureBindings();
+  }
+
+  private void configureBindings() {}
+
+  public Command getAutonomousCommand() {
+    return Commands.print("No autonomous command configured");
+  }
+}

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

@@ -0,0 +1,35 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+
+public class DifferentialDriveConstants {
+    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 kLeftEncoderChannelA = 0;
+    public static final int kLeftEncoderChannelB = 1;
+    public static final int kRightEncoderChannelA = 2;
+    public static final int kRightEncoderChannelB = 3;
+
+    // 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 double kTrackWidthMeters = 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 kInvertGyro = true;
+    public static final boolean kInvertEncoders = false;
+
+    public static final DifferentialDriveKinematics kKinematics = new DifferentialDriveKinematics(
+        kTrackWidthMeters
+    );
+}

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -0,0 +1,15 @@
+package frc.robot.interfaces;
+
+import java.util.Optional;
+import java.util.OptionalDouble;
+import java.util.OptionalInt;
+
+import edu.wpi.first.math.geometry.Transform2d;
+
+public interface IAprilTagProvider {
+    public OptionalInt getClosestTagID();
+
+    public OptionalDouble getDistanceToTag(int tagID);
+
+    public Optional<Transform2d> getTransformRobotRelative(int tagID);
+}

src/main/java/frc/robot/interfaces/IDrivetrain.java

@@ -0,0 +1,30 @@
+package frc.robot.interfaces;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+public interface IDrivetrain {
+    public boolean isSlowMode();
+
+    public boolean isFieldOriented();
+
+    public Pose2d getPose();
+
+    public void resetPose(Pose2d pose);
+
+    public ChassisSpeeds getChassisSpeeds();
+
+    public Command toggleSlowMode();
+
+    public Command toggleFieldOriented();
+
+    public Command drive(CommandXboxController controller);
+
+    public Command drive(ChassisSpeeds speeds);
+
+    public Command drive(Pose2d targetPose);
+
+    public Command stop();
+}

src/main/java/frc/robot/interfaces/ISimpleMotor.java

@@ -0,0 +1,9 @@
+package frc.robot.interfaces;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public interface ISimpleMotor {
+    public Command setSpeed();
+
+    public Command stop();
+}

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -0,0 +1,11 @@
+package frc.robot.interfaces;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public interface IVisualPoseProvider {
+    public record VisualPose(Pose2d visualPose, double timestamp) {}
+
+    public Optional<VisualPose> getVisualPose();
+}

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);
+    }
+}