From 76a9e3b1fcf013c7495df2e91d726fcf05ef5bb3 Mon Sep 17 00:00:00 2001
From: Rohan Godha <rgodha24@brophybroncos.org>
Date: Sat, 24 Feb 2024 08:32:25 -0700
Subject: [PATCH] pivot

---
 src/main/java/frc/robot/RobotContainer.java   |  4 ++++
 .../frc/robot/commands/PivotCommands.java     | 24 +++++++++++++++++++
 .../robot/subsystems/intake/IntakeIOReal.java |  3 +--
 .../frc/robot/subsystems/pivot/Pivot.java     | 21 ++++++++++++++++
 .../frc/robot/subsystems/pivot/PivotIO.java   |  2 +-
 .../subsystems/shooter/ShooterIOSim.java      |  2 +-
 6 files changed, 52 insertions(+), 4 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/PivotCommands.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d7dc536..eebe1aa 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/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()));
   }
 
   /**
diff --git a/src/main/java/frc/robot/commands/PivotCommands.java b/src/main/java/frc/robot/commands/PivotCommands.java
new file mode 100644
index 0000000..6ed66e0
--- /dev/null
+++ b/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);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index 0d58de0..89cfb3d 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/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 {
diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java
index f23ff5f..7f53cc3 100644
--- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java
+++ b/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);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java
index 7ddb5d1..2d35b9a 100644
--- a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java
+++ b/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();
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java
index 5b8f314..881cda0 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java
+++ b/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
 }