140 lines
4.4 KiB
Java
140 lines
4.4 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import com.ctre.phoenix6.controls.Follower;
|
|
import com.ctre.phoenix6.controls.VoltageOut;
|
|
import com.ctre.phoenix6.hardware.TalonFX;
|
|
import com.ctre.phoenix6.signals.MotorAlignmentValue;
|
|
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.networktables.NetworkTable;
|
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
import edu.wpi.first.wpilibj.Timer;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.Commands;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.Constants;
|
|
|
|
/**
|
|
* Intaker rotation control for the 2026 fuel path.
|
|
*
|
|
* The rotater uses timed voltage actions only. Position is published for
|
|
* telemetry, but it is not used for closed-loop control.
|
|
*/
|
|
public class IntakeRotaterSubsystem extends SubsystemBase {
|
|
public final TalonFX leftIntakeRotater;
|
|
private final TalonFX rightIntakeRotater;
|
|
|
|
private final VoltageOut voltageRequest = new VoltageOut(0);
|
|
private final Follower rightFollower;
|
|
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater");
|
|
|
|
private double outputVoltage = 0.0;
|
|
|
|
public IntakeRotaterSubsystem() {
|
|
leftIntakeRotater = new TalonFX(Constants.Intaker.leftIntakeRotaterID);
|
|
rightIntakeRotater = new TalonFX(Constants.Intaker.rightIntakeRotaterID);
|
|
leftIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
|
rightIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
|
rightFollower = new Follower(leftIntakeRotater.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
|
followLeft();
|
|
|
|
leftIntakeRotater.setPosition(0.0);
|
|
rightIntakeRotater.setPosition(0.0);
|
|
}
|
|
|
|
public void lowerForMatch() {
|
|
setVoltage(0.05);
|
|
}
|
|
|
|
public Command lowerForMatchCommand() {
|
|
return Commands.sequence(
|
|
Commands.runOnce(() -> setVoltage(-1.5), this),
|
|
Commands.waitSeconds(0.7),
|
|
Commands.runOnce(() -> setVoltage(0.05), this),
|
|
Commands.waitSeconds(0.7),
|
|
Commands.runOnce(this::stop, this)
|
|
);
|
|
}
|
|
|
|
public void raiseForMatch() {
|
|
setVoltage(0.8);
|
|
}
|
|
|
|
public Command raiseForMatchCommand() {
|
|
return Commands.sequence(
|
|
Commands.runOnce(this::raiseForMatch, this),
|
|
Commands.waitSeconds(0.5),
|
|
Commands.runOnce(this::stop, this)
|
|
);
|
|
}
|
|
|
|
public void shootAssist() {
|
|
double cycleTime = Timer.getFPGATimestamp() % 1.7;
|
|
|
|
if (cycleTime < 0.7) {
|
|
setVoltage(1);
|
|
} else if (cycleTime < 0.9) {
|
|
setVoltage(0.0);
|
|
} else {
|
|
setVoltage(0.03);
|
|
}
|
|
}
|
|
|
|
public void postShootIdleAssist(double elapsedSeconds) {
|
|
double cycleTime = elapsedSeconds % 0.2;
|
|
setVoltage(cycleTime < 0.1 ? 0.0 : 0.0);
|
|
}
|
|
|
|
public void manualRaise() {
|
|
setVoltage(Constants.Intaker.intakeRotaterRaiseVoltage);
|
|
}
|
|
|
|
public void manualLower() {
|
|
setVoltage(Constants.Intaker.intakeRotaterLowerVoltage);
|
|
}
|
|
|
|
public void stop() {
|
|
setVoltage(0.0);
|
|
}
|
|
|
|
public boolean isRunning() {
|
|
return Math.abs(outputVoltage) > 1e-6;
|
|
}
|
|
|
|
public double getPosition() {
|
|
return leftIntakeRotater.getPosition().getValueAsDouble();
|
|
}
|
|
|
|
public double getLeftVelocity() {
|
|
return leftIntakeRotater.getVelocity().getValueAsDouble();
|
|
}
|
|
|
|
public double getRightVelocity() {
|
|
return rightIntakeRotater.getVelocity().getValueAsDouble();
|
|
}
|
|
|
|
private double getVelocityDifference() {
|
|
return Math.abs(getLeftVelocity()) - Math.abs(getRightVelocity());
|
|
}
|
|
|
|
private void setVoltage(double volts) {
|
|
outputVoltage = MathUtil.clamp(volts, -12.0, 12.0);
|
|
leftIntakeRotater.setControl(voltageRequest.withOutput(outputVoltage));
|
|
followLeft();
|
|
}
|
|
|
|
private void followLeft() {
|
|
rightIntakeRotater.setControl(rightFollower);
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
table.getEntry("position").setDouble(getPosition());
|
|
table.getEntry("isRunning").setBoolean(isRunning());
|
|
table.getEntry("outputVoltage").setDouble(outputVoltage);
|
|
table.getEntry("leftVelocity").setDouble(getLeftVelocity());
|
|
table.getEntry("rightVelocity").setDouble(getRightVelocity());
|
|
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
|
|
}
|
|
}
|