Code go brrr

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index d9af07497f0a3009dc94e4c3f92fc4283f757905..119fa14ed74c4873c6f0086d83612f23a3b67c95 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -79,10 +79,10 @@ public class DefaultDriveCommand extends Command {
                 double yawDegrees = swerve.getYaw().getDegrees();
                 double snappedDeg;
 
-                if (swerve.getPose().getX() < FieldConstants.FIELD_LENGTH / 2) {
-                    snappedDeg = (swerve.getYaw().getDegrees() > 135 || swerve.getYaw().getDegrees() < 225) ? 180 : 0;
+                if (swerve.getPose().getX() < swerve.getDesiredPose().getX()) {
+                    snappedDeg = (yawDegrees > 135 || yawDegrees < 225) ? 180 : 0;
                 } else {
-                    snappedDeg = (swerve.getYaw().getDegrees() < 45 || swerve.getYaw().getDegrees() > 315) ? 0 : 180;
+                    snappedDeg = (yawDegrees < 45 || yawDegrees > 315) ? 0 : 180;
                 }
 
                 swerve.setAlignAngle(Units.degreesToRadians(snappedDeg));

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index ef784cca385bf5c8ee2beba52f654443ca080ee8..854aaf918037bf50808c6d427b9c2c728310de0a 100644 (file)
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -155,33 +155,33 @@ public class VisionConstants {
                 new Pair<String, Transform3d>(
                         "CameraFrontLeft",
                         new Transform3d(
-                                new Translation3d(Units.inchesToMeters(10.965663), 
-                                        Units.inchesToMeters(-8.602606),
-                                        Units.inchesToMeters(19.337757)),
+                                new Translation3d(Units.inchesToMeters(8.47), 
+                                        Units.inchesToMeters(-11.54),
+                                        Units.inchesToMeters(19.7)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(55.0)))),
                 new Pair<String, Transform3d>(
                         "CameraFrontRight",
                         new Transform3d(
-                                new Translation3d(Units.inchesToMeters(-10.965663), 
-                                        Units.inchesToMeters(-8.602606),
-                                        Units.inchesToMeters(19.337757)),
+                                new Translation3d(Units.inchesToMeters(-8.47), 
+                                        Units.inchesToMeters(-11.54),
+                                        Units.inchesToMeters(19.7)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(-55.0)))),
                 new Pair<String, Transform3d>(
                         "CameraBackLeft",
                         new Transform3d(
-                                new Translation3d(Units.inchesToMeters(10.518135), 
-                                        Units.inchesToMeters(-11.140663),
-                                        Units.inchesToMeters(19.337757)),
+                                new Translation3d(Units.inchesToMeters(10.91), 
+                                        Units.inchesToMeters(-12),
+                                        Units.inchesToMeters(19.66)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(145.0)))),
                 new Pair<String, Transform3d>(
                         "CameraBackRight",
                         new Transform3d(
-                                new Translation3d(Units.inchesToMeters(-10.518135), 
-                                        Units.inchesToMeters(-11.140663),
-                                        Units.inchesToMeters(19.337757)),
+                                new Translation3d(Units.inchesToMeters(-10.91), 
+                                        Units.inchesToMeters(-12),
+                                        Units.inchesToMeters(19.66)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(-145.0))))
                 ));

diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
index c9f65912f5e04e354f33a16505c90aea2e1586b9..5cb0da5aaec69ea33a35fc08f496dcf8229fbfd4 100644 (file)
--- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java
+++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java
@@ -214,7 +214,7 @@ public class DriveConstants {
 
                 // Gear ratios 
                 //DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R2 Ratio
-                DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R1 Ratio
+                DRIVE_GEAR_RATIO = (54.0 / 12.0) * (25.0 / 32.0) * (30.0 / 15.0); //R1 Ratio
                 STEER_GEAR_RATIO = 287.0 / 11.0;
 
                 // Gyro is mounted under the robot 

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 057224ca6fed21d88dcfef6d610f82487fd58870..d48f879a644df40b40299b2b1219ae70e687caaa 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -122,6 +122,8 @@ public class Turret extends SubsystemBase implements TurretIO{
                //Sets the initial motor position
                motor.setPosition(motorRotations);
 
+               motor.setPosition(0.0);
+
                SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
                SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));
                SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);}));