a

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index ac05b79dfbd810b64d3f0783610749c325b1c640..e9abb0bc6b0b0df2d256d80195db08130cdb09de 100644 (file)
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -74,7 +74,7 @@ public class Robot extends LoggedRobot {
         //   SimGUI: Persistent Values, Preferences, RobotId, then restart Simulation
         //     changes networktables.json, networktables.json.bck (both Untracked)
         //   Uncomment the next line, set the desired RobotId, deploy, and then comment the line out
-        //  RobotId.setRobotId(RobotId.SwerveCompetition);
+          RobotId.setRobotId(RobotId.TestBed2);
         DriveConstants.update(RobotId.getRobotId());
         RobotController.setBrownoutVoltage(6.0);
         // obtain this robot's identity

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0c8b25d71b910c6d842bf5e61aeee19c9ef38153..c7c8cb7a0048229f2a2ffbbd0b08c7956bc6bde4 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -75,6 +75,7 @@ public class RobotContainer {
         break;
 
       case TestBed2:
+        turret = new Turret();
         break;
 
       default:

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index e33b325d0b5cd62f1058cc2ae05a9ea6ddce4b3e..fac3856aa1b069608f1d0af2e624caa921cce7f9 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -50,8 +50,8 @@ public class Turret extends SubsystemBase implements TurretIO{
        /* ---------------- Hardware ---------------- */
 
        private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
-       private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
-    private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
+       private final CANcoder encoderLeft = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private final CANcoder encoderRight = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
 
        private TalonFXSimState simState;
        private SingleJointedArmSim turretSim;
@@ -106,22 +106,30 @@ public class Turret extends SubsystemBase implements TurretIO{
                                        false,
                                        0.0);
                }
+               // Do this for both encoders in the constructor
+               double leftPosition = encoderLeft.getAbsolutePosition().getValueAsDouble();
 
-               double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET);
-               double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
+               double leftAbs = wrapUnit(leftPosition - TurretConstants.LEFT_ENCODER_OFFSET);
+
+               double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble();
+
+               double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET);
 
                int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH)
                                % TurretConstants.LEFT_ENCODER_TEETH;
+               SmartDashboard.putNumber("Left Tooth", leftTooth);
 
                int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH)
                                % TurretConstants.RIGHT_ENCODER_TEETH;
+               SmartDashboard.putNumber("Right Tooth", rightTooth);
 
                int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH);
+               SmartDashboard.putNumber("Turret Index", turretIndex);
 
                double totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
         * TurretConstants.RIGHT_ENCODER_TEETH;
 
-               double turretRotations = turretIndex / (double) totalTeeth;
+               double turretRotations = turretIndex / (double) 140.0;
 
                double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
                motor.setPosition(motorRotations);
@@ -208,6 +216,11 @@ public class Turret extends SubsystemBase implements TurretIO{
                updateInputs();
                Logger.processInputs("Turret", inputs);
 
+               SmartDashboard.putNumber("Turret position", Units.radiansToDegrees(getPositionRad()));
+               SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble());
+               SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble());
+
+
        }
 
        /* ---------------- Simulation ---------------- */

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 10fee61e04ec1ff88e2b2aaaea7c4fd4fe040d06..86fe6fd805ad60973b8657a9b6ec5704494f410b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -14,13 +14,13 @@ public class TurretConstants {
     public static double TURRET_RADIUS = TURRET_WIDTH / 2;
 
     public static double TURRET_TEETH_COUNT = 140.0; // the turret teeth count
-    public static double TURRET_GEAR_RATIO = 36.81818182;
+    public static double TURRET_GEAR_RATIO = 38.81818182;
     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
 
-    public static double LEFT_ENCODER_OFFSET = 9.52; // degrees 9.52 rot
-    public static double RIGHT_ENCODER_OFFSET = 6.53; // degrees 6.53 rot
+    public static double LEFT_ENCODER_OFFSET = 0.463379; //  rot
+    public static double RIGHT_ENCODER_OFFSET = 0.197266; //  rot
 
     public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters