pivot

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

@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.commands.PivotCommands;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.intake.*;
 import frc.robot.subsystems.pivot.*;
@@ -143,6 +144,9 @@ private void configureButtonBindings() {
     operator.x().toggleOnTrue(shooter.conveyorOnCommand());
     operator.y().toggleOnTrue(shooter.conveyorOffCommand());
 
+    pivot.setDefaultCommand(
+        PivotCommands.basicOperatorControl(
+            pivot, () -> operator.getRightX() - operator.getLeftX()));
   }
 
   /**

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

@@ -0,0 +1,24 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.pivot.Pivot;
+import java.util.function.DoubleSupplier;
+
+public class PivotCommands {
+  private PivotCommands() {}
+
+  public static Command basicOperatorControl(Pivot pivot, DoubleSupplier rotationSupplier) {
+    return Commands.run(
+        () -> {
+          var target =
+              pivot
+                  .getCurrentPosition()
+                  .plus(Rotation2d.fromDegrees(rotationSupplier.getAsDouble()));
+
+          pivot.setTargetPosition(target);
+        },
+        pivot);
+  }
+}

src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java

@@ -1,8 +1,7 @@
 package frc.robot.subsystems.intake;
 
-import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
-
+import com.revrobotics.CANSparkMax;
 import frc.robot.Constants.IntakeConstants;
 
 public class IntakeIOReal implements IntakeIO {

src/main/java/frc/robot/subsystems/pivot/Pivot.java

@@ -3,19 +3,40 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.PivotConstants;
+import org.littletonrobotics.junction.Logger;
 
 public class Pivot extends SubsystemBase {
   private final PivotIO io;
+  private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged();
 
   public Pivot(PivotIO io) {
     this.io = io;
   }
 
   public void setTargetPosition(Rotation2d target) {
+    if (target.getRadians() < PivotConstants.pivotStart.getRadians()) {
+      target = PivotConstants.pivotStart;
+    }
+
+    if (target.getRadians() > PivotConstants.pivotEnd.getRadians()) {
+      target = PivotConstants.pivotEnd;
+    }
+
+    // surely this isnt necessary
     if (target.getRadians() < PivotConstants.pivotStart.getRadians()
         || target.getRadians() > PivotConstants.pivotEnd.getRadians()) {
       throw new IllegalArgumentException("Target position is out of bounds");
     }
     io.setTargetPosition(target);
   }
+
+  public Rotation2d getCurrentPosition() {
+    return inputs.pivotCurentAngle;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Pivot", inputs);
+  }
 }

src/main/java/frc/robot/subsystems/pivot/PivotIO.java

@@ -5,7 +5,7 @@
 
 public interface PivotIO {
   @AutoLog
-  class PivotIOInputs {
+  public static class PivotIOInputs {
     public Rotation2d pivotCurentAngle = new Rotation2d();
     public Rotation2d pivotTargetAngle = new Rotation2d();
   }

src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java

@@ -1,5 +1,5 @@
 package frc.robot.subsystems.shooter;
 
 public class ShooterIOSim implements ShooterIO {
-  //todo
+  // todo
 }