set target commands

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

@@ -146,11 +146,18 @@ public RobotContainer() {
     // Set up auto routines
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
+    // Intake SysID Routines
+    autoChooser.addDefaultOption(
+        "Intake SysID (Dynamic Forward)",
+        intake.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addDefaultOption(
+        "Intake SysID (Dynamic Reverse)",
+        intake.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     autoChooser.addDefaultOption(
         "Intake SysID (Quasistatic Forward)",
         intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
     autoChooser.addDefaultOption(
-        "Intake SysID (Dynamic Forward)", intake.sysIdDynamic(SysIdRoutine.Direction.kForward));
+        "Intake SysID (Dynamic Reverse)", intake.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
     configureButtonBindings();
   }

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

@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotMap;
+import frc.robot.subsystems.Elevator.Elevator;
+import frc.robot.subsystems.Elevator.ElevatorExtenderIOSim;
+import frc.robot.subsystems.Elevator.ElevatorExtenderIOTalonFX;
+import frc.robot.subsystems.Elevator.ElevatorPivotIOSim;
+import frc.robot.subsystems.Elevator.ElevatorPivotIOTalonFX;
+
+public class SetExtenderTarget extends Command {
+  /** Creates a new ExtendElevator. */
+  private final Elevator elevator;
+
+  private double setPoint;
+
+  public SetExtenderTarget(double setPoint, Elevator elevator) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    //addRequirements(elevator);
+    setPoint = this.setPoint;
+    this.elevator = elevator;
+
+  }
+
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    elevator.setExtenderGoal(setPoint);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevator.elevatorStop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.extenderAtSetpoint();
+  }
+}

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

@@ -0,0 +1,38 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotMap;
+import frc.robot.subsystems.Elevator.Elevator;
+import frc.robot.subsystems.Elevator.ElevatorExtenderIOSim;
+import frc.robot.subsystems.Elevator.ElevatorExtenderIOTalonFX;
+import frc.robot.subsystems.Elevator.ElevatorPivotIOSim;
+import frc.robot.subsystems.Elevator.ElevatorPivotIOTalonFX;
+
+public class SetPivotTarget extends Command {
+  /** Creates a new SetPivotTarget. */
+  private final Elevator elevator;
+  private double setPoint;
+  public SetPivotTarget(double setPoint, Elevator elevator) {
+    setPoint = this.setPoint;
+     this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  public void initialize() {
+    elevator.setPivotGoal(setPoint);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.pivotAtSetpoint();
+  }
+}

src/main/java/frc/robot/subsystems/Elevator/ElevatorPivotIO.java

@@ -10,6 +10,9 @@ public static class ElevatorPivotIOInputs {
     public double currentAmps = 0;
     public double appliedVolts = 0;
     public double positionSetpoint = 0;
+
+    public boolean gyroConnected = false;
+    public double pitch = 0;
   }
 
   public default void updateInputs(ElevatorPivotIOInputs inputs) {}

src/main/java/frc/robot/subsystems/Elevator/ElevatorPivotIOTalonFX.java

@@ -1,13 +1,16 @@
 package frc.robot.subsystems.Elevator;
 
 import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -18,14 +21,17 @@ public class ElevatorPivotIOTalonFX implements ElevatorPivotIO {
   private final TalonFX leader;
   private final TalonFX follower;
 
+  private final Pigeon2 pigeon;
+
   private double positionSetpoint;
 
   private final StatusSignal<Double> pivotPosition;
   private final StatusSignal<Double> pivotVelocity;
   private final StatusSignal<Double> appliedVolts;
   private final StatusSignal<Double> currentAmps;
+  private final StatusSignal<Double> pitch;
 
-  public ElevatorPivotIOTalonFX(int leadID, int followID) {
+  public ElevatorPivotIOTalonFX(int leadID, int followID, int gyroID) {
     TalonFXConfiguration config = new TalonFXConfiguration();
     config.CurrentLimits.StatorCurrentLimit = Constants.ElevatorConstants.PIVOT_CURRENT_LIMIT;
     config.CurrentLimits.StatorCurrentLimitEnable =
@@ -34,6 +40,9 @@ public ElevatorPivotIOTalonFX(int leadID, int followID) {
     config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
     leader = new TalonFX(leadID);
     follower = new TalonFX(followID);
+    pigeon = new Pigeon2(gyroID);
+
+    pigeon.getConfigurator().apply(new Pigeon2Configuration());
 
     leader.getConfigurator().apply(config);
 
@@ -44,15 +53,23 @@ public ElevatorPivotIOTalonFX(int leadID, int followID) {
     appliedVolts = leader.getMotorVoltage();
     currentAmps = leader.getStatorCurrent();
 
+    pitch = pigeon.getRoll();
+
     positionSetpoint = Constants.ElevatorConstants.PIVOT_STOW;
 
+    pigeon.optimizeBusUtilization();
+    leader.optimizeBusUtilization();
+    follower.optimizeBusUtilization();
+
     BaseStatusSignal.setUpdateFrequencyForAll(
         100, pivotPosition, pivotVelocity, appliedVolts, currentAmps);
   }
 
   @Override
   public void updateInputs(ElevatorPivotIOInputs inputs) {
-    BaseStatusSignal.refreshAll(pivotPosition, pivotVelocity, appliedVolts, currentAmps);
+    BaseStatusSignal.refreshAll(pivotPosition, pivotVelocity, appliedVolts, currentAmps, pitch);
+    inputs.gyroConnected = BaseStatusSignal.refreshAll(pitch).equals(StatusCode.OK);
+    inputs.pitch = pitch.getValueAsDouble();
     inputs.pivotPosition = Units.rotationsToDegrees(pivotPosition.getValueAsDouble());
     inputs.pivotVelocity =
         Units.rotationsPerMinuteToRadiansPerSecond(pivotVelocity.getValueAsDouble());