FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Turret Field orientation

This commit is contained in:
Tyler Wilcox 2026-03-29 11:34:22 -04:00
parent bf3e87ea06
commit 2610f4fd77
2 changed files with 39 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/java/frc/robot/commands/AutoTurretCommand.java
View File

@ -72,10 +72,12 @@ public class AutoTurretCommand extends Command {
Logger.recordOutput("AutoTurret-TurretPose", turretPose); Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
Logger.recordOutput("AutoTurret-Translation", targetTranslation);
if(turretSubsystem.isTurretReady()){ if(turretSubsystem.isTurretReady()){
turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure()); turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(45, Units.Degrees)); // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
} }
} }

56
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -11,6 +11,7 @@ import org.littletonrobotics.junction.Logger;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs;
import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs;
@ -18,7 +19,9 @@ import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.ForwardLimitValue;
import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.ReverseLimitValue;
import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState;
import com.revrobotics.PersistMode; import com.revrobotics.PersistMode;
import com.revrobotics.ResetMode; import com.revrobotics.ResetMode;
@ -50,6 +53,9 @@ public class TurretSubsystem extends TestableSubsystem {
private TalonFX turretMotor; private TalonFX turretMotor;
private PositionVoltage turretControlMode = new PositionVoltage(0); private PositionVoltage turretControlMode = new PositionVoltage(0);
private TalonFXSimState turretSim; private TalonFXSimState turretSim;
private StatusSignal<Angle> turretMotorPosition;
private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
private SparkMax launcherMotor; private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler; private SparkClosedLoopController launcherControler;
@ -73,24 +79,21 @@ public class TurretSubsystem extends TestableSubsystem {
) )
.withMotorOutput( .withMotorOutput(
new MotorOutputConfigs() new MotorOutputConfigs()
.withInverted(InvertedValue.CounterClockwise_Positive) .withInverted(InvertedValue.Clockwise_Positive)
) )
.withCurrentLimits( .withCurrentLimits(
new CurrentLimitsConfigs() new CurrentLimitsConfigs()
.withStatorCurrentLimit(50) .withStatorCurrentLimit(50)
) )
.withSoftwareLimitSwitch( .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
new SoftwareLimitSwitchConfigs()
.withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(Angle.ofRelativeUnits(90, Units.Degree).times(Constants.ShooterConstants.TURRET_GEAR_RATIO))
.withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(Angle.ofRelativeUnits(-90, Units.Degree).times(Constants.ShooterConstants.TURRET_GEAR_RATIO))
)
.withFeedback( .withFeedback(
new FeedbackConfigs() new FeedbackConfigs()
.withFeedbackRotorOffset(0) .withFeedbackRotorOffset(0)
) )
); );
turretMotorPosition = turretMotor.getPosition();
turretMotorForwardLimit = turretMotor.getForwardLimit();
turretMotorReverseLimit = turretMotor.getReverseLimit();
turretSim = turretMotor.getSimState(); turretSim = turretMotor.getSimState();
@ -146,11 +149,11 @@ public class TurretSubsystem extends TestableSubsystem {
public Angle getTurretAngle() public Angle getTurretAngle()
{ {
return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(Angle.ofRelativeUnits(180, Units.Degrees)).times(-1); return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
} }
private Angle getTurretMotorPosition(){ private Angle getTurretMotorPosition(){
return turretMotor.getPosition().refresh().getValue().minus(turretOffset); return turretMotorPosition.refresh().getValue().minus(turretOffset);
} }
@Override @Override
@ -158,14 +161,25 @@ public class TurretSubsystem extends TestableSubsystem {
if(turretState == TurretState.HOMING) if(turretState == TurretState.HOMING)
{ {
turretMotor.set(-0.05); turretMotor.set(0.05);
if(turretLimitSwitch.get()) if(turretLimitSwitch.get())
{ // Turret has tripped the switch { // Turret has tripped the switch
turretMotor.stopMotor(); turretMotor.stopMotor();
turretOffset = turretMotor.getPosition().refresh().getValue().plus(Angle.ofRelativeUnits(85, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); turretOffset = turretMotorPosition.refresh().getValue().plus(Angle.ofRelativeUnits(100, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
turretState = TurretState.READY; turretState = TurretState.READY;
setTurretRotation(Angle.ofRelativeUnits(0, Units.Degrees)); 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));
} }
} }
@ -175,8 +189,12 @@ public class TurretSubsystem extends TestableSubsystem {
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState); Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretMotorPosition", getTurretMotorPosition().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-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());
} }
} }
@ -189,23 +207,17 @@ public class TurretSubsystem extends TestableSubsystem {
return; return;
} }
if(pos.gt(Angle.ofRelativeUnits(90, Units.Degrees)) || pos.lt(Angle.ofRelativeUnits(-90, Units.Degrees)))
{
// Don't allow to over extend
return;
}
setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
} }
public void setTurretMotorRotation(Angle pos){ public void setTurretMotorRotation(Angle pos){
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset))); turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset)));
turretSim.setRawRotorPosition(pos); turretSim.setRawRotorPosition(pos.plus(turretOffset));
} }
@PostTest() @PostTest()
public void TurretSubsystem_CANDevicesConnected(){ public void TurretSubsystem_CANDevicesConnected(){
assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isTrue(); assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue(); assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
} }