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

HOMEEEEEEEEEEEEEEEE

This commit is contained in:
Team578 Driver Station 2026-04-17 18:40:16 -04:00
parent 1280197ccf
commit 3f01a07548
1 changed files with 28 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -203,30 +203,37 @@ public class TurretSubsystem extends TestableSubsystem {
@Override
public void periodic() {
if(turretState == TurretState.HOMING)
{
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
if(!Robot.isReal()){
turretOffsetInRotations = 0; // Flip turret around
while(turretState != TurretState.READY)
{
long start = System.currentTimeMillis();
while(System.currentTimeMillis()-1000<start)
{
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
turretMotor.stopMotor();
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
if(!Robot.isReal()){
turretOffsetInRotations = 0; // Flip turret around
}
turretState = TurretState.READY;
forwardLimit = (1.5 + turretOffsetInRotations);
reverseLimit = (-1.5 + turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs()
.withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)
);
// setTurretRobotRelative(0);
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}
turretState = TurretState.READY;
forwardLimit = (1.5 + turretOffsetInRotations);
reverseLimit = (-1.5 + turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs()
.withForwardSoftLimitEnable(true)
.withForwardSoftLimitThreshold(forwardLimit)
.withReverseSoftLimitEnable(true)
.withReverseSoftLimitThreshold(reverseLimit)
);
// setTurretRobotRelative(0);
// setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
turretMotor.set((turretMotor.get()*-1));
}
}