Constant Changes, remove CRT

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 6a4802312a96cb0919d95fb83792ae5190637fff..f2351697488fb9965608002b23b0adae27be0eda 100644 (file)
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -157,7 +157,7 @@ public class VisionConstants {
                         new Transform3d(
                                 new Translation3d(Units.inchesToMeters(-8.47), 
                                         Units.inchesToMeters(11.54),
-                                        Units.inchesToMeters(19.7)),
+                                        Units.inchesToMeters(18.7)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(55.0)))),
                 new Pair<String, Transform3d>(
@@ -165,7 +165,7 @@ public class VisionConstants {
                         new Transform3d(
                                 new Translation3d(Units.inchesToMeters(-8.47), 
                                         Units.inchesToMeters(-11.54),
-                                        Units.inchesToMeters(19.7)),
+                                        Units.inchesToMeters(18.7)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(-55.0)))),
                 new Pair<String, Transform3d>(
@@ -173,7 +173,7 @@ public class VisionConstants {
                         new Transform3d(
                                 new Translation3d(Units.inchesToMeters(-10.91), 
                                         Units.inchesToMeters(12),
-                                        Units.inchesToMeters(19.66)),
+                                        Units.inchesToMeters(18.66)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(145.0)))),
                 new Pair<String, Transform3d>(
@@ -181,7 +181,7 @@ public class VisionConstants {
                         new Transform3d(
                                 new Translation3d(Units.inchesToMeters(-10.91), 
                                         Units.inchesToMeters(-12),
-                                        Units.inchesToMeters(19.66)),
+                                        Units.inchesToMeters(18.66)),
                                 new Rotation3d(0, Units.degreesToRadians(-22.0),
                                         Units.degreesToRadians(-145.0))))
                 ));

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index f77aa8279c1657d5863fcb4dd8392639fca3d195..0b6f00d4459bb189cb502185e5a415f6962e42e0 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -115,23 +115,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0)));
                turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0)));
                SmartDashboard.putData("Turret Test Positions", turretTestChooser);
-
-               double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble();
-               double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET);
-
-               double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble();
-               double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET);
-
-               crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT);
-
-               double turretRot = crt.solve(leftAbs, rightAbs); 
-               
-               double motorRotations = turretRot * TurretConstants.GEAR_RATIO;
-
-               //Sets the initial motor position
-               inputs.positionDeg = turretRot;
-               motor.setPosition(motorRotations);
-
                //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
 
                motor.setPosition(0.0);
@@ -245,19 +228,6 @@ public class Turret extends SubsystemBase implements TurretIO{
                SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble());
                SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble());
 
-
-               double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble();
-               double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET);
-
-               double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble();
-               double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET);
-
-               crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT);
-
-               double turretRot = crt.solve(leftAbs, rightAbs); 
-
-               SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot));
-
                SmartDashboard.putBoolean("Turret Calibrated", !calibrating);
                SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint());
        }
@@ -324,11 +294,4 @@ public class Turret extends SubsystemBase implements TurretIO{
                calibrating = false;
                setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0);
        }
-
-       // in rotations
-       public Pair<Double, Double> getEncoderPositions() {
-               return new Pair<Double, Double>(
-                               encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET,
-                               encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
-       }
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 69486c390b1abd7a9bf9ddd32047a6be87789374..5dac346bebc5c85389862aa28abb4294ce9b789c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -4,8 +4,8 @@ import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 
 public class TurretConstants {
-    public static double MAX_ANGLE = 200; // Deg
-    public static double MIN_ANGLE = -200; // Deg
+    public static double MAX_ANGLE = 220; // Deg
+    public static double MIN_ANGLE = -220; // Deg
 
     public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop
 
@@ -17,21 +17,11 @@ public class TurretConstants {
     public static double TURRET_WIDTH = Units.inchesToMeters(6.4);
     public static double TURRET_RADIUS = TURRET_WIDTH / 2;
 
-    public static int TURRET_TEETH_COUNT = 140; // the turret teeth count
-    public static double GEAR_RATIO = 25.454545454;
-    public static int LEFT_ENCODER_TEETH = 15; // gear teeth
-    public static int RIGHT_ENCODER_TEETH = 22; // read above
-    public static int ENCODER_COUNT_TOTAL = 8192; // how many intervals it can have, like clicks on a clock chat gpt explained to me
-
-       // TODO: set these properly
-    public static double LEFT_ENCODER_OFFSET = 0.364502; //  rot
-    public static double RIGHT_ENCODER_OFFSET = 0.718506; //  rot
+    public static double GEAR_RATIO = 28;
 
     // Turret is in center of robot, but make use of the height in shooter physics
     public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters
 
-    public static double CRT_TOLERANCE = 0.01;
-
        public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06;
 
        public static final double FEEDFORWARD_KV = 0.185;