252 lines
9.2 KiB
Java
252 lines
9.2 KiB
Java
// 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;
|
|
|
|
import org.fairportrobotics.frc.posty.TestableSubsystem;
|
|
import org.fairportrobotics.frc.posty.test.PostTest;
|
|
import org.littletonrobotics.junction.Logger;
|
|
|
|
import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
|
|
|
|
import com.ctre.phoenix6.controls.PositionVoltage;
|
|
import com.ctre.phoenix6.StatusSignal;
|
|
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
|
import com.ctre.phoenix6.configs.FeedbackConfigs;
|
|
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
|
import com.ctre.phoenix6.configs.Slot0Configs;
|
|
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
|
|
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
|
import com.ctre.phoenix6.hardware.TalonFX;
|
|
import com.ctre.phoenix6.signals.ForwardLimitValue;
|
|
import com.ctre.phoenix6.signals.InvertedValue;
|
|
import com.ctre.phoenix6.signals.ReverseLimitValue;
|
|
import com.ctre.phoenix6.sim.TalonFXSimState;
|
|
import com.revrobotics.PersistMode;
|
|
import com.revrobotics.ResetMode;
|
|
import com.revrobotics.spark.ClosedLoopSlot;
|
|
import com.revrobotics.spark.FeedbackSensor;
|
|
import com.revrobotics.spark.SparkClosedLoopController;
|
|
import com.revrobotics.spark.SparkMax;
|
|
import com.revrobotics.spark.SparkBase.ControlType;
|
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
import com.revrobotics.spark.config.SparkMaxConfig;
|
|
|
|
import frc.robot.Constants;
|
|
import edu.wpi.first.units.AngleUnit;
|
|
import edu.wpi.first.units.Units;
|
|
import edu.wpi.first.units.measure.Angle;
|
|
import edu.wpi.first.wpilibj.DigitalInput;
|
|
import edu.wpi.first.wpilibj.Servo;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.Commands;
|
|
|
|
public class TurretSubsystem extends TestableSubsystem {
|
|
public enum TurretState {
|
|
INIT,
|
|
HOMING,
|
|
READY;
|
|
}
|
|
|
|
private final boolean LOGGING = true;
|
|
private TurretState turretState;
|
|
private DigitalInput turretLimitSwitch;
|
|
private TalonFX turretMotor;
|
|
private PositionVoltage turretControlMode = new PositionVoltage(0);
|
|
private TalonFXSimState turretSim;
|
|
private StatusSignal<Angle> turretMotorPosition;
|
|
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
|
|
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
|
|
private SparkMax launcherMotor;
|
|
private SparkClosedLoopController launcherControler;
|
|
|
|
private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees);
|
|
|
|
private Servo hood;
|
|
|
|
public TurretSubsystem() {
|
|
|
|
// Turret configuration
|
|
turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
|
|
turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
|
|
turretMotor.getConfigurator().apply(
|
|
new TalonFXConfiguration()
|
|
.withSlot0(
|
|
new Slot0Configs()
|
|
.withKP(2)
|
|
.withKI(1)
|
|
.withKD(0)
|
|
.withKS(0)
|
|
)
|
|
.withMotorOutput(
|
|
new MotorOutputConfigs()
|
|
.withInverted(InvertedValue.Clockwise_Positive)
|
|
)
|
|
.withCurrentLimits(
|
|
new CurrentLimitsConfigs()
|
|
.withStatorCurrentLimit(50)
|
|
)
|
|
.withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
|
|
.withFeedback(
|
|
new FeedbackConfigs()
|
|
.withFeedbackRotorOffset(0)
|
|
)
|
|
);
|
|
turretMotorPosition = turretMotor.getPosition();
|
|
turretMotorForwardLimit = turretMotor.getForwardLimit();
|
|
turretMotorReverseLimit = turretMotor.getReverseLimit();
|
|
|
|
turretSim = turretMotor.getSimState();
|
|
|
|
// Launcher configuration
|
|
launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
|
|
SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
|
|
config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
|
|
config.voltageCompensation(10);
|
|
config.closedLoop.p(0.0001).i(0).d(0.0); // I = 0.0000001
|
|
config.closedLoop.feedForward.kS(0.2).kV(0.002).kA(0.00);
|
|
// config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
|
|
config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
|
|
config.closedLoop.positionWrappingEnabled(false);
|
|
// config.closedLoopRampRate(0.5);
|
|
// config.smartCurrentLimit(5, 50);
|
|
launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
|
|
launcherControler = launcherMotor.getClosedLoopController();
|
|
|
|
// Hood configuration
|
|
hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
|
|
turretState = TurretState.INIT;
|
|
}
|
|
|
|
public void setLauncher(double speed) {
|
|
if(speed == 0)
|
|
{
|
|
launcherMotor.stopMotor();
|
|
return;
|
|
}
|
|
launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
|
|
}
|
|
|
|
public boolean isLauncherUpToSpeed() {
|
|
// Valid if velcity is within 10% of setpoint.
|
|
// May need to lower this
|
|
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1;
|
|
}
|
|
|
|
public void turretControl(double position)
|
|
{
|
|
turretMotor.setControl(new PositionVoltage(position));
|
|
}
|
|
|
|
public void setTargetElevation(double elev)
|
|
{
|
|
double pos = Math.abs((elev - 300)/300);
|
|
Logger.recordOutput("ShooterHood-Angle", pos);
|
|
hood.set(pos);
|
|
}
|
|
|
|
public Command revSpeedCommand()
|
|
{
|
|
return this.runOnce(() -> setLauncher(-.5));
|
|
}
|
|
|
|
public Command setShooter(int speedRPM){
|
|
return Commands.runEnd(() -> {
|
|
setLauncher(speedRPM);
|
|
}, () -> {
|
|
launcherMotor.stopMotor();
|
|
}, this);
|
|
}
|
|
|
|
public void homeTurret()
|
|
{
|
|
turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
|
|
turretState = TurretState.HOMING;
|
|
}
|
|
|
|
public boolean isTurretReady(){
|
|
return turretState == TurretState.READY;
|
|
}
|
|
|
|
public void turretRotateDir(double speed)
|
|
{
|
|
turretMotor.set(speed);
|
|
}
|
|
|
|
public Angle getTurretAngle()
|
|
{
|
|
return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
|
|
}
|
|
|
|
private Angle getTurretMotorPosition(){
|
|
return turretMotorPosition.refresh().getValue().minus(turretOffset);
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
|
|
if(turretState == TurretState.HOMING)
|
|
{
|
|
turretMotor.set(0.05);
|
|
|
|
if(turretLimitSwitch.get())
|
|
{ // Turret has tripped the switch
|
|
turretMotor.stopMotor();
|
|
turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
|
|
turretState = TurretState.READY;
|
|
Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset);
|
|
Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
|
|
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
|
|
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
|
|
turretMotor.getConfigurator().apply(
|
|
new SoftwareLimitSwitchConfigs()
|
|
.withForwardSoftLimitEnable(true)
|
|
.withForwardSoftLimitThreshold(forwardLimit)
|
|
.withReverseSoftLimitEnable(true)
|
|
.withReverseSoftLimitThreshold(reverseLimit)
|
|
);
|
|
// setTurretRotation(Angle.ofRelativeUnits(0, Units.Degrees));
|
|
}
|
|
}
|
|
|
|
if(LOGGING)
|
|
{
|
|
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
|
|
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
|
|
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
|
|
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees));
|
|
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees));
|
|
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
|
|
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees));
|
|
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees));
|
|
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
|
|
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
|
|
}
|
|
|
|
}
|
|
|
|
public void setTurretRotation(Angle pos)
|
|
{
|
|
if(turretState != TurretState.READY)
|
|
{
|
|
// Not ready to set angle
|
|
return;
|
|
}
|
|
|
|
setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
|
|
}
|
|
|
|
public void setTurretMotorRotation(Angle pos){
|
|
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
|
|
turretSim.setRawRotorPosition(pos.plus(turretOffset));
|
|
}
|
|
|
|
@PostTest()
|
|
public void TurretSubsystem_CANDevicesConnected(){
|
|
assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
|
|
assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
|
|
}
|
|
|
|
}
|