Eh

lib/RoboLib

@@ -1 +1 @@
-Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51
+Subproject commit 840c851253187081a50ca9492c032cec8d376e7b

src/main/java/frc/robot/Constants.java

@@ -129,7 +129,7 @@ public final class Constants {
     public static final double DEFLECTOR_STORED_ANGLE = 0;
     public static final double DEFLECTOR_SET_ANGLE1 = 100;
     public static final double DEFLECTOR_SET_ANGLE2 = 200;
-    public static final double DEFLECTOR_SET_ANGLE3 = 300;
+    public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
     public static final double DEFLECTOR_SERVO_RATIO = 1.0;
   }
   

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

@@ -76,8 +76,8 @@ public class RobotContainer {
     m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
 
     m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
-    // m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
+    //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
-    m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+    // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
 
     m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
     m_driverController.rightBumper().onTrue(intakeSubsystem.intake());

src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java

@@ -54,12 +54,12 @@ public class AutoShootCommandChassisTurret extends Command{
 
         this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
 
-        cameraAutoCenterController = new PIDController(0.1, 0, 0.0);
+        cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
-        cameraAutoCenterController.setTolerance(0.2); // Units is meters
+        cameraAutoCenterController.setTolerance(0.2); // Units degrees
         cameraAutoCenterController.setSetpoint(0);
 
 
-        deadreckoningAutoCenterController = new PIDController(Math.PI, 0, 0.0);
+        deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0);
         deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations
         deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI);
     }
@@ -80,7 +80,7 @@ public class AutoShootCommandChassisTurret extends Command{
         cameraAutoCenterController.calculate(-100);
 
         // Change to HUB tracking pipeline
-        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER);
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
         LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
 
         Pose3d botPose = driveSubsystem.getBotPose();
@@ -106,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{
             Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
 
             Pose3d botPose = driveSubsystem.getBotPose();
-            driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
+            //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
         }
         else // We have an april tag, center it to the camera frame
         {
@@ -114,6 +114,11 @@ public class AutoShootCommandChassisTurret extends Command{
             // [tx, ty, tz, pitch, yaw, roll]
             double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
             if(targetsPose.length >= 1){
+                Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
+
+                // Calculate turret Angle
+                double targetDistance = targetsPose[2];
+
                 driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
             }
         }

src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java

@@ -115,7 +115,7 @@ public class AutoShootCommandChassisTurretPathPlanner extends Command {
 
     @Override
     public void end(boolean interrupted) {
-        pathFollowCommand.cancel();
+        CommandScheduler.getInstance().cancel(pathFollowCommand);
         turretSubsystem.setLauncher(0);
 
         hopperSubsystem.stopKicker();

src/main/java/frc/robot/commands/AutoTurretCommand.java

@@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command {
 
         Pose3d botPose = driveSubsystem.getBotPose();
         Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
-                new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngle().in(Units.Rotations)))));
+                new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative().in(Units.Rotations)))));
         Logger.recordOutput("AutoTurret-TurretPose", turretPose);
 
         Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());

src/main/java/frc/robot/commands/RelayCommand.java

@@ -1,5 +1,7 @@
 package frc.robot.commands;
 
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -30,6 +32,7 @@ public class RelayCommand extends Command{
         turretSubsystem.setLauncher(5500);
         hopperSubsystem.spindexerOn();
         CommandScheduler.getInstance().schedule(waitCommand);
+        turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(-6, Units.Degrees)); // Return to 0 after MIAMI VALLEY
     }
 
     @Override
@@ -54,6 +57,7 @@ public class RelayCommand extends Command{
 
         turretSubsystem.setLauncher(0);
         turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+        turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY
     }
 
 }

src/main/java/frc/robot/subsystems/HopperSubsystem.java

@@ -7,6 +7,7 @@ package frc.robot.subsystems;
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
 
+import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 
@@ -21,6 +22,8 @@ public class HopperSubsystem extends TestableSubsystem {
   public HopperSubsystem() {
     spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
     spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION));
+    // spindexerMotor.getConfigurator().apply(new ClosedLoopGeneralConfigs()
+    //   .);
 
     kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
     kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION));

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -75,7 +75,7 @@ public class TurretSubsystem extends TestableSubsystem {
         .withSlot0(
           new Slot0Configs()
             .withKP(3)
-            .withKI(1)
+            .withKI(1.5)
             .withKD(0)
             .withKS(0.3)
         )
@@ -172,7 +172,7 @@ public class TurretSubsystem extends TestableSubsystem {
     turretMotor.set(speed);
   }
 
-  public Angle getTurretAngle()
+  public Angle getTurretAngleRobotRelative()
   {
     return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
   }
@@ -200,12 +200,12 @@ public class TurretSubsystem extends TestableSubsystem {
         turretMotor.getConfigurator().apply(
           new SoftwareLimitSwitchConfigs()
             // .withForwardSoftLimitEnable(true)
-            .withForwardSoftLimitThreshold(forwardLimit)
+            // .withForwardSoftLimitThreshold(forwardLimit)
             // .withReverseSoftLimitEnable(true)
-            .withReverseSoftLimitThreshold(reverseLimit)
+            // .withReverseSoftLimitThreshold(reverseLimit)
         );
         // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
-        setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees));
+        setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY
       }
     }
 
@@ -214,10 +214,11 @@ public class TurretSubsystem extends TestableSubsystem {
       Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
       Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
       Logger.recordOutput("TurretSubsystem-TurretState", turretState);
-      Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative().in(Units.Degrees));
       Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees));
       Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
-      Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative().in(Units.Degrees));
       Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees));
       Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
       Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
@@ -245,11 +246,15 @@ public class TurretSubsystem extends TestableSubsystem {
     setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
   }
 
-  private void setTurretMotorRotation(Angle pos){
+  public void setTurretMotorRotation(Angle pos){
     turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1)));
     turretSim.setRawRotorPosition(pos.plus(turretOffset));
   }
 
+  private Angle getRequestedPosTurretRelative() {
+    return turretControlMode.getPositionMeasure().minus(turretOffset).div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
+  }
+
   @PostTest()
   public void TurretSubsystem_CANDevicesConnected(){
     assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();