Merge remote-tracking branch 'origin/autoalign'

.Glass/glass.json

@@ -1,3 +1,24 @@
+{
+  "NetworkTables": {
+    "types": {
+      "/AdvantageKit/RealOutputs/Alerts": "Alerts",
+      "/FMSInfo": "FMSInfo",
+      "/Pose": "Field2d",
+      "/SmartDashboard/Alerts": "Alerts",
+      "/SmartDashboard/Module 0": "Mechanism2d",
+      "/SmartDashboard/Module 1": "Mechanism2d",
+      "/SmartDashboard/Module 2": "Mechanism2d",
+      "/SmartDashboard/Module 3": "Mechanism2d"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)",
+    "serverTeam": "4909"
+  }
+}
 {
   "NetworkTables": {
     "transitory": {

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

@@ -20,6 +20,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;
@@ -95,28 +96,32 @@ public RobotContainer(){
 
     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)),
-    //     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)),
-    //     new Rotation3d(
-    //     Units.degreesToRadians(0.0),
-    //     Units.degreesToRadians(-28.125),
-    //     Units.degreesToRadians(150.0)))));
+    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(-21.173),
+        Units.degreesToRadians(-20)))),
+
+    new VisionIOPhotonVision("front-left-cam", new Transform3d(new Translation3d(
+        Units.inchesToMeters(7.211),
+        Units.inchesToMeters(10.607),
+        Units.inchesToMeters(9.411)),
+        new Rotation3d(
+        Units.degreesToRadians(0.0),
+        Units.degreesToRadians(-25.414),
+        Units.degreesToRadians(-20)))));
 }
 
 
 
 
   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,
@@ -156,7 +161,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());
@@ -170,15 +175,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(4),new Rotation2d())));
+    joystick.x().whileTrue(new DriveToPose(drivetrain, new Transform2d(Units.inchesToMeters(-3),Units.inchesToMeters(17),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/Vision/VisionConstants.java

@@ -24,13 +24,13 @@ public class VisionConstants {
       AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
   // Camera names, must match names configured on coprocessor
-  public static String camera0Name = "back-left-cam";
-  public static String camera1Name = "front-cam";
+  // public static String camera0Name = "back-left-cam";
+  // public static String camera1Name = "front-cam";
 
   // Robot to camera transforms
   // (Not used by Limelight, configure in web UI instead)
-  public static Transform3d robotToCamera0 =
-      new Transform3d(-0.3302, -0.3302, 0, new Rotation3d((3/4)*Math.PI, 0.2, 0.0));
+  // public static Transform3d robotToCamera0 =
+  //     new Transform3d(-0.3302, -0.3302, 0, new Rotation3d((3/4)*Math.PI, 0.2, 0.0));
 //   public static Transform3d robotToCamera1 =
 //       new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
 

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

@@ -8,6 +8,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 com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;

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,11 +15,11 @@
 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;
 
-import static frc.robot.subsystems.vision.VisionConstants.robotToCamera0;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -27,14 +28,15 @@ public class DriveToPose extends Command {
   private final CommandSwerveDrivetrain m_drivetrain;
   private final ProfiledPIDController m_translationController, m_thetaController;
   private Translation2d m_lastSetpointTranslation;
-  private Pose2d m_pose;
   private Pose2d m_goalPose;
   private SwerveRequest.ApplyFieldSpeeds m_drive;
+  Transform2d m_shift;
 
 
-  public DriveToPose(CommandSwerveDrivetrain drivetrain) {
+  public DriveToPose(CommandSwerveDrivetrain drivetrain, Transform2d shift) {
+    m_shift = shift;
     m_translationController =
-        new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1.2, 0.5));
+        new ProfiledPIDController(4.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 +51,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;
+    m_goalPose = m_drivetrain.findClosestNode().transformBy(m_shift);
     Pose2d initialPose = m_drivetrain.getState().Pose;
 
     m_translationController.setTolerance(0.05);