101 lines
3.6 KiB
Java
101 lines
3.6 KiB
Java
package frc.robot.subsystems;
|
||
|
||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||
import com.ctre.phoenix6.hardware.TalonFX;
|
||
|
||
|
||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||
import frc.robot.Constants;
|
||
|
||
public class IntakerSubsystem extends SubsystemBase{
|
||
//定义intakerotater&intakegetter
|
||
private TalonFX intakerotater;
|
||
private TalonFX intakegetter;
|
||
|
||
//PID
|
||
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
|
||
private final PositionVoltage positionRequest = new PositionVoltage(0);
|
||
|
||
private double intakeRotaterTargetPosition = 0.0;
|
||
|
||
public double getGravityFFVolts(){
|
||
double intakerotaterPosition = getIntakerotaterPosition();
|
||
double gravityFFVolts = Constants.Intaker.IntakeRotaterGravityFF * intakerotaterPosition;
|
||
return gravityFFVolts;
|
||
}
|
||
|
||
public void setIntakerRotaterVelocityRps(double IntakeRotaterTargetVelocity) {
|
||
double gravityFeedforward = getGravityFFVolts();
|
||
intakerotater.setControl(velocityRequest.withVelocity(IntakeRotaterTargetVelocity).withFeedForward(gravityFeedforward));// 重力前馈);
|
||
}
|
||
|
||
public IntakerSubsystem() {
|
||
//初始化rotater&getter
|
||
intakegetter = new TalonFX(47);
|
||
intakerotater = new TalonFX(48);
|
||
intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
|
||
intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
||
}
|
||
|
||
public void setIntakerRotaterPosition(double targetPosition) {
|
||
//目标position
|
||
if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
|
||
targetPosition = Constants.Intaker.intakeRotaterUpLimit; // 限制最大上限
|
||
} else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
|
||
targetPosition = Constants.Intaker.intakeRotaterDownLimit; // 限制最小下限
|
||
}
|
||
|
||
intakeRotaterTargetPosition = targetPosition;
|
||
intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
|
||
}
|
||
|
||
// 获取intakerotater当前角度
|
||
public double getIntakerotaterPosition() {
|
||
return intakerotater.getPosition().getValueAsDouble();
|
||
}
|
||
|
||
public boolean isIntakerotaterAtTarget() {
|
||
// 到位判断容差
|
||
double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
|
||
return error <= Constants.Intaker.positionToleranceRotations;
|
||
}
|
||
|
||
public void stopIntakerotater() {
|
||
intakerotater.set(0);
|
||
// 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
|
||
}
|
||
|
||
//intakegetter的on/off
|
||
// public void setIntakeGetterOn(boolean on) {
|
||
// if (on) {
|
||
// intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
|
||
// }
|
||
// else {
|
||
// intakegetter.setControl(velocityRequest.withVelocity(0));
|
||
// }
|
||
// }
|
||
public void setIntakerGetterSpeed(double speed) {
|
||
intakegetter.set(speed);
|
||
}
|
||
public void zeroEncoder(){
|
||
intakerotater.setPosition(0);
|
||
stopIntakerotater();
|
||
}
|
||
public void IntakerDownNonStop(){
|
||
intakerotater.set(-0.1);
|
||
}
|
||
|
||
public void IntakerUpNonStop(){
|
||
intakerotater.set(0.2);
|
||
}
|
||
|
||
public void setIntakerRotater(double rotaterspeed){
|
||
intakerotater.set(rotaterspeed);
|
||
}
|
||
public void stopAll() {
|
||
stopIntakerotater();
|
||
setIntakerGetterSpeed(0);
|
||
}
|
||
}
|