Coral intake + algae pivot (#27)

* juno's intake

* IntakeAdvantageKit

* Intake Advantagekit Done

* Edited intake a bit

* Intake edits

* Formatting fixes

* Worked on coral pivot

* fjioe

* continued algae pivot

* htuiew

* gfi

* jkjlkjlk

---------

Co-authored-by: Ishan1522 <[email protected]>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

build.gradle

@@ -1,3 +1,4 @@
+
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2025.2.1"

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

@@ -11,14 +11,21 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.SimulationConstants;
+import frc.robot.commands.algaePivot.ManualAlgaePivot;
 import frc.robot.commands.autodrive.AutoAlign;
 import frc.robot.commands.drive.DriveCommand;
+import frc.robot.commands.intake.Eject;
+import frc.robot.commands.intake.Intake;
 import frc.robot.extras.simulation.field.SimulatedField;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
 import frc.robot.extras.util.JoystickUtil;
+import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem;
+import frc.robot.subsystems.algaePivot.PhysicalAlgaePivot;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.intake.PhysicalIntake;
 import frc.robot.subsystems.swerve.SwerveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
@@ -42,6 +49,9 @@ public class RobotContainer {
 
   private final CommandXboxController operatorController = new CommandXboxController(1);
   private final CommandXboxController driverController = new CommandXboxController(0);
+  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(new PhysicalIntake());
+  private final AlgaePivotSubsystem algaePivotSubsystem =
+      new AlgaePivotSubsystem(new PhysicalAlgaePivot());
 
   // Simulation, we store them here in the robot container
   // private final SimulatedField simulatedArena;
@@ -171,6 +181,35 @@ private void configureButtonBindings() {
     Trigger driverRightDirectionPad = new Trigger(driverController.pov(90));
     Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270));
 
+    driverController.a().whileTrue(new Intake(intakeSubsystem));
+    driverController.b().whileTrue(new Eject(intakeSubsystem));
+    driverController
+        .x()
+        .whileTrue(new ManualAlgaePivot(algaePivotSubsystem, operatorController::getLeftY));
+
+    // // autodrive
+    // Trigger driverAButton = new Trigger(driverController::getAButton);
+    // lol whatever
+    // // intake
+    // Trigger operatorLeftTrigger = new Trigger(()->operatorController.getLeftTriggerAxis() > 0.2);
+    // Trigger operatorLeftBumper = new Trigger(operatorController::getLeftBumper);
+    // // amp and speaker
+    // Trigger operatorBButton = new Trigger(operatorController::getBButton);
+    // Trigger operatorRightBumper = new Trigger(operatorController::getRightBumper);
+    // Trigger operatorRightTrigger = new Trigger(()->operatorController.getRightTriggerAxis() >
+    // 0.2);
+    // Trigger driverRightTrigger = new Trigger(()->driverController.getRightTriggerAxis() > 0.2);
+
+    // // manual pivot and intake rollers
+    // Trigger operatorAButton = new Trigger(operatorController::getAButton);
+    // Trigger operatorXButton = new Trigger(operatorController::getXButton);
+    // Trigger operatorYButton = new Trigger(operatorController::getYButton);
+    // DoubleSupplier operatorRightStickY = operatorController::getRightY;
+    // // unused
+    // Trigger operatorUpDirectionPad = new Trigger(()->operatorController.getPOV() == 0);
+    // Trigger operatorLeftDirectionPad = new Trigger(()->operatorController.getPOV() == 270);
+    // Trigger operatorDownDirectionPad = new Trigger(()->operatorController.getPOV() == 180);
+    // Trigger driverLeftTrigger = new Trigger(()->driverController.getLeftTriggerAxis() > 0.2);
     Trigger driverLeftBumper = new Trigger(driverController.leftBumper());
 
     // DRIVER BUTTONS

src/main/java/frc/robot/commands/algaePivot/ManualAlgaePivot.java

@@ -0,0 +1,35 @@
+package frc.robot.commands.algaePivot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.algaePivot.AlgaeConstants;
+import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem;
+import java.util.function.DoubleSupplier;
+
+public class ManualAlgaePivot extends Command {
+  private final AlgaePivotSubsystem algaePivotSubsystem;
+  private final DoubleSupplier speed;
+
+  public ManualAlgaePivot(AlgaePivotSubsystem algaePivotSubsystem, DoubleSupplier speed) {
+    this.algaePivotSubsystem = algaePivotSubsystem;
+    this.speed = speed;
+    addRequirements(algaePivotSubsystem);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    algaePivotSubsystem.setAlgaeSpeed(speed.getAsDouble());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    algaePivotSubsystem.setAlgaeSpeed(AlgaeConstants.ALGAE_NEUTRAL_SPEED);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/intake/Eject.java

@@ -0,0 +1,29 @@
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+
+public class Eject extends Command {
+  private final IntakeSubsystem intakeSubsystem;
+
+  public Eject(IntakeSubsystem intakeSubsystem) {
+    this.intakeSubsystem = intakeSubsystem;
+    addRequirements(this.intakeSubsystem);
+  }
+
+  @Override
+  public void execute() {
+    intakeSubsystem.setIntakeSpeed(IntakeConstants.EJECT_SPEED);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    intakeSubsystem.setIntakeSpeed(0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/intake/Intake.java

@@ -0,0 +1,29 @@
+package frc.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+
+public class Intake extends Command {
+  private final IntakeSubsystem intakeSubsystem;
+
+  public Intake(IntakeSubsystem intakeSubsystem) {
+    this.intakeSubsystem = intakeSubsystem;
+    addRequirements(this.intakeSubsystem);
+  }
+
+  @Override
+  public void execute() {
+    intakeSubsystem.setIntakeSpeed(IntakeConstants.INTAKE_SPEED);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    intakeSubsystem.setIntakeSpeed(0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/AlgaePivot/AlgaeConstants.java

@@ -0,0 +1,31 @@
+package frc.robot.subsystems.algaePivot;
+
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+public class AlgaeConstants {
+  public static final int ALGAE_PIVOT_MOTOR_ID = 0;
+  public static final int ALGAE_ENCODER_MOTOR_ID = 0;
+
+  public static final SensorDirectionValue ALGAE_ENCODER_REVERSED =
+      SensorDirectionValue.Clockwise_Positive;
+  public static final double MAX_VELOCITY_ROTATIONS_PER_SECOND = 0.0;
+  public static final double MAX_ACCELERATION_ROTATIONS_PER_SECOND = 0.0;
+
+  public static final double MAX_ANGLE = 0.0;
+  public static final double MIN_ANGLE = 0.0;
+
+  public static final double ANGLE_ZERO = 0.0;
+  public static final double PIVOT_P = 0.0;
+  public static final double PIVOT_I = 0.0;
+  public static final double PIVOT_D = 0.0;
+  public static final double PIVOT_G = 0.0;
+
+  public static final double ALGAE_GEAR_RATIO = 0.0;
+  public static final double ALGAE_PIVOT_MASS = 0.0;
+  public static final double ALGAE_PIVOT_LENGTH = 0.0;
+
+  public static final double ALGAE_PIVOT_ANGLE = 0.0;
+  public static final double ALGAE_PIVOT_SPEED = 0.0;
+  public static final double ALGAE_VOLTAGE = 0.0;
+  public static final double ALGAE_NEUTRAL_SPEED = 0.0;
+}

src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotInterface.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems.algaePivot;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface AlgaePivotInterface {
+  @AutoLog
+  public static class AlgaePivotInputs {
+    public boolean isConnected = true;
+    public double algaeAngle = 0.0;
+    public double algaeVoltage = 0.0;
+    public double algaeVelocity = 0.0;
+    public double algaeTemp = 0.0;
+    public double algaeSupplyCurrentAmps = 0.0;
+    public double algaeTorqueCurrentAmps = 0.0;
+  }
+
+  default void updateInputs(AlgaePivotInputs inputs) {}
+
+  default void setAlgaeSpeed(double speed) {}
+
+  default void setAlgaeAngle(double angle) {}
+
+  default void setAlgaeVoltage(double voltage) {}
+
+  default boolean isPivotWithinAcceptapleError() {
+    return true;
+  }
+
+  default double getAlgaeAngle() {
+    return 0.0;
+  }
+
+  default double getAlgaePivotTarget() {
+    return 0.0;
+  }
+}

src/main/java/frc/robot/subsystems/AlgaePivot/AlgaePivotSubsystem.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.algaePivot;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class AlgaePivotSubsystem extends SubsystemBase {
+  private final AlgaePivotInterface algaePivotInterface;
+  private final AlgaePivotInputsAutoLogged inputs = new AlgaePivotInputsAutoLogged();
+
+  public AlgaePivotSubsystem(AlgaePivotInterface algaePivotInterface) {
+    this.algaePivotInterface = algaePivotInterface;
+  }
+
+  public void setAlgaeSpeed(double speed) {
+    algaePivotInterface.setAlgaeSpeed(speed);
+  }
+
+  public void setAlgaeAngle(double angle) {
+    algaePivotInterface.setAlgaeAngle(angle);
+  }
+
+  public void setAlgaeVoltage(double voltage) {
+    algaePivotInterface.setAlgaeVoltage(voltage);
+  }
+
+  public void periodic() {
+    algaePivotInterface.updateInputs(inputs);
+    Logger.processInputs("AlgaePivotSubsystem/", inputs);
+  }
+}

src/main/java/frc/robot/subsystems/AlgaePivot/PhysicalAlgaePivot.java

@@ -0,0 +1,115 @@
+package frc.robot.subsystems.algaePivot;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import frc.robot.Constants.HardwareConstants;
+
+public class PhysicalAlgaePivot implements AlgaePivotInterface {
+  private final TalonFX algaeMotor;
+  private final CANcoder algaeEncoder;
+  private final TalonFXConfiguration algaeMotorConfig;
+  private final CANcoderConfiguration algaeEncoderConfig;
+  private final StatusSignal<Voltage> algaeVoltage;
+  private final StatusSignal<AngularVelocity> algaeVelocity;
+  private StatusSignal<Angle> algaeAngle;
+  private final StatusSignal<Current> algaeSupplyCurrent;
+  private final StatusSignal<Current> algaeStatorCurrent;
+  private final MotionMagicVoltage mmPositionRequest;
+  private double algaeTargetAngle;
+  private final VoltageOut voltageOut;
+
+  public PhysicalAlgaePivot() {
+    algaeMotor = new TalonFX(AlgaeConstants.ALGAE_PIVOT_MOTOR_ID);
+    algaeEncoder = new CANcoder(AlgaeConstants.ALGAE_ENCODER_MOTOR_ID);
+    algaeMotorConfig = new TalonFXConfiguration();
+    algaeEncoderConfig = new CANcoderConfiguration();
+    mmPositionRequest = new MotionMagicVoltage(0);
+
+    algaeVoltage = algaeMotor.getMotorVoltage();
+    algaeVelocity = algaeMotor.getVelocity();
+    algaeAngle = algaeEncoder.getAbsolutePosition();
+    algaeSupplyCurrent = algaeMotor.getSupplyCurrent();
+    algaeStatorCurrent = algaeMotor.getStatorCurrent();
+    voltageOut = new VoltageOut(0);
+
+    algaeEncoderConfig.MagnetSensor.MagnetOffset = -AlgaeConstants.ANGLE_ZERO;
+    algaeEncoderConfig.MagnetSensor.SensorDirection = AlgaeConstants.ALGAE_ENCODER_REVERSED;
+    algaeEncoder.getConfigurator().apply(algaeEncoderConfig, HardwareConstants.TIMEOUT_S);
+
+    algaeMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    algaeMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    algaeMotorConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND;
+
+    algaeMotorConfig.Slot0.kP = AlgaeConstants.PIVOT_P;
+    algaeMotorConfig.Slot0.kI = AlgaeConstants.PIVOT_I;
+    algaeMotorConfig.Slot0.kD = AlgaeConstants.PIVOT_D;
+    algaeMotorConfig.Slot0.kG = AlgaeConstants.PIVOT_G;
+    algaeMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
+
+    algaeMotorConfig.MotionMagic.MotionMagicAcceleration =
+        AlgaeConstants.MAX_VELOCITY_ROTATIONS_PER_SECOND;
+    algaeMotorConfig.MotionMagic.MotionMagicCruiseVelocity =
+        AlgaeConstants.MAX_ACCELERATION_ROTATIONS_PER_SECOND;
+
+    algaeMotorConfig.ClosedLoopGeneral.ContinuousWrap = true;
+
+    algaeMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
+    algaeMotorConfig.Feedback.FeedbackRemoteSensorID = algaeEncoder.getDeviceID();
+
+    algaeMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = AlgaeConstants.MAX_ANGLE;
+    algaeMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = AlgaeConstants.MIN_ANGLE;
+    algaeMotorConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
+    algaeMotorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        100.0, algaeAngle, algaeVelocity, algaeVoltage, algaeSupplyCurrent, algaeStatorCurrent);
+  }
+
+  @Override
+  public void updateInputs(AlgaePivotInputs inputs) {
+    inputs.algaeAngle = algaeAngle.getValueAsDouble();
+    inputs.algaeVelocity = algaeVelocity.getValueAsDouble();
+    inputs.algaeVoltage = algaeVoltage.getValueAsDouble();
+  }
+
+  @Override
+  public void setAlgaeSpeed(double speed) {
+    algaeMotor.set(speed);
+  }
+
+  @Override
+  public void setAlgaeAngle(double angle) {
+    algaeTargetAngle = angle;
+    algaeMotor.setControl(mmPositionRequest.withPosition(angle));
+  }
+
+  @Override
+  public void setAlgaeVoltage(double voltage) {
+    algaeMotor.setControl(voltageOut.withOutput(voltage));
+  }
+
+  @Override
+  public double getAlgaeAngle() {
+    algaeAngle.refresh();
+    return algaeAngle.getValueAsDouble();
+  }
+
+  @Override
+  public double getAlgaePivotTarget() {
+    return algaeTargetAngle;
+  }
+}