a

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 1d222583b429523101a58262541bbc0bb388848e..a8581f1d0aaa5c74584710311714a9921260c431 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -166,7 +166,7 @@ public class AutoShootCommand extends Command {
 
         /** Spindexer Stuff!! */
         if(spindexer != null){
-            spindexer.turnOnSpindexer();
+            spindexer.maxSpindexer();
         }
     }
 

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 53744dcec1217a09d4af18c85c32b862cdc708aa..33ed5eb9db80da30bc77e8cfe775213dd6bb20f0 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -89,6 +89,7 @@ public class Hood extends SubsystemBase implements HoodIO{
     @Override
     public void periodic() {
                updateInputs();
+
         State goalState = new State(
                                MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD),
                                goalVelocityRadPerSec);

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 0d784312bfeadc0124c6209cb1090fd227f2efb2..f811e3769048b0ad6844d84c98fba586e06dceac 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -8,6 +8,7 @@ import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -30,6 +31,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
@@ -46,8 +48,8 @@ public class Turret extends SubsystemBase implements TurretIO{
 
        /* ---------------- Constants ---------------- */
 
-       private static final double MIN_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MIN_ANGLE);
-       private static final double MAX_ANGLE_RAD = Units.degreesToRadians(TurretConstants.MAX_ANGLE);
+       private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-90);
+       private static final double MAX_ANGLE_RAD = Units.degreesToRadians(90);
 
        private static final double MAX_VEL_RAD_PER_SEC = 25;
        private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
@@ -58,8 +60,8 @@ public class Turret extends SubsystemBase implements TurretIO{
        //private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0);
        private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0);
 
-    private final DutyCycleEncoder encoderLeft = new DutyCycleEncoder(1);
-    private final DutyCycleEncoder encoderRight = new DutyCycleEncoder(2);
+    private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
 
     private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
 
@@ -69,7 +71,7 @@ public class Turret extends SubsystemBase implements TurretIO{
 
        /* ---------------- Hardware ---------------- */
 
-       private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.RIO_CAN);
+       private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
 
        private TalonFXSimState simState;
        private SingleJointedArmSim turretSim;
@@ -134,15 +136,15 @@ public class Turret extends SubsystemBase implements TurretIO{
                setpoint = new State(getPositionRad(), 0.0);
                lastGoalRad = setpoint.position;
 
-        EasyCRTConfig crt_cfg = new EasyCRTConfig(() -> Rotations.of(encoderLeft.get()), () -> Rotations.of(encoderRight.get()))
+        EasyCRTConfig crt_cfg = new EasyCRTConfig(() -> Rotations.of(encoderLeft.getPosition().getValueAsDouble()), () -> Rotations.of(encoderRight.getPosition().getValueAsDouble()))
         .withEncoderRatios(TurretConstants.LEFT_ENCODER_RATIO, TurretConstants.RIGHT_ENCODER_RATIO)
         .withAbsoluteEncoderOffsets(Degrees.of(TurretConstants.LEFT_ENCODER_OFFSET), Degrees.of(TurretConstants.RIGHT_ENCODER_OFFSET))
         .withMechanismRange(Degrees.of(TurretConstants.MIN_ANGLE), Degrees.of(TurretConstants.MAX_ANGLE))
-        .withMatchTolerance(Degrees.of(2)) // Tune this
-        .withAbsoluteEncoderInversions(false, false)
+        .withMatchTolerance(Degrees.of(3)) // Tune this
+        .withAbsoluteEncoderInversions(false, false);
         // shared drive gear / pinion (the tan gear/tiny first gear on motor shaft)
         // turret ring / shared drive pulley (big turret gear / first black belt gear weird thingy?)
-            .withCrtGearRecommendationInputs(10, 140.0/22.0);  // prob need to fix
+            //.withCrtGearRecommendationInputs(10, 140.0/22.0);  // prob need to fix
         
         this.easyCRT = new EasyCRT(crt_cfg);
         
@@ -150,6 +152,10 @@ public class Turret extends SubsystemBase implements TurretIO{
             motor.setPosition(angle.in(Rotations) * GEAR_RATIO);
         });
 
+               //motor.setPosition(motor.getPosition().getValueAsDouble() - Units.degreesToRotations(50.6) * GEAR_RATIO);
+
+               motor.setPosition(0.0); // Delete this when easy crt is working
+
                if (RobotBase.isSimulation()) {
                        simState = motor.getSimState();
                        turretSim = new SingleJointedArmSim(
@@ -164,6 +170,10 @@ public class Turret extends SubsystemBase implements TurretIO{
                }
 
                SmartDashboard.putData("Turret Mech", mech);
+
+               SmartDashboard.putData("Turret to 90", new InstantCommand(()-> setFieldRelativeTarget(new Rotation2d(Math.PI/2), 0.0)));
+               SmartDashboard.putData("Turret to -90", new InstantCommand(()-> setFieldRelativeTarget(new Rotation2d(-Math.PI/2), 0.0)));
+
        }
 
        /* ---------------- Public API ---------------- */
@@ -185,7 +195,6 @@ public class Turret extends SubsystemBase implements TurretIO{
 
        @Override
        public void periodic() {
-               updateInputs();
                
                double robotRelativeGoal = goalAngle.getRadians();
 
@@ -279,6 +288,12 @@ public class Turret extends SubsystemBase implements TurretIO{
                                Units.radiansToDegrees(targetVelocity));
                SmartDashboard.putNumber("Turret Position Deg",
                                Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO);
+
+               updateInputs();
+               Logger.processInputs("Turret", inputs);
+
+               // SmartDashboard.putNumber("Encoder Left", encoderLeft.get());
+               // SmartDashboard.putNumber("Encoder Right", encoderRight.get());
        }
 
        /* ---------------- Simulation ---------------- */
@@ -300,7 +315,7 @@ public class Turret extends SubsystemBase implements TurretIO{
                inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO;
                inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO;
                inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
-        inputs.encoderLeftRot = encoderLeft.get();
-        inputs.encoderRightRot = encoderRight.get();
+        inputs.encoderLeftRot = encoderLeft.getPosition().getValueAsDouble();
+        inputs.encoderRightRot = encoderRight.getPosition().getValueAsDouble();
        }
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index ac0a14d1e67100543ad44cacec21d9fb4d439ab9..3df6d108a4bb4d6df19ac8d025e28d7eeb8aac82 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -19,8 +19,8 @@ public class TurretConstants {
     public static double RIGHT_ENCODER_RATIO = 28.0/3.0; // The amount of times this encoder turns for every time the turret turns
     public static double ENCODER_COUNT_TOTAL = 8192.0; // how many intervals it can have, like clicks on a clock chat gpt explained to me
 
-    public static double LEFT_ENCODER_OFFSET = 0; // degrees
-    public static double RIGHT_ENCODER_OFFSET = 0; // degrees
+    public static double LEFT_ENCODER_OFFSET = Units.rotationsToDegrees(9.52); // degrees 9.52 rot
+    public static double RIGHT_ENCODER_OFFSET = Units.rotationsToDegrees(6.53); // degrees 6.53 rot
 
     public static Translation3d DISTANCE_FROM_ROBOT_CENTER = new Translation3d(0,0, Units.inchesToMeters(22.172)); //meters