Merge branch 'mayhem-geared-up' of https://github.com/Team766/2024 in…

…to mayhem-geared-up

src/main/java/com/team766/odometry/DifferentialDriveOdometry.java

@@ -0,0 +1,127 @@
+package com.team766.odometry;
+
+import com.team766.hal.wpilib.REVThroughBoreDutyCycleEncoder;
+import com.team766.library.RateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class DifferentialDriveOdometry {
+
+    private static final double RATE_LIMITER_TIME = 0.05;
+
+    private RateLimiter odometryLimiter;
+    private REVThroughBoreDutyCycleEncoder leftEncoder, rightEncoder;
+
+    private Pose2d currentPosition;
+
+    private double prevLeftEncoderValue, prevRightEncoderValue;
+    private double currLeftEncoderValue, currRightEncoderValue;
+
+    // Physical properties of the robot
+    private double wheelCircumference;
+    private double gearRatio;
+    private double encoderToRevolutionConstant;
+    private double wheelBaseWidth; // Distance between the left and right wheels (in meters)
+
+    /**
+     * Constructor for DifferentialDriveOdometry.
+     *
+     * @param leftMotor The motor controlling the left wheel.
+     * @param rightMotor The motor controlling the right wheel.
+     * @param wheelCircumference The circumference of the wheels, including treads (in meters).
+     * @param gearRatio The gear ratio of the wheels.
+     * @param encoderToRevolutionConstant The encoder to revolution constant of the wheels.
+     * @param wheelBaseWidth The distance between the left and right wheels (in meters).
+     */
+    public DifferentialDriveOdometry(
+            REVThroughBoreDutyCycleEncoder leftEncoder,
+            REVThroughBoreDutyCycleEncoder rightEncoder,
+            double wheelCircumference,
+            double gearRatio,
+            double encoderToRevolutionConstant,
+            double wheelBaseWidth) {
+
+        this.leftEncoder = leftEncoder;
+        this.rightEncoder = rightEncoder;
+        this.wheelCircumference = wheelCircumference;
+        this.gearRatio = gearRatio;
+        this.encoderToRevolutionConstant = encoderToRevolutionConstant;
+        this.wheelBaseWidth = wheelBaseWidth;
+
+        odometryLimiter = new RateLimiter(RATE_LIMITER_TIME);
+
+        currentPosition = new Pose2d(0, 0, new Rotation2d()); // Start at the origin
+        prevLeftEncoderValue = 0;
+        prevRightEncoderValue = 0;
+        currLeftEncoderValue = 0;
+        currRightEncoderValue = 0;
+    }
+
+    /**
+     * Sets the current position of the robot to a specified Pose2d.
+     *
+     * @param position The position to set the robot to.
+     */
+    public void setCurrentPosition(Pose2d position) {
+        currentPosition = position;
+    }
+
+    /**
+     * Updates the encoder values for both the left and right wheels.
+     */
+    private void setCurrentEncoderValues() {
+        prevLeftEncoderValue = currLeftEncoderValue;
+        prevRightEncoderValue = currRightEncoderValue;
+
+        currLeftEncoderValue = leftEncoder.getAbsolutePosition();
+        currRightEncoderValue = rightEncoder.getAbsolutePosition();
+    }
+
+    /**
+     * Calculates the robot's position and heading (guess)
+     */
+    private void updatePosition() {
+        // Get the change in encoder values (how much each wheel moved)
+        double deltaLeft =
+                (currLeftEncoderValue - prevLeftEncoderValue)
+                        * (wheelCircumference / (gearRatio * encoderToRevolutionConstant));
+        double deltaRight =
+                (currRightEncoderValue - prevRightEncoderValue)
+                        * (wheelCircumference / (gearRatio * encoderToRevolutionConstant));
+
+        // Calculate the distance the robot traveled and the rotation change
+        double distanceTraveled = (deltaLeft + deltaRight) / 2;
+        double rotationChange = (deltaRight - deltaLeft) / wheelBaseWidth;
+
+        double currentHeading = currentPosition.getRotation().getRadians();
+
+        double deltaX = distanceTraveled * Math.cos(currentHeading + rotationChange / 2);
+        double deltaY = distanceTraveled * Math.sin(currentHeading + rotationChange / 2);
+
+        // Update the current position and heading of the robot
+        currentPosition =
+                new Pose2d(
+                        currentPosition.getX() + deltaX,
+                        currentPosition.getY() + deltaY,
+                        currentPosition.getRotation().plus(Rotation2d.fromRadians(rotationChange)));
+    }
+
+    /**
+     * Main odometry update loop. Should be called periodically to update the robot's position.
+     */
+    public void run() {
+        if (odometryLimiter.next()) {
+            setCurrentEncoderValues();
+            updatePosition();
+        }
+    }
+
+    /**
+     * Returns the current position of the robot.
+     *
+     * @return The current Pose2d of the robot.
+     */
+    public Pose2d getCurrentPosition() {
+        return currentPosition;
+    }
+}

src/main/java/com/team766/robot/common/DriverOI.java

@@ -52,7 +52,7 @@ public void handleOI(Context context) {
                         * ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) {
-            drive.resetGyro();
+            drive.resetHeading();
         }
 
         if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_POS)) {

src/main/java/com/team766/robot/common/constants/ConfigConstants.java

@@ -5,7 +5,7 @@ public final class ConfigConstants {
     // utility class
     private ConfigConstants() {}
 
-    // drive config values
+    // swerve drive config values
     public static final String DRIVE_GYRO = "drive.Gyro";
     public static final String DRIVE_DRIVE_FRONT_RIGHT = "drive.DriveFrontRight";
     public static final String DRIVE_DRIVE_FRONT_LEFT = "drive.DriveFrontLeft";
@@ -19,6 +19,10 @@ private ConfigConstants() {}
 
     public static final String DRIVE_TARGET_ROTATION_PID = "drive.setpointPid";
 
+    // burro drive config values
+    public static final String DRIVE_RIGHT = "drive.Right";
+    public static final String DRIVE_LEFT = "drive.Left";
+
     // pathplanner config values
     public static final String PATH_FOLLOWING_MAX_MODULE_SPEED_MPS =
             "followpath.maxSpeedMetersPerSecond";

src/main/java/com/team766/robot/common/mechanisms/BurroDrive.java

@@ -0,0 +1,129 @@
+package com.team766.robot.common.mechanisms;
+
+import static com.team766.robot.common.constants.ConfigConstants.*;
+
+import com.team766.hal.MotorController;
+import com.team766.hal.MotorController.ControlMode;
+import com.team766.hal.wpilib.REVThroughBoreDutyCycleEncoder;
+import com.team766.hal.RobotProvider;
+import com.team766.logging.Category;
+import com.team766.odometry.DifferentialDriveOdometry;
+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.DifferentialDriveKinematics;
+// import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+
+public class BurroDrive extends Drive {
+
+    private final MotorController leftMotor;
+    private final MotorController rightMotor;
+    private final REVThroughBoreDutyCycleEncoder leftEncoder;
+    private final REVThroughBoreDutyCycleEncoder rightEncoder;
+    private DifferentialDriveKinematics differentialDriveKinematics;
+    private DifferentialDriveOdometry differentialDriveOdometry;
+
+    // TODO set actual ratio
+    private static final double DRIVE_GEAR_RATIO = 1; // Gear ratio
+
+    // TODO set actual radius
+    private static final double WHEEL_RADIUS = 1; // Radius of the wheels
+
+    // TODO
+    private static final double ENCODER_UNIT_TO_REVOLUTION_CONSTANT = 1.; 
+
+    private static final double MOTOR_WHEEL_FACTOR_MPS =
+            1.
+                    / WHEEL_RADIUS // Wheel radians/sec
+                    * DRIVE_GEAR_RATIO // Motor radians/sec
+                    / (2 * Math.PI) // Motor rotations/sec (what velocity mode takes))
+                    * ENCODER_UNIT_TO_REVOLUTION_CONSTANT; // Encoder units/sec
+
+    // TODO
+    private static final double TRACK_WIDTH_METERS = 0.4; // Distance between left and right wheel
+
+    public BurroDrive() {
+        loggerCategory = Category.DRIVE;
+
+        leftMotor = RobotProvider.instance.getMotor("drive.leftMotor");
+        rightMotor = RobotProvider.instance.getMotor("drive.rightMotor");
+
+        leftEncoder = null; //FIXME
+        rightEncoder = null; //FIXME
+
+        differentialDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
+        differentialDriveOdometry =
+                new DifferentialDriveOdometry(
+                        leftEncoder,
+                        rightEncoder,
+                        WHEEL_RADIUS * 2 * Math.PI,
+                        DRIVE_GEAR_RATIO,
+                        ENCODER_UNIT_TO_REVOLUTION_CONSTANT,
+                        TRACK_WIDTH_METERS);
+    }
+
+    /**
+     * @param forward how much power to apply to moving the robot (positive being forward)
+     * @param turn how much power to apply to turning the robot (positive being CCW)
+     */
+    public void drive(double forward, double turn) {
+        checkContextOwnership();
+        leftMotor.set(forward - turn);
+        rightMotor.set(forward + turn);
+    }
+
+    public void controlRobotOriented(ChassisSpeeds chassisSpeeds) {
+        DifferentialDriveWheelSpeeds wheelSpeeds =
+                differentialDriveKinematics.toWheelSpeeds(chassisSpeeds);
+        leftMotor.set(
+                ControlMode.Velocity, MOTOR_WHEEL_FACTOR_MPS * wheelSpeeds.leftMetersPerSecond);
+        rightMotor.set(
+                ControlMode.Velocity, MOTOR_WHEEL_FACTOR_MPS * wheelSpeeds.rightMetersPerSecond);
+    }
+
+    public Pose2d getCurrentPosition() {
+        return differentialDriveOdometry.getCurrentPosition();
+    }
+
+    public void resetCurrentPosition() {
+        differentialDriveOdometry.setCurrentPosition(new Pose2d(0, 0, new Rotation2d()));
+    }
+
+    public void setCurrentPosition(Pose2d P) {
+        differentialDriveOdometry.setCurrentPosition(P);
+    }
+
+    public void resetHeading(double angle) {
+        Pose2d curPose = getCurrentPosition();
+        differentialDriveOdometry.setCurrentPosition(
+                new Pose2d(curPose.getX(), curPose.getY(), Rotation2d.fromDegrees(angle)));
+    }
+
+    public double getHeading() {
+        return getCurrentPosition().getRotation().getDegrees();
+    }
+
+    /*
+     * Stops each drive motor
+     */
+    public void stopDrive() {
+        checkContextOwnership();
+        leftMotor.stopMotor();
+        rightMotor.stopMotor();
+    }
+
+    public ChassisSpeeds getChassisSpeeds() {
+        return differentialDriveKinematics.toChassisSpeeds(
+                new DifferentialDriveWheelSpeeds(
+                        leftMotor.getSensorVelocity() / MOTOR_WHEEL_FACTOR_MPS,
+                        rightMotor.getSensorVelocity() / MOTOR_WHEEL_FACTOR_MPS));
+    }
+
+    public void setCross() {}
+
+    @Override
+    public void run() {
+        differentialDriveOdometry.run();
+    }
+}

src/main/java/com/team766/robot/common/mechanisms/Drive.java

@@ -0,0 +1,33 @@
+package com.team766.robot.common.mechanisms;
+
+import com.team766.framework.Mechanism;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+public abstract class Drive extends Mechanism {
+    /**
+     * Controls the robot with a ChassisSpeeds input
+     *
+     * @param chassisSpeeds the velocity vector of the robot
+     */
+    public abstract void controlRobotOriented(ChassisSpeeds chassisSpeeds);
+
+    /**
+     * Stops the robot drive
+     */
+    public abstract void stopDrive();
+
+    public abstract Pose2d getCurrentPosition();
+
+    public abstract void resetCurrentPosition();
+
+    public abstract double getHeading();
+
+    public abstract void resetHeading(double angle);
+
+    public abstract void setCurrentPosition(Pose2d P);
+
+    public abstract ChassisSpeeds getChassisSpeeds();
+
+    public abstract void setCross(); // dummy method
+}

src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java

@@ -4,7 +4,6 @@
 
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.team766.controllers.PIDController;
-import com.team766.framework.Mechanism;
 import com.team766.hal.GyroReader;
 import com.team766.hal.MotorController;
 import com.team766.hal.RobotProvider;
@@ -27,7 +26,7 @@
 import java.util.Optional;
 import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
 
-public class SwerveDrive extends Mechanism {
+public class SwerveDrive extends Drive {
 
     private final SwerveConfig config;
 
@@ -315,15 +314,15 @@ public void setCross() {
      * Resets gyro to zero degrees relative to the driver
      * Sets to 180 degrees if the driver is on red (facing backwards)
      */
-    public void resetGyro() {
-        resetGyro(alliance.isPresent() && alliance.get() == Alliance.Blue ? 0 : 180);
+    public void resetHeading() {
+        resetHeading(alliance.isPresent() && alliance.get() == Alliance.Blue ? 0 : 180);
     }
 
     /**
      * Sets gyro to value in degrees
      * @param angle in degrees
      */
-    public void resetGyro(double angle) {
+    public void resetHeading(double angle) {
         checkContextOwnership();
         gyro.setAngle(angle);
     }