Added relay command, and many attempts to fix auto align

lib/RoboLib

@@ -1 +1 @@
-Subproject commit cd9dee709893a27de40fb86e441223dd4c068b12
+Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -7,17 +12,20 @@
           "incKey": 68
         },
         {
-          "decKey": 87,
+          "decKey": 83,
-          "incKey": 83
+          "incKey": 87
         },
         {
-          "decKey": 69,
+          "decayRate": 0.009999999776482582,
-          "decayRate": 0.0,
-          "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -91,7 +99,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000004c050000cc09000011810000"
+      "guid": "Keyboard0"
     }
   ]
 }

src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto

@@ -9,6 +9,25 @@
           "data": {
             "pathName": "S. Hub. shoot 8. E-Hub"
           }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 10.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ChassisAlignShoot"
+                }
+              }
+            ]
+          }
         }
       ]
     }

src/main/deploy/pathplanner/paths/M. Depot. .path

@@ -36,7 +36,7 @@
         "y": 7.138469879518072
       },
       "prevControl": {
-        "x": 0.4124120733073493,
+        "x": 0.41241207330734936,
         "y": 6.876706525398199
       },
       "nextControl": {

src/main/deploy/pathplanner/paths/Relay. R-Tr..path

@@ -37,7 +37,7 @@
       },
       "prevControl": {
         "x": 7.691609840125085,
-        "y": 7.387443181134998
+        "y": 7.387443181134999
       },
       "nextControl": {
         "x": 7.64634196710383,
@@ -56,7 +56,7 @@
         "y": 7.138469879518072
       },
       "nextControl": {
-        "x": 6.511975903614461,
+        "x": 6.511975903614458,
         "y": 7.138469879518072
       },
       "isLocked": false,
@@ -85,11 +85,11 @@
       },
       "prevControl": {
         "x": 6.020824888067715,
-        "y": 0.8531028227756468
+        "y": 0.8531028227756466
       },
       "nextControl": {
         "x": 5.667271497474452,
-        "y": 1.2066562133689314
+        "y": 1.2066562133689316
       },
       "isLocked": false,
       "linkedName": null

src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path

@@ -32,12 +32,12 @@
     },
     {
       "anchor": {
-        "x": 3.473,
+        "x": 2.9483700538099855,
-        "y": 4.013
+        "y": 4.00221686746988
       },
       "prevControl": {
-        "x": 3.1233132530120478,
+        "x": 2.5986833068220334,
-        "y": 4.034855421686746
+        "y": 4.024072289156626
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path

@@ -24,7 +24,7 @@
         "y": 3.77441480327739
       },
       "nextControl": {
-        "x": 2.348744281004639,
+        "x": 2.3487442810046395,
         "y": 4.0114647147949
       },
       "isLocked": false,

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

@@ -4,6 +4,12 @@
 
 package frc.robot;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 
@@ -76,6 +82,19 @@ public final class Constants {
     public static final int BACK_RIGHT_DRIVE_ID = 42;
     public static final int BACK_RIGHT_ENCODER_ID = 43;
   }
+
+  public static class RobotChassisLimits{
+    // Max velocity in meters per second
+    public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
+    // In meters per second squared
+    public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
+
+    // In radians per second
+    public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
+    // In radians per second squared
+    public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
+  }
+
   //Bird
   public static class ExtraIDEntities {
     public static final int PIGEON_ID = 1;
@@ -121,4 +140,12 @@ public final class Constants {
     public static final int SPINDEXER_MOTOR_ID = 8;
     public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
   }
+
+  public static class FieldPoses {
+    private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+    public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
+    public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
+
+  }
 }

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

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -11,10 +13,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.AutoShootCommandChassisTurret;
+import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
 import frc.robot.commands.AutoTurretCommand;
 import frc.robot.commands.ManualHopperCommand;
 import frc.robot.commands.ManualIntake;
 import frc.robot.commands.ManualShootCommand;
+import frc.robot.commands.RelayCommand;
 import frc.robot.commands.SetDeflectorCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
@@ -45,11 +49,14 @@ public class RobotContainer {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
 
-    turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
+    // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
     // turretSubsystem.homeTurret();
 
     // Configure the trigger bindings
     configureBindings();
+
+    NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+
   }
 
   /**
@@ -68,8 +75,14 @@ public class RobotContainer {
     m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
     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 AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+
+    m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
     m_driverController.rightBumper().onTrue(intakeSubsystem.intake());
-    m_driverController.y().onTrue(intakeSubsystem.deploy());
+    m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
+    m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
     m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
   }
 
@@ -80,6 +93,6 @@ public class RobotContainer {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return Commands.parallel(Commands.run(()-> {/*turretSubsystem.homeTurret();*/}, turretSubsystem),  driveSubsystem.getAutoCommand());
+    return Commands.parallel(intakeSubsystem.deploy(), Commands.run(()-> {turretSubsystem.homeTurret();}, turretSubsystem),  driveSubsystem.getAutoCommand());
   }
 }

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

@@ -75,6 +75,7 @@ public class AutoShootCommandChassisTurret extends Command{
         });
 
         turretSubsystem.setLauncher(5500);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
         hopperSubsystem.spindexerOn();
         cameraAutoCenterController.calculate(-100);
 
@@ -91,8 +92,12 @@ public class AutoShootCommandChassisTurret extends Command{
         Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
         Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
 
-        // Add 180 because we want the back of the robot to face the HUB
+        DriverStation.getAlliance().ifPresent((aliance) -> {
-        deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
+            if (aliance == Alliance.Blue) {
+                // Add 180 because we want the back of the robot to face the HUB
+                deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
+            }
+        });
     }
 
     @Override

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

@@ -0,0 +1,130 @@
+package frc.robot.commands;
+
+import java.util.Arrays;
+import java.util.List;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoShootCommandChassisTurretPathPlanner extends Command {
+
+    // For Chassis Control
+    private DriveSubsystem driveSubsystem;
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+
+    private int[] tagFilters;
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+    private Pose3d closestTag;
+    private Pose2d targetPPPose;
+
+    private Command pathFollowCommand;
+
+    public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem,
+            TurretSubsystem turretSubsystem) {
+        this.driveSubsystem = driveSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
+
+    }
+
+    @Override
+    public void initialize() {
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            Pose3d targetHub;
+            if (aliance == Alliance.Blue) {
+                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+                targetHub = Constants.FieldPoses.BLUE_HUB_POSE;
+            } else {
+                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+                targetHub = Constants.FieldPoses.RED_HUB_POSE;
+            }
+
+            Pose3d botPose = driveSubsystem.getBotPose();
+            // Find closest target to us
+            List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get())
+                    .toList();
+            closestTag = botPose.nearest(tagPoses);
+            Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag);
+
+            Translation2d delta = botPose.getTranslation().toTranslation2d()
+                    .minus(closestTag.getTranslation().toTranslation2d());
+            Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta);
+
+            // Add 180 because we want the back of the robot to face the HUB
+            // deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
+
+            // Translate back 1 meter from HUB
+            Translation2d newTrans = targetHub.getTranslation().toTranslation2d()
+                    .plus(new Translation2d(1.5, delta.getAngle()));
+            targetPPPose = new Pose2d(newTrans, delta.getAngle());
+            Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose);
+
+            pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose,
+                    new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
+                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL,
+                            Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY,
+                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL));
+
+            CommandScheduler.getInstance().schedule(pathFollowCommand);
+        });
+
+        turretSubsystem.setLauncher(5500);
+        hopperSubsystem.spindexerOn();
+
+    }
+
+    @Override
+    public void execute() {
+
+        // If robot is centered on HUB and shooter is up to speed. Run the kicker
+        if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) {
+            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
+            hopperSubsystem.feedKicker();
+        } else {
+            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        pathFollowCommand.cancel();
+        turretSubsystem.setLauncher(0);
+
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+
+        driveSubsystem.stopDrive();
+
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
+                Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
+    }
+
+}

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

@@ -29,8 +29,6 @@ public class AutoTurretCommand extends Command {
 
     private int[] shootingTagFilters;
 
-    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
-
     private Pose3d closestTag;
     private Pose3d scoringHubPose;
 
@@ -49,14 +47,10 @@ public class AutoTurretCommand extends Command {
         DriverStation.getAlliance().ifPresent((aliance) -> {
             if (aliance == Alliance.Blue) {
                 shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
-
+                scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE;
-                scoringHubPose = fieldTags.getTagPose(26).get();
-                scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
             } else {
                 shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
-
+                scoringHubPose = Constants.FieldPoses.RED_HUB_POSE;
-                scoringHubPose = fieldTags.getTagPose(10).get();
-                scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
             }
             Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose);
         });
@@ -76,7 +70,7 @@ public class AutoTurretCommand extends Command {
 
 
         if(turretSubsystem.isTurretReady()){
-            turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
+            turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
             // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
         }
 

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

@@ -0,0 +1,59 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class RelayCommand extends Command{
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+    private IntakeSubsystem intakeSubsystem;
+
+    private Command waitCommand = Commands.waitSeconds(1.5);
+
+    public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+
+        addRequirements(hopperSubsystem, turretSubsystem, intakeSubsystem);
+    }
+    
+    @Override
+    public void initialize() {
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
+        turretSubsystem.setLauncher(5500);
+        hopperSubsystem.spindexerOn();
+        CommandScheduler.getInstance().schedule(waitCommand);
+    }
+
+    @Override
+    public void execute() {
+        if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
+            hopperSubsystem.feedKicker();
+        }else{
+            hopperSubsystem.stopKicker();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        waitCommand.cancel();
+        hopperSubsystem.spindexerOff();
+        hopperSubsystem.stopKicker();
+
+        turretSubsystem.setLauncher(0);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+    }
+
+}

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

@@ -10,6 +10,8 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
 import org.littletonrobotics.junction.Logger;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
@@ -50,8 +52,8 @@ public class DriveSubsystem extends TestableSubsystem {
         driveSystem = swerveBuilder
                 .withCanbusName("Drive")
                 .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
-                .withMaxLinearVelocity(3.0)
+                .withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY)
-                .withMaxAngularVelocity(Math.PI)
+                .withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY)
                 .withSwerveModule(
                         swerveBuilder.new SwerveModuleBuilder()
                                 .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
@@ -135,7 +137,7 @@ public class DriveSubsystem extends TestableSubsystem {
 
 
         // Set vision measurement confidence values
-        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7));
+        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
 
         this.setDefaultCommand(Commands.run(new Runnable() {
 
@@ -163,8 +165,8 @@ public class DriveSubsystem extends TestableSubsystem {
                     driveSystem::getRobotRelativeSpeeds,
                     (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
                     new PPHolonomicDriveController(
-                            new PIDConstants(5.0, 0, 0),
+                            new PIDConstants(1.5, 0, 0),
-                            new PIDConstants(5.0, 0, 0)),
+                            new PIDConstants(Math.PI/2, 0, 0)),
                     config,
                     () -> {
                         var alliance = DriverStation.getAlliance();

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

@@ -41,21 +41,18 @@ public class IntakeSubsystem extends TestableSubsystem {
     return Commands.sequence(
       Commands.deadline(
         Commands.waitSeconds(2),
-        this.runEnd(
+        this.runOnce(
-          () -> {deployMotor.set(0.5);}, // Bump this speed up if intake doesn't deploy
+          () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
-          () -> {deployMotor.stopMotor();} 
         )
       ),
-      Commands.deadline(
+      this.runOnce(() -> deployMotor.stopMotor())
-        Commands.waitSeconds(2),
-        this.runEnd(
-          () -> { deployMotor.set(-0.5); }, // Same here
-          () -> { deployMotor.stopMotor(); }
-        )
-      )
     );
   }
 
+  public Command resetDeploy(){
+    return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
+  }
+
   public void setSpeed(double speed)
   {
     intakeMotor.set(speed);
@@ -63,7 +60,7 @@ public class IntakeSubsystem extends TestableSubsystem {
 
   public Command intake()
   {
-    return this.runOnce(() -> {if(intakeMotor.get()!=0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
+    return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
   }
 
   public Command reverseIntake()

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

@@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
       new TalonFXConfiguration()
         .withSlot0(
           new Slot0Configs()
-            .withKP(2)
+            .withKP(3)
             .withKI(1)
             .withKD(0)
-            .withKS(0)
+            .withKS(0.3)
         )
         .withMotorOutput(
           new MotorOutputConfigs()
@@ -134,11 +134,6 @@ public class TurretSubsystem extends TestableSubsystem {
     return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1;
   }
 
-  public void turretControl(double position)
-  {
-      turretMotor.setControl(new PositionVoltage(position));
-  }
-
   public void setTargetElevation(double elev)
   {
     double pos = Math.abs((elev - 300)/300);
@@ -161,6 +156,9 @@ public class TurretSubsystem extends TestableSubsystem {
 
   public void homeTurret()
   {
+    if(turretState != TurretState.INIT){
+      return; // Early return, we don't want to home if we already have
+    }
     turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
     turretState = TurretState.HOMING;
   }
@@ -193,7 +191,7 @@ public class TurretSubsystem extends TestableSubsystem {
       if(turretLimitSwitch.get())
       { // Turret has tripped the switch
         turretMotor.stopMotor();
-        turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
+        turretOffset = turretMotorPosition.refresh().getValue().minus(Angle.ofRelativeUnits(65, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
         turretState = TurretState.READY;
         Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
         Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
@@ -201,12 +199,13 @@ public class TurretSubsystem extends TestableSubsystem {
         Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
         turretMotor.getConfigurator().apply(
           new SoftwareLimitSwitchConfigs()
-            .withForwardSoftLimitEnable(true)
+            // .withForwardSoftLimitEnable(true)
             .withForwardSoftLimitThreshold(forwardLimit)
-            .withReverseSoftLimitEnable(true)
+            // .withReverseSoftLimitEnable(true)
             .withReverseSoftLimitThreshold(reverseLimit)
         );
-        //setTurretRotation(Angle.ofRelativeUnits(180, Units.Degrees));
+        // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
+        setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees));
       }
     }
 
@@ -226,7 +225,16 @@ public class TurretSubsystem extends TestableSubsystem {
 
   }
 
-  public void setTurretRotation(Angle pos)
+  public void setTurretFieldRelative(Angle pos)
+  {
+    if(turretState != TurretState.READY)
+    {
+      // Not ready to set angle
+      return;
+    }
+  }
+
+  public void setTurretRobotRelative(Angle pos)
   {
     if(turretState != TurretState.READY)
     {
@@ -237,8 +245,8 @@ public class TurretSubsystem extends TestableSubsystem {
     setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
   }
 
-  public void setTurretMotorRotation(Angle pos){
+  private void setTurretMotorRotation(Angle pos){
-    turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
+    turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1)));
     turretSim.setRawRotorPosition(pos.plus(turretOffset));
   }