535 lines
26 KiB
Java
535 lines
26 KiB
Java
package frc.robot.subsystems;
|
||
|
||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||
import com.ctre.phoenix6.controls.Follower;
|
||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||
import com.ctre.phoenix6.controls.VoltageOut;
|
||
import com.ctre.phoenix6.hardware.TalonFX;
|
||
import com.ctre.phoenix6.hardware.TalonFXS;
|
||
import com.ctre.phoenix6.signals.InvertedValue;
|
||
import com.ctre.phoenix6.signals.MotorAlignmentValue;
|
||
|
||
import edu.wpi.first.math.MathUtil;
|
||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
|
||
import edu.wpi.first.networktables.NetworkTable;
|
||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||
import edu.wpi.first.wpilibj.Encoder;
|
||
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;
|
||
|
||
/**
|
||
* Shooter 总成的硬件封装。
|
||
*
|
||
* 飞轮和 shooter 内部 conveyor 使用 VoltageOut 开环电压输出；
|
||
* shooter 内部 conveyor 仍由右侧电机反向 follower 跟随左侧电机。
|
||
* 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
|
||
* 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。
|
||
*/
|
||
public class ShooterSubsystem extends SubsystemBase {
|
||
public final TalonFX leftFlywheel;
|
||
public final TalonFX rightFlywheel;
|
||
public final TalonFX leftConveyor;
|
||
public final TalonFX rightConveyor;
|
||
|
||
public final TalonFXS backboardMotor;
|
||
public final Encoder backboardEncoder;
|
||
public final ProfiledPIDController backboardPID;
|
||
|
||
private final VelocityVoltage leftFlywheelVelocityRequest = new VelocityVoltage(0).withSlot(0);
|
||
private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0).withSlot(0);
|
||
private final VoltageOut backboardVoltageRequest = new VoltageOut(0);
|
||
|
||
private final Follower flywheelFollower;
|
||
private final Follower conveyorFollower;
|
||
|
||
private final NetworkTable shooterNetworkTable;
|
||
private final NetworkTable flywheelTuningTable;
|
||
private final NetworkTable conveyorTuningTable;
|
||
private final NetworkTable backboardTuningTable;
|
||
|
||
private double flywheelOutputVolts = 0.0;
|
||
private double flywheelTargetVelocity = 0.0;
|
||
private double flywheelVelocityTolerance = Constants.Shooter.flywheelReadyVelocityTolerance;
|
||
private double flywheelVelocityReadyStartTimestamp = 0.0;
|
||
private boolean flywheelVelocityTuningActive = false;
|
||
private double shooterConveyorTargetVelocity = 0.0;
|
||
private boolean shooterConveyorVelocityTuningActive = false;
|
||
private double shooterConveyorOutputVolts = 0.0;
|
||
private double backboardOutputVolts = 0.0;
|
||
private double backboardRawPidOutputVolts = 0.0;
|
||
private double backboardGravityFeedforwardVolts = 0.0;
|
||
private double backboardClampedPidOutputVolts = 0.0;
|
||
private boolean backboardOutputSaturated = false;
|
||
private boolean backboardMinimumOutputActive = false;
|
||
|
||
public ShooterSubsystem() {
|
||
leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
|
||
rightFlywheel = new TalonFX(Constants.Shooter.rightFlywheelID);
|
||
leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
|
||
rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
|
||
|
||
leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||
rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||
leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||
rightConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||
|
||
var motorOutputConfigs = new MotorOutputConfigs();
|
||
motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
|
||
leftFlywheel.getConfigurator().apply(motorOutputConfigs);
|
||
leftConveyor.getConfigurator().apply(motorOutputConfigs);
|
||
|
||
flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||
conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||
setRightFlywheelFollowLeft();
|
||
setRightConveyorFollowLeft();
|
||
|
||
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
|
||
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
|
||
|
||
backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
|
||
backboardEncoder.setSamplesToAverage(5);
|
||
backboardEncoder.setMinRate(1.0);
|
||
backboardEncoder.setDistancePerPulse(1);
|
||
|
||
backboardPID = new ProfiledPIDController(
|
||
Constants.Shooter.backboardPositionPID.kP,
|
||
Constants.Shooter.backboardPositionPID.kI,
|
||
Constants.Shooter.backboardPositionPID.kD,
|
||
new Constraints(10000, 100000)
|
||
);
|
||
backboardPID.setTolerance(10.0);
|
||
|
||
shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
|
||
flywheelTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/flywheel");
|
||
flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false);
|
||
flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity);
|
||
flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance);
|
||
conveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/conveyor");
|
||
conveyorTuningTable.getEntry("conveyorVelocityEnable").setDefaultBoolean(false);
|
||
conveyorTuningTable.getEntry("conveyorTargetVelocity").setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity);
|
||
backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
|
||
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
|
||
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
|
||
}
|
||
|
||
/* ====================== */
|
||
/* 飞轮 */
|
||
/* ====================== */
|
||
|
||
public void setRightConveyorFollowLeft() {
|
||
// 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
|
||
rightConveyor.setControl(conveyorFollower);
|
||
}
|
||
|
||
public void setRightFlywheelFollowLeft() {
|
||
rightFlywheel.setControl(flywheelFollower);
|
||
}
|
||
|
||
public void applyLeftConfigurationToRight() {
|
||
Slot0Configs configs = new Slot0Configs();
|
||
leftFlywheel.getConfigurator().refresh(configs);
|
||
rightFlywheel.getConfigurator().apply(configs);
|
||
}
|
||
|
||
public void setFlywheelVoltage(double outputVolts) {
|
||
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||
setFlywheelVelocity(clampedOutputVolts / Constants.Shooter.flywheelNominalKv);
|
||
}
|
||
|
||
public void setFlywheelVelocity(double targetVelocityRps) {
|
||
double clampedTargetVelocity = MathUtil.clamp(
|
||
targetVelocityRps,
|
||
-Constants.Shooter.flywheelMaxVelocity,
|
||
Constants.Shooter.flywheelMaxVelocity
|
||
);
|
||
if (Math.abs(clampedTargetVelocity - flywheelTargetVelocity) > 1e-6) {
|
||
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
|
||
}
|
||
flywheelTargetVelocity = clampedTargetVelocity;
|
||
leftFlywheel.setControl(leftFlywheelVelocityRequest.withVelocity(flywheelTargetVelocity));
|
||
setRightFlywheelFollowLeft();
|
||
}
|
||
|
||
public void setFlywheelVoltageByDistance(double distance) {
|
||
setFlywheelVelocityByDistance(distance);
|
||
}
|
||
|
||
public void setFlywheelVelocityByDistance(double distance) {
|
||
double targetVelocity = Constants.Shooter.flywheelVelocityByDistance.get(distance);
|
||
setFlywheelVelocity(targetVelocity);
|
||
}
|
||
|
||
public void stopFlywheel() {
|
||
setFlywheelVelocity(0.0);
|
||
}
|
||
|
||
public boolean atVelocity() {
|
||
if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) {
|
||
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
|
||
return false;
|
||
}
|
||
if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) {
|
||
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
|
||
return false;
|
||
}
|
||
return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp
|
||
>= Constants.Shooter.flywheelReadyDelaySeconds;
|
||
}
|
||
|
||
public boolean atVoltage() {
|
||
return atVelocity();
|
||
}
|
||
public double getLeftFlywheelVelocity() {
|
||
return leftFlywheel.getVelocity().getValueAsDouble();
|
||
}
|
||
|
||
public double getRightFlywheelVelocity() {
|
||
return rightFlywheel.getVelocity().getValueAsDouble();
|
||
}
|
||
|
||
public double getAverageFlywheelVelocity() {
|
||
return (Math.abs(getLeftFlywheelVelocity()) + Math.abs(getRightFlywheelVelocity())) / 2.0;
|
||
}
|
||
|
||
public double getFlywheelVelocityDifference() {
|
||
return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
|
||
}
|
||
|
||
public double getTargetFlywheelVelocity() {
|
||
return flywheelTargetVelocity;
|
||
}
|
||
|
||
public double getFlywheelVelocityError() {
|
||
return Math.abs(flywheelTargetVelocity) - getAverageFlywheelVelocity();
|
||
}
|
||
|
||
public double getTargetFlywheelVoltage() {
|
||
return flywheelTargetVelocity * Constants.Shooter.flywheelNominalKv;
|
||
}
|
||
|
||
/* ====================== */
|
||
/* shooter conveyor */
|
||
/* ====================== */
|
||
|
||
public void runShooterConveyorVelocity(double targetVelocityRps) {
|
||
shooterConveyorTargetVelocity = MathUtil.clamp(
|
||
targetVelocityRps,
|
||
-Constants.Shooter.shooterConveyorMaxVelocity,
|
||
Constants.Shooter.shooterConveyorMaxVelocity
|
||
);
|
||
leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity));
|
||
setRightConveyorFollowLeft();
|
||
}
|
||
|
||
public void runShooterConveyorVoltage(double outputVolts) {
|
||
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||
runShooterConveyorVelocity(clampedOutputVolts / Constants.Shooter.shooterConveyorNominalKv);
|
||
}
|
||
|
||
public void stopShooterConveyor() {
|
||
runShooterConveyorVelocity(0.0);
|
||
}
|
||
|
||
public double getShooterConveyorVelocity() {
|
||
return leftConveyor.getVelocity().getValueAsDouble();
|
||
}
|
||
|
||
public double getShooterConveyorTargetVelocity() {
|
||
return shooterConveyorTargetVelocity;
|
||
}
|
||
|
||
public double getShooterConveyorVelocityError() {
|
||
return shooterConveyorTargetVelocity - getShooterConveyorVelocity();
|
||
}
|
||
|
||
public double getConveyorStatorCurrent() {
|
||
return leftConveyor.getStatorCurrent().getValueAsDouble();
|
||
}
|
||
|
||
/* ====================== */
|
||
/* 背板 */
|
||
/* ====================== */
|
||
|
||
public void setBackboardVoltage(double outputVolts) {
|
||
backboardOutputVolts = MathUtil.clamp(
|
||
outputVolts,
|
||
-Constants.Shooter.backboardSpeedMax,
|
||
Constants.Shooter.backboardSpeedMax
|
||
);
|
||
backboardMotor.setControl(backboardVoltageRequest.withOutput(backboardOutputVolts));
|
||
}
|
||
|
||
public void setBackboardPosition(double targetPosition) {
|
||
// 保留旧背板机构的软件限位。
|
||
if (targetPosition > Constants.Shooter.backboardUpLimit) {
|
||
targetPosition = Constants.Shooter.backboardUpLimit;
|
||
} else if (targetPosition < Constants.Shooter.backboardDownLimit) {
|
||
targetPosition = Constants.Shooter.backboardDownLimit;
|
||
}
|
||
backboardPID.setGoal(targetPosition);
|
||
}
|
||
|
||
public void outputBackboard() {
|
||
// 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
|
||
double backboardPosition = getBackboardPosition();
|
||
double rawPidOutput = backboardPID.calculate(backboardPosition);
|
||
double pidOutput = MathUtil.clamp(
|
||
rawPidOutput,
|
||
-Constants.Shooter.backboardSpeedMax,
|
||
Constants.Shooter.backboardSpeedMax
|
||
);
|
||
backboardRawPidOutputVolts = rawPidOutput;
|
||
backboardClampedPidOutputVolts = pidOutput;
|
||
backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9;
|
||
|
||
if (pidOutput != 0.0 && Math.abs(pidOutput) < Constants.Shooter.backboardSpeedMin) {
|
||
pidOutput = Math.copySign(Constants.Shooter.backboardSpeedMin, pidOutput);
|
||
backboardMinimumOutputActive = true;
|
||
} else {
|
||
backboardMinimumOutputActive = false;
|
||
}
|
||
|
||
double gravityOutput = calculateBackboardGravityFeedforward(backboardPosition);
|
||
double outputVolts = MathUtil.clamp(
|
||
pidOutput + gravityOutput,
|
||
-Constants.Shooter.backboardSpeedMax,
|
||
Constants.Shooter.backboardSpeedMax
|
||
);
|
||
backboardGravityFeedforwardVolts = gravityOutput;
|
||
backboardOutputSaturated = backboardOutputSaturated
|
||
|| Math.abs((pidOutput + gravityOutput) - outputVolts) > 1e-9;
|
||
|
||
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(outputVolts);
|
||
setBackboardVoltage(outputVolts);
|
||
}
|
||
|
||
private double calculateBackboardGravityFeedforward(double backboardPosition) {
|
||
double clampedPosition = MathUtil.clamp(
|
||
backboardPosition,
|
||
Constants.Shooter.backboardGravityFeedforwardMinPosition,
|
||
Constants.Shooter.backboardGravityFeedforwardMaxPosition
|
||
);
|
||
double positionRange = Constants.Shooter.backboardGravityFeedforwardMaxPosition
|
||
- Constants.Shooter.backboardGravityFeedforwardMinPosition;
|
||
if (positionRange <= 0.0) {
|
||
return 0.0;
|
||
}
|
||
|
||
double positionRatio = (clampedPosition - Constants.Shooter.backboardGravityFeedforwardMinPosition)
|
||
/ positionRange;
|
||
double feedforwardVolts = Constants.Shooter.backboardGravityFeedforwardMinVolts
|
||
+ positionRatio
|
||
* (Constants.Shooter.backboardGravityFeedforwardMaxVolts
|
||
- Constants.Shooter.backboardGravityFeedforwardMinVolts);
|
||
return Constants.Shooter.backboardGravityFeedforwardDirection * feedforwardVolts;
|
||
}
|
||
|
||
public void holdBackboardAt(double targetPosition) {
|
||
// 上层只给目标位置；到位后仍继续输出重力补偿。
|
||
setBackboardPosition(targetPosition);
|
||
outputBackboard();
|
||
}
|
||
|
||
public void stopBackboard() {
|
||
backboardRawPidOutputVolts = 0.0;
|
||
backboardGravityFeedforwardVolts = 0.0;
|
||
backboardClampedPidOutputVolts = 0.0;
|
||
backboardOutputSaturated = false;
|
||
backboardMinimumOutputActive = false;
|
||
setBackboardVoltage(0.0);
|
||
}
|
||
|
||
public double getBackboardPosition() {
|
||
return backboardEncoder.getDistance();
|
||
}
|
||
|
||
public boolean isBackboardAtTarget() {
|
||
return backboardPID.atGoal();
|
||
}
|
||
|
||
public double getBackboardGoalPosition() {
|
||
return backboardPID.getGoal().position;
|
||
}
|
||
|
||
public double getBackboardPositionError() {
|
||
return getBackboardGoalPosition() - getBackboardPosition();
|
||
}
|
||
|
||
public boolean isBackboardNearTarget(double tolerance) {
|
||
return Math.abs(getBackboardPositionError()) < tolerance;
|
||
}
|
||
|
||
public boolean isBackboardPositionTuningActive() {
|
||
return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
|
||
}
|
||
|
||
public boolean isFlywheelTuningControlActive() {
|
||
boolean voltageTunerEnabled = flywheelTuningTable.getEntry("flywheelEnable").getBoolean(false);
|
||
boolean voltageTunerRunning = flywheelTuningTable.getEntry("flywheelEnableRunning").getBoolean(false);
|
||
boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
|
||
return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled;
|
||
}
|
||
|
||
public boolean isShooterConveyorTuningControlActive() {
|
||
boolean voltageTunerEnabled = conveyorTuningTable.getEntry("conveyorEnable").getBoolean(false);
|
||
boolean voltageTunerRunning = conveyorTuningTable.getEntry("conveyorEnableRunning").getBoolean(false);
|
||
boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
|
||
return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled;
|
||
}
|
||
|
||
public void resetbackboardencoder() {
|
||
backboardEncoder.reset();
|
||
backboardPID.reset(0);
|
||
}
|
||
|
||
/**
|
||
* 背板回零命令：闭环回到 0 位后停止输出。
|
||
* 命令留在 subsystem 内，controls 层只负责绑定按键。
|
||
*/
|
||
public Command homeBackboardCommand() {
|
||
return Commands.sequence(
|
||
Commands.runOnce(() -> setBackboardPosition(0.0), this),
|
||
Commands.run(() -> holdBackboardAt(0.0), this).until(this::isBackboardAtTarget),
|
||
Commands.runOnce(this::stopBackboard, this)
|
||
);
|
||
}
|
||
|
||
/* ====================== */
|
||
/* 整体 */
|
||
/* ====================== */
|
||
|
||
public void stopAll() {
|
||
stopFlywheel();
|
||
stopShooterConveyor();
|
||
stopBackboard();
|
||
}
|
||
|
||
public void debugBackboardTargetPeriodic() {
|
||
boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
|
||
double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0);
|
||
double clampedTarget = MathUtil.clamp(
|
||
requestedTarget,
|
||
Constants.Shooter.backboardDownLimit,
|
||
Constants.Shooter.backboardUpLimit
|
||
);
|
||
|
||
backboardTuningTable.getEntry("backboardPositionEnableActive").setBoolean(positionEnable);
|
||
backboardTuningTable.getEntry("backboardCommandedTargetPosition").setDouble(requestedTarget);
|
||
backboardTuningTable.getEntry("backboardClampedTargetPosition").setDouble(clampedTarget);
|
||
|
||
if (positionEnable) {
|
||
holdBackboardAt(clampedTarget);
|
||
}
|
||
}
|
||
|
||
public void debugFlywheelVelocityPeriodic() {
|
||
flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
|
||
double requestedTargetVelocity =
|
||
flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity);
|
||
flywheelVelocityTolerance = Math.max(
|
||
0.0,
|
||
flywheelTuningTable.getEntry("flywheelVelocityTolerance")
|
||
.getDouble(Constants.Shooter.flywheelReadyVelocityTolerance)
|
||
);
|
||
|
||
double clampedTargetVelocity = MathUtil.clamp(
|
||
requestedTargetVelocity,
|
||
-Constants.Shooter.flywheelMaxVelocity,
|
||
Constants.Shooter.flywheelMaxVelocity
|
||
);
|
||
flywheelTuningTable.getEntry("flywheelVelocityEnableActive").setBoolean(flywheelVelocityTuningActive);
|
||
flywheelTuningTable.getEntry("flywheelCommandedTargetVelocity").setDouble(requestedTargetVelocity);
|
||
flywheelTuningTable.getEntry("flywheelClampedTargetVelocity").setDouble(clampedTargetVelocity);
|
||
flywheelTuningTable.getEntry("flywheelVelocityToleranceActive").setDouble(flywheelVelocityTolerance);
|
||
|
||
if (flywheelVelocityTuningActive) {
|
||
setFlywheelVelocity(clampedTargetVelocity);
|
||
}
|
||
}
|
||
|
||
public void debugShooterConveyorVelocityPeriodic() {
|
||
shooterConveyorVelocityTuningActive =
|
||
conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false);
|
||
double requestedTargetVelocity =
|
||
conveyorTuningTable.getEntry("conveyorTargetVelocity")
|
||
.getDouble(Constants.Shooter.shooterConveyorFeedVelocity);
|
||
double clampedTargetVelocity = MathUtil.clamp(
|
||
requestedTargetVelocity,
|
||
-Constants.Shooter.shooterConveyorMaxVelocity,
|
||
Constants.Shooter.shooterConveyorMaxVelocity
|
||
);
|
||
|
||
conveyorTuningTable.getEntry("conveyorVelocityEnableActive")
|
||
.setBoolean(shooterConveyorVelocityTuningActive);
|
||
conveyorTuningTable.getEntry("conveyorCommandedTargetVelocity").setDouble(requestedTargetVelocity);
|
||
conveyorTuningTable.getEntry("conveyorClampedTargetVelocity").setDouble(clampedTargetVelocity);
|
||
|
||
if (shooterConveyorVelocityTuningActive) {
|
||
runShooterConveyorVelocity(clampedTargetVelocity);
|
||
}
|
||
}
|
||
|
||
@Override
|
||
public void periodic() {
|
||
double backboardPosition = getBackboardPosition();
|
||
double backboardGoalPosition = backboardPID.getGoal().position;
|
||
double backboardSetpointPosition = backboardPID.getSetpoint().position;
|
||
flywheelOutputVolts = leftFlywheel.getMotorVoltage().getValueAsDouble();
|
||
shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
|
||
shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
|
||
shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
|
||
shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
|
||
shooterNetworkTable.getEntry("flywheelTargetVelocity").setDouble(getTargetFlywheelVelocity());
|
||
shooterNetworkTable.getEntry("flywheelVelocityError").setDouble(getFlywheelVelocityError());
|
||
shooterNetworkTable.getEntry("flywheelVelocityTolerance").setDouble(flywheelVelocityTolerance);
|
||
shooterNetworkTable.getEntry("flywheelAtVelocity").setBoolean(atVelocity());
|
||
shooterNetworkTable.getEntry("flywheelVelocityTuningActive").setBoolean(flywheelVelocityTuningActive);
|
||
shooterNetworkTable.getEntry("flywheelFollowerOpposed").setBoolean(true);
|
||
shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage());
|
||
shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
|
||
shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage());
|
||
shooterNetworkTable.getEntry("leftFlywheelOutputVolts").setDouble(leftFlywheel.getMotorVoltage().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("rightFlywheelOutputVolts").setDouble(rightFlywheel.getMotorVoltage().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("leftFlywheelStatorCurrent").setDouble(leftFlywheel.getStatorCurrent().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("rightFlywheelStatorCurrent").setDouble(rightFlywheel.getStatorCurrent().getValueAsDouble());
|
||
shooterConveyorOutputVolts = leftConveyor.getMotorVoltage().getValueAsDouble();
|
||
shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
|
||
shooterNetworkTable.getEntry("conveyorTargetVelocity").setDouble(getShooterConveyorTargetVelocity());
|
||
shooterNetworkTable.getEntry("conveyorVelocity").setDouble(getShooterConveyorVelocity());
|
||
shooterNetworkTable.getEntry("conveyorVelocityError").setDouble(getShooterConveyorVelocityError());
|
||
shooterNetworkTable.getEntry("conveyorVelocityTuningActive").setBoolean(shooterConveyorVelocityTuningActive);
|
||
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
|
||
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
|
||
shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
|
||
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
|
||
shooterNetworkTable.getEntry("backboardGoalPosition").setDouble(backboardGoalPosition);
|
||
shooterNetworkTable.getEntry("backboardSetpointPosition").setDouble(backboardSetpointPosition);
|
||
shooterNetworkTable.getEntry("backboardSetpointVelocity").setDouble(backboardPID.getSetpoint().velocity);
|
||
shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition);
|
||
shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition);
|
||
shooterNetworkTable.getEntry("backboardRawPIDOutput").setDouble(backboardRawPidOutputVolts);
|
||
shooterNetworkTable.getEntry("backboardGravityFeedforward").setDouble(backboardGravityFeedforwardVolts);
|
||
shooterNetworkTable.getEntry("backboardClampedPIDOutput").setDouble(backboardClampedPidOutputVolts);
|
||
shooterNetworkTable.getEntry("backboardOutputSaturated").setBoolean(backboardOutputSaturated);
|
||
shooterNetworkTable.getEntry("backboardMinimumOutputActive").setBoolean(backboardMinimumOutputActive);
|
||
shooterNetworkTable.getEntry("backboardMaxOutputVolts").setDouble(Constants.Shooter.backboardSpeedMax);
|
||
shooterNetworkTable.getEntry("backboardMinOutputVolts").setDouble(Constants.Shooter.backboardSpeedMin);
|
||
shooterNetworkTable.getEntry("backboardGravityFeedforwardMinVolts").setDouble(Constants.Shooter.backboardGravityFeedforwardMinVolts);
|
||
shooterNetworkTable.getEntry("backboardGravityFeedforwardMaxVolts").setDouble(Constants.Shooter.backboardGravityFeedforwardMaxVolts);
|
||
shooterNetworkTable.getEntry("backboardPositionTolerance").setDouble(10.0);
|
||
shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition);
|
||
shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition);
|
||
shooterNetworkTable.getEntry("backboardMotorPosition").setDouble(backboardMotor.getPosition().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("backboardMotorVelocity").setDouble(backboardMotor.getVelocity().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("backboardMotorVoltage").setDouble(backboardMotor.getMotorVoltage().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("backboardMotorStatorCurrent").setDouble(backboardMotor.getStatorCurrent().getValueAsDouble());
|
||
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
|
||
}
|
||
}
|
||
|