Merge branch 'main' into trajectoryTestBranch

src/main/java/frc/robot/Constants.java

@@ -158,7 +158,7 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_BL = 8;
 					ANGULAR_MOTOR_ID_BL = 9;
 					ANGULAR_MOTOR_ENCODER_ID_BL = 2;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.634033203125;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 +0.5;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 10;
@@ -234,8 +234,8 @@ public static class SwerveDrivetrainConstants {
 
 				case PRACTICE_BOT:
 				default: // Temporary default to practice bot
-					MODULE_LOCATION_X = 54 / 100;
-					MODULE_LOCATION_Y = 54 / 100;
+					MODULE_LOCATION_X = 54.0 / 100;
+					MODULE_LOCATION_Y = 54.0 / 100;
 					break;
 
 				// case COMPETITION_BOT:

src/main/java/frc/robot/commands/AutoRotateTo.java

@@ -46,7 +46,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 
 	@Override
 	public void initialize() {
-		currentAngleGoal = !fieldRelative ? drivetrain.getHeading().getRadians() : 0;
+		currentAngleGoal = fieldRelative ? 0 : drivetrain.getHeading().getRadians();
 		currentAngleGoal += angleGoal;
 		SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal);
 	}

src/main/java/frc/robot/commands/TurnToTag.java

@@ -0,0 +1,77 @@
+package frc.robot.commands;
+
+import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.Vision;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** Command to automatically drive a follow a tag a certain translation away */
+public class TurnToTag extends Command {
+	private final SwerveDrivetrain drivetrain;
+
+	private final Vision vision;
+	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
+
+	private final PIDController rotatePID;
+
+	/**
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 * @param vision              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
+	 */
+	public TurnToTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) {
+		this.drivetrain = drivetrain;
+
+		this.vision = vision;
+		this.tagID = tagID;
+
+        rotatePID = new PIDController(
+            RobotMovementConstants.ROTATION_PID_P,
+            RobotMovementConstants.ROTATION_PID_I,
+            RobotMovementConstants.ROTATION_PID_D);
+        rotatePID.enableContinuousInput(-Math.PI, Math.PI);
+        rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void initialize() {
+		drivetrain.toDefaultStates();
+
+        final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
+        rotatePID.setSetpoint(tag.getYaw());
+	}
+
+
+	@Override
+	public void execute() {
+    	final double currentAngle = drivetrain.getHeading().getRadians();
+
+		double turnsSeed = rotatePID.calculate(currentAngle);
+		SmartDashboard.putNumber("Turn Speed Auto", turnsSeed);
+
+		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
+
+		drivetrain.updateSmartDashboard();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return rotatePID.atSetpoint();
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
+}

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

@@ -100,7 +100,8 @@ public SwerveDrivetrain(AHRS gyro,
 		modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
 
 		// create kinematics object using swerve module distance from center
-		kinematics = new SwerveDriveKinematics(new Translation2d(.3, .3),new Translation2d(.3, -.3),new Translation2d(-.3, .3),new Translation2d(-.3, -.3));
+		kinematics = new SwerveDriveKinematics(
+			modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new));
 
 		// create starting state for odometry
 		odometry = new SwerveDriveOdometry(