climb pivot + algae flywheel

src/main/java/frc/robot/BuildConstants.java

@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "robot-code-2025";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 194;
-  public static final String GIT_SHA = "72e5a7017f4b898cc0611c8042a9a1ac9896f999";
-  public static final String GIT_DATE = "2025-02-21 16:04:44 EST";
-  public static final String GIT_BRANCH = "devbot";
-  public static final String BUILD_DATE = "2025-02-22 11:29:28 EST";
-  public static final long BUILD_UNIX_TIME = 1740241768263L;
+  public static final int GIT_REVISION = 71;
+  public static final String GIT_SHA = "ccc81fce327e5f5b1ce5df44b6b91796d3bcb76e";
+  public static final String GIT_DATE = "2025-02-18 20:25:25 EST";
+  public static final String GIT_BRANCH = "coral-pivot";
+  public static final String BUILD_DATE = "2025-02-22 16:40:54 EST";
+  public static final long BUILD_UNIX_TIME = 1740260454695L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/frc/robot/subsystems/climbpivot/ClimbPivot.java

@@ -0,0 +1,96 @@
+// 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.subsystems.climbPivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.climbpivot.ClimbPivotInputsAutoLogged;
+
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
+
+/** This */
+public class ClimbPivot extends SubsystemBase {
+  /** Creates a new ClimbPivot. */
+  ClimbPivotInterface climbPivotInterface;
+
+  ClimbPivotInputsAutoLogged inputsAutoLogged = new ClimbPivotInputsAutoLogged();
+
+  public ClimbPivot(ClimbPivotInterface climbPivotInterface) {
+    this.climbPivotInterface = climbPivotInterface;
+  }
+
+  /** angle is in rotations, idk where the angles are or what 0 is */
+  public void setClimbPivotPosition(double angle) {
+    climbPivotInterface.setClimbPivotPosition(angle);
+  }
+
+  /** in rotations, i think that's a problem y'all */
+  public double getClimbPivotPosition() {
+    return climbPivotInterface.getClimbPivotPosition();
+  }
+
+  public double getVolts() {
+    return climbPivotInterface.getVolts();
+  }
+
+  /**
+   * in rotations, endAngle is something i guess i need help
+   *
+   * @param endAngle this is the angle where the pivot ends
+   */
+  public void stop(double endAngle) {
+    climbPivotInterface.setClimbPivotPosition(endAngle);
+  }
+
+  /**
+   * Sets the pivot motor speed manually from -1 to 1
+   *
+   * @param speed the percent speed from -1 to 1
+   */
+  public void manualPivot(double speed) {
+    climbPivotInterface.manualPivot(speed);
+  }
+
+  @Override
+  public void periodic() {
+    climbPivotInterface.updateInputs(inputsAutoLogged);
+    Logger.processInputs("Climb Pivot/", inputsAutoLogged);
+  }
+
+  /**
+   * actually pivots the thing to a set angle
+   *
+   * @param setAngle the command that sets it to the angle
+   * @return
+   */
+  public Command setAngle() {
+    return new StartEndCommand(
+            () -> setClimbPivotPosition(PivotConstants.TARGET_POSITION),
+            () -> stop(PivotConstants.TARGET_POSITION),
+            this)
+        .until(() -> isPivotCloseEnough());
+  }
+
+  /**
+   * @param position i dont actually know what this is
+   * @return
+   */
+  public Command manualPivotClimb(DoubleSupplier position) {
+    return new RunCommand(() -> climbPivotInterface.manualPivot(position.getAsDouble()), this);
+  }
+
+  /**
+   * Private helper method to check if the pivot is close enough to the target position
+   *
+   * @return
+   */
+  private boolean isPivotCloseEnough() {
+    return Math.abs(getClimbPivotPosition() - PivotConstants.TARGET_POSITION)
+        < PivotConstants.PIVOT_ACCEPTABLE_ERROR_DEGREES;
+  }
+}

src/main/java/frc/robot/subsystems/climbpivot/ClimbPivotInterface.java

@@ -0,0 +1,78 @@
+// 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.subsystems.climbPivot;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/** Add your docs here. */
+public interface ClimbPivotInterface {
+  @AutoLog
+  public static class ClimbPivotInputs {
+    /**
+     * THESE DOCSTRINGS WERE ALL WRITTEN BY CHATGPT, i think they're mostly accurate but idk so TAKE
+     * NONE OF THESE SERIOUSLY The current position of the climb pivot (in radians or encoder
+     * units).
+     */
+    public double position = 0.0;
+
+    /** The voltage currently applied to the climb pivot motor (in volts). */
+    public double climbPivotAppliedVolts = 0.0;
+
+    public double currentVolts = 0.0;
+  }
+
+  /**
+   * Updates the provided `ClimbPivotInputs` with the current state of the climb pivot system. This
+   * method can be implemented to update the inputs with the current position and applied voltage.
+   *
+   * @param inputs The `ClimbPivotInputs` object to update. It will be populated with the current
+   *     position and voltage.
+   */
+  public default void updateInputs(ClimbPivotInputs inputs) {}
+
+  /**
+   * Retrieves the current position of the climb pivot system. This method should return the current
+   * position in radians or any other relevant unit.
+   *
+   * @return The current position of the climb pivot.
+   */
+  public default double getClimbPivotPosition() {
+    return 0.0;
+  }
+
+  /**
+   * Manually pivots the climb pivot system to the desired position. This method should be
+   * implemented to control the climb pivot motor to reach the target position.
+   *
+   * @param position
+   */
+  public default void manualPivot(double position) {}
+
+  /**
+   * Sets the desired position for the climb pivot system. This method should be implemented to
+   * control the climb pivot motor to reach the target position.
+   *
+   * @param position The target position to set for the climb pivot.
+   */
+  public default void setClimbPivotPosition(double position) {}
+
+  /**
+   * Sets the applied voltage for the climb pivot motor. This method is used to directly control the
+   * motor's voltage.
+   *
+   * @param volts The voltage to apply to the climb pivot motor.
+   */
+  public default void setVolts(double volts) {}
+
+  /**
+   * Retrieves the current applied voltage to the climb pivot system. This method should return the
+   * voltage that is currently being applied to the climb pivot motor.
+   *
+   * @return The current applied voltage to the climb pivot motor.
+   */
+  public default double getVolts() {
+    return 0.0;
+  }
+}

src/main/java/frc/robot/subsystems/climbpivot/PhysicalClimbPivot.java

@@ -0,0 +1,74 @@
+// 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.subsystems.climbPivot;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
+
+/** Add your docs here. */
+public class PhysicalClimbPivot implements ClimbPivotInterface {
+  public TalonFXConfiguration config;
+  private TalonFX climbMotor = new TalonFX(PivotConstants.CLIMB_PIVOT_MOTOR_ID);
+  MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0);
+  DutyCycleOut dutyCycleOut = new DutyCycleOut(0.0);
+
+  private final StatusSignal<Voltage> climbMotorAppliedVoltage;
+  private final StatusSignal<Angle> climbMotorAngle;
+
+  public PhysicalClimbPivot() {
+    config.Slot0.kP = PivotConstants.CLIMB_PIVOT_P;
+    config.Slot0.kI = PivotConstants.CLIMB_PIVOT_I;
+    config.Slot0.kD = PivotConstants.CLIMB_PIVOT_D;
+
+    config.Slot0.kS = PivotConstants.FF_CLIMB_PIVOT_S;
+    config.Slot0.kV = PivotConstants.FF_CLIMB_PIVOT_G;
+    config.Slot0.kA = PivotConstants.FF_CLIMB_PIVOT_V;
+
+    climbMotor.getConfigurator().apply(config);
+
+    climbMotorAppliedVoltage = climbMotor.getMotorVoltage();
+    climbMotorAngle = climbMotor.getPosition();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(50.0);
+    climbMotor.optimizeBusUtilization();
+  }
+
+  @Override
+  public void updateInputs(ClimbPivotInputs inputs) {
+    inputs.position = climbMotorAngle.getValueAsDouble();
+    inputs.currentVolts = climbMotorAppliedVoltage.getValueAsDouble();
+  }
+
+  @Override
+  public void setClimbPivotPosition(double position) {
+    climbMotor.setControl(motionMagicVoltage.withPosition(position));
+  }
+
+  @Override
+  public void manualPivot(double percentSpeed) {
+    climbMotor.set(percentSpeed);
+    // climbMotor.setControl(dutyCycleOut.withOutput(percentSpeed));
+  }
+
+  @Override
+  public double getClimbPivotPosition() {
+    return climbMotor.getPosition().getValueAsDouble();
+  }
+
+  public double getVolts() {
+    return climbMotor.getMotorVoltage().getValueAsDouble();
+  }
+
+  // cocaine
+  public void setVolts(double volts) {
+    climbMotor.setVoltage(volts);
+  }
+}

src/main/java/frc/robot/subsystems/climbpivot/PivotConstants.java

@@ -0,0 +1,26 @@
+// 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.subsystems.climbPivot;
+
+/** Add your docs here. */
+public class PivotConstants {
+  public static final int CLIMB_PIVOT_MOTOR_ID = 0 - 9;
+
+  public static final double CLIMB_PIVOT_P = 0 - 9;
+  public static final double CLIMB_PIVOT_I = 0 - 9;
+  public static final double CLIMB_PIVOT_D = 0 - 9;
+
+  public static final double FF_CLIMB_PIVOT_S = 0 - 9;
+  public static final double FF_CLIMB_PIVOT_G = 0 - 9;
+  public static final double FF_CLIMB_PIVOT_V = 0 - 9;
+
+  public static final double PIVOT_MASS = 0 - 9;
+  public static final double PIVOT_LENGTH = 0 - 9;
+
+  public static final double PIVOT_ACCEPTABLE_ERROR_DEGREES = 1.0;
+
+  public static final double TARGET_POSITION = 0.0;
+  public static final double GEAR_RATIO = 64;
+}