add new camera positions (front right and front left)

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -74,28 +75,32 @@ public RobotContainer(){
     s_Elevator = new Elevator(new ElevatorIOTalonFX());
     configureBindings();
 
-    m_vision = new Vision(drivetrain::addVisionMeasurement, new VisionIOPhotonVision("front-cam", new Transform3d(new Translation3d(
-        Units.inchesToMeters(11.094),
-        Units.inchesToMeters(-4.925),
-        Units.inchesToMeters(11.5)),
+    m_vision = new Vision(drivetrain::addVisionMeasurement, 
+
+    new VisionIOPhotonVision("front-right-cam", new Transform3d(new Translation3d(
+        Units.inchesToMeters(7.16),
+        Units.inchesToMeters(-10.92),
+        Units.inchesToMeters(9.39)),
         new Rotation3d(
         Units.degreesToRadians(0.0),
-        Units.degreesToRadians(0),
-        Units.degreesToRadians(0)))), new VisionIOPhotonVision("back-left-cam", new Transform3d(new Translation3d(
-        Units.inchesToMeters(-9.124 + 2.5),
-        Units.inchesToMeters(10.646),
-        Units.inchesToMeters(8.25)),
+        Units.degreesToRadians(-21.173),
+        Units.degreesToRadians(-20)))),
+
+    new VisionIOPhotonVision("front-left-cam", new Transform3d(new Translation3d(
+        Units.inchesToMeters(11.151),
+        Units.inchesToMeters(6.43),
+        Units.inchesToMeters(9.41)),
         new Rotation3d(
         Units.degreesToRadians(0.0),
-        Units.degreesToRadians(-28.125),
-        Units.degreesToRadians(150.0)))));
+        Units.degreesToRadians(-25.414),
+        Units.degreesToRadians(-40)))));
 }
 
 
 
 
   public void periodic() {
-    System.out.println("Elevator Command"+s_Elevator.getCurrentCommand());
+    // System.out.println("Elevator Command"+s_Elevator.getCurrentCommand());
   }
   private void configureBindings() {
     // Note that X is defined as forward according to WPILib convention,
@@ -135,7 +140,7 @@ private void configureBindings() {
 
     joystick.rightTrigger().whileTrue(s_Shooter.shootTrough()).onFalse(s_Shooter.stop());
     joystick.y().whileTrue(s_Shooter.intake()).onFalse(s_Shooter.stop());
-    // joystick.a().whileTrue(s_Shooter.shootTrough()).onFalse(s_Shooter.stop());
+    joystick.a().whileTrue(s_Shooter.shootTrough()).onFalse(s_Shooter.stop());
 
     joystick.b().onTrue(s_Elevator.goToL1()).onFalse(s_Elevator.stop());
     joystick.leftBumper().onTrue(s_Elevator.goToL2()).onFalse(s_Elevator.goToL1());
@@ -149,15 +154,17 @@ private void configureBindings() {
 
 
 
-    joystick
-        .b()
-        .whileTrue(
-            drivetrain.applyRequest(
-                () ->
-                    point.withModuleDirection(
-                        new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
+    // joystick
+    //     .b()
+    //     .whileTrue(
+    //         drivetrain.applyRequest(
+    //             () ->
+    //                 point.withModuleDirection(
+    //                     new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
 
-    joystick.x().whileTrue(new DriveToPose(drivetrain));
+    joystick.b().whileTrue(new DriveToPose(drivetrain, new Transform2d(Units.inchesToMeters(-3),Units.inchesToMeters(6),new Rotation2d())));
+    joystick.x().whileTrue(new DriveToPose(drivetrain, new Transform2d(Units.inchesToMeters(-3),Units.inchesToMeters(19),new Rotation2d())));
+
 
     // joystick.x().whileTrue(new DriveToPose( new Pose2d(
     //   Units.inchesToMeters(144.003)-Units.inchesToMeters(13),

src/main/java/frc/robot/generated/TunerConstants.java

@@ -152,7 +152,7 @@ public class TunerConstants {
     private static final int kFrontLeftDriveMotorId = 4;
     private static final int kFrontLeftSteerMotorId = 3;
     private static final int kFrontLeftEncoderId = 12;
-    private static final Angle kFrontLeftEncoderOffsetRobot1 = Rotations.of(0.375); //0.375 green robot drive rail offset V2
+    private static final Angle kFrontLeftEncoderOffsetRobot1 = Rotations.of(-0.12451171875); //0.375 green robot drive rail offset V2
     private static final Angle kFrontLeftEncoderOffsetRobot2 = Rotations.of(0.27880859375); //0.375 green robot drive rail offset V2
     private static final boolean kFrontLeftSteerMotorInverted = true;
     private static final boolean kFrontLeftEncoderInverted = false;
@@ -164,7 +164,7 @@ public class TunerConstants {
     private static final int kFrontRightDriveMotorId = 6;
     private static final int kFrontRightSteerMotorId = 5;
     private static final int kFrontRightEncoderId = 13;
-    private static final Angle kFrontRightEncoderOffsetRobot1 = Rotations.of(0.39453125); //0.39453125 green robot drive rail offset V2
+    private static final Angle kFrontRightEncoderOffsetRobot1 = Rotations.of(-0.37255859375); //0.39453125 green robot drive rail offset V2
     private static final Angle kFrontRightEncoderOffsetRobot2 = Rotations.of(0.05419921875); //0.39453125 green robot drive rail offset V2
     private static final boolean kFrontRightSteerMotorInverted = true;
     private static final boolean kFrontRightEncoderInverted = false;
@@ -176,7 +176,7 @@ public class TunerConstants {
     private static final int kBackLeftDriveMotorId = 2;
     private static final int kBackLeftSteerMotorId = 1;
     private static final int kBackLeftEncoderId = 11;
-    private static final Angle kBackLeftEncoderOffsetRobot1 = Rotations.of(0.27490234375); //0.375 green robot drive rail offset V2
+    private static final Angle kBackLeftEncoderOffsetRobot1 = Rotations.of(0.409912109375); //0.375 green robot drive rail offset V2
     private static final Angle kBackLeftEncoderOffsetRobot2 = Rotations.of(0.34765625); //0.27490234375 green robot drive rail offset V2
     private static final boolean kBackLeftSteerMotorInverted = true;
     private static final boolean kBackLeftEncoderInverted = false;
@@ -188,7 +188,7 @@ public class TunerConstants {
     private static final int kBackRightDriveMotorId = 7;
     private static final int kBackRightSteerMotorId = 8;
     private static final int kBackRightEncoderId = 14;
-    private static final Angle kBackRightEncoderOffsetRobot1 = Rotations.of(-0.206298828125); //-0.206298828125 green robot drive rail offset V2
+    private static final Angle kBackRightEncoderOffsetRobot1 = Rotations.of(-0.039794921875); //-0.206298828125 green robot drive rail offset V2
     private static final Angle kBackRightEncoderOffsetRobot2 = Rotations.of(0.0888671875); //-0.206298828125 green robot drive rail offset V2
     private static final boolean kBackRightSteerMotorInverted = true;
     private static final boolean kBackRightEncoderInverted = false;
@@ -197,7 +197,7 @@ public class TunerConstants {
     private static final Distance kBackRightYPos = Inches.of(-10.5);
 
 
-    private final static String ROBOT1 = "UNKNOWN";
+    private final static String ROBOT1 = "032380FD";
     private final static String ROBOT2 = "032243C9";
 
 

src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java

@@ -7,6 +7,8 @@
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.util.GeometryUtil;
+
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;

src/main/java/frc/robot/subsystems/drivetrain/DriveToPose.java

@@ -2,6 +2,7 @@
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType;
+import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.pathplanner.lib.util.GeometryUtil;
 import edu.wpi.first.math.MathUtil;
@@ -14,6 +15,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Unit;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
@@ -32,9 +34,10 @@ public class DriveToPose extends Command {
   private SwerveRequest.ApplyFieldSpeeds m_drive;
 
 
-  public DriveToPose(CommandSwerveDrivetrain drivetrain) {
+  public DriveToPose(CommandSwerveDrivetrain drivetrain, Transform2d shift) {
+    m_pose = drivetrain.findClosestNode().transformBy(shift);
     m_translationController =
-        new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1.2, 0.5));
+        new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2, 2));
     m_thetaController =
         new ProfiledPIDController(
             5.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1 * Math.PI, 1 * Math.PI));
@@ -49,12 +52,7 @@ public DriveToPose(CommandSwerveDrivetrain drivetrain) {
 
   @Override
   public void initialize() {
-    m_pose = m_drivetrain.findClosestNode().transformBy(new Transform2d(0,Units.inchesToMeters(6),new Rotation2d()));
     m_goalPose = m_pose;
-    // m_goalPose =
-    //     Constants.onRedAllianceSupplier.getAsBoolean()
-    //         ? GeometryUtil.flipFieldPose(m_pose)
-    //         : m_pose;
     Pose2d initialPose = m_drivetrain.getState().Pose;
 
     m_translationController.setTolerance(0.05);