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

Turret in Rotations units now

This commit is contained in:
Tyler Wilcox 2026-04-03 16:51:47 -04:00
parent a5cf72bc38
commit 624420ad68
4 changed files with 30 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -74,7 +74,7 @@ public class AutoShootCommandChassisTurret extends Command{
}
});
turretSubsystem.setLauncher(5500);
turretSubsystem.setLauncher(6000);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
hopperSubsystem.spindexerOn();
cameraAutoCenterController.calculate(-100);

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

@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command {
Pose3d botPose = driveSubsystem.getBotPose();
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative().in(Units.Rotations)))));
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative()))));
Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
@ -70,7 +70,7 @@ public class AutoTurretCommand extends Command {
if(turretSubsystem.isTurretReady()){
turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretFieldRelative(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure());
// turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees));
}

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

@ -29,10 +29,10 @@ public class RelayCommand extends Command{
@Override
public void initialize() {
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
turretSubsystem.setLauncher(5500);
turretSubsystem.setLauncher(6000);
hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand);
turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(-6, Units.Degrees)); // Return to 0 after MIAMI VALLEY
turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY
}
@Override
@ -57,7 +57,7 @@ public class RelayCommand extends Command{
turretSubsystem.setLauncher(0);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY
turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}

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

@ -61,7 +61,7 @@ public class TurretSubsystem extends TestableSubsystem {
private SparkMax launcherMotor;
private SparkClosedLoopController launcherControler;
private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees);
private double turretOffsetInRotations = 0;
private Servo hood;
@ -131,7 +131,7 @@ public class TurretSubsystem extends TestableSubsystem {
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;
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.075;
}
public void setTargetElevation(double elev)
@ -172,13 +172,13 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotor.set(speed);
}
public Angle getTurretAngleRobotRelative()
public double getTurretAngleRobotRelative()
{
return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
}
private Angle getTurretMotorPosition(){
return turretMotorPosition.refresh().getValue().minus(turretOffset);
private double getTurretMotorPosition(){
return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
}
@Override
@ -191,12 +191,12 @@ public class TurretSubsystem extends TestableSubsystem {
if(turretLimitSwitch.get())
{ // Turret has tripped the switch
turretMotor.stopMotor();
turretOffset = turretMotorPosition.refresh().getValue().minus(Angle.ofRelativeUnits(65, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
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);
// 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)
@ -205,7 +205,7 @@ public class TurretSubsystem extends TestableSubsystem {
// .withReverseSoftLimitThreshold(reverseLimit)
);
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees));
setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY
setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
}
}
@ -214,19 +214,19 @@ public class TurretSubsystem extends TestableSubsystem {
Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
Logger.recordOutput("TurretSubsystem-TurretState", turretState);
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
}
}
public void setTurretFieldRelative(Angle pos)
public void setTurretFieldRelative(double pos)
{
if(turretState != TurretState.READY)
{
@ -235,7 +235,7 @@ public class TurretSubsystem extends TestableSubsystem {
}
}
public void setTurretRobotRelative(Angle pos)
public void setTurretRobotRelative(double pos)
{
if(turretState != TurretState.READY)
{
@ -243,16 +243,16 @@ public class TurretSubsystem extends TestableSubsystem {
return;
}
setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO));
setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
}
public void setTurretMotorRotation(Angle pos){
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1)));
turretSim.setRawRotorPosition(pos.plus(turretOffset));
public void setTurretMotorRotation(double pos){
turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
}
private Angle getRequestedPosTurretRelative() {
return turretControlMode.getPositionMeasure().minus(turretOffset).div(Constants.ShooterConstants.TURRET_GEAR_RATIO);
private double getRequestedPosTurretRelative() {
return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
}
@PostTest()