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); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
hopperSubsystem.spindexerOn(); hopperSubsystem.spindexerOn();
cameraAutoCenterController.calculate(-100); 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 botPose = driveSubsystem.getBotPose();
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), 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); Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
@ -70,7 +70,7 @@ public class AutoTurretCommand extends Command {
if(turretSubsystem.isTurretReady()){ 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)); // 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 @Override
public void initialize() { public void initialize() {
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3);
turretSubsystem.setLauncher(5500); turretSubsystem.setLauncher(6000);
hopperSubsystem.spindexerOn(); hopperSubsystem.spindexerOn();
CommandScheduler.getInstance().schedule(waitCommand); 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 @Override
@ -57,7 +57,7 @@ public class RelayCommand extends Command{
turretSubsystem.setLauncher(0); turretSubsystem.setLauncher(0);
turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); 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 SparkMax launcherMotor;
private SparkClosedLoopController launcherControler; private SparkClosedLoopController launcherControler;
private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees); private double turretOffsetInRotations = 0;
private Servo hood; private Servo hood;
@ -131,7 +131,7 @@ public class TurretSubsystem extends TestableSubsystem {
public boolean isLauncherUpToSpeed() { public boolean isLauncherUpToSpeed() {
// Valid if velcity is within 10% of setpoint. // Valid if velcity is within 10% of setpoint.
// May need to lower this // 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) public void setTargetElevation(double elev)
@ -172,13 +172,13 @@ public class TurretSubsystem extends TestableSubsystem {
turretMotor.set(speed); 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(){ private double getTurretMotorPosition(){
return turretMotorPosition.refresh().getValue().minus(turretOffset); return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
} }
@Override @Override
@ -191,12 +191,12 @@ public class TurretSubsystem extends TestableSubsystem {
if(turretLimitSwitch.get()) if(turretLimitSwitch.get())
{ // Turret has tripped the switch { // Turret has tripped the switch
turretMotor.stopMotor(); 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; turretState = TurretState.READY;
Angle forwardLimit = Angle.ofRelativeUnits(-90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).plus(turretOffset); // 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); // Angle reverseLimit = Angle.ofRelativeUnits(90, Units.Degrees).times(Constants.ShooterConstants.TURRET_GEAR_RATIO).minus(turretOffset);
Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit); // Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit); // Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
turretMotor.getConfigurator().apply( turretMotor.getConfigurator().apply(
new SoftwareLimitSwitchConfigs() new SoftwareLimitSwitchConfigs()
// .withForwardSoftLimitEnable(true) // .withForwardSoftLimitEnable(true)
@ -205,7 +205,7 @@ public class TurretSubsystem extends TestableSubsystem {
// .withReverseSoftLimitThreshold(reverseLimit) // .withReverseSoftLimitThreshold(reverseLimit)
); );
// setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); // 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-TurretLimitSwitch", turretLimitSwitch.get());
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", getTurretAngleRobotRelative().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees));
Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
} }
} }
public void setTurretFieldRelative(Angle pos) public void setTurretFieldRelative(double pos)
{ {
if(turretState != TurretState.READY) 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) if(turretState != TurretState.READY)
{ {
@ -243,16 +243,16 @@ public class TurretSubsystem extends TestableSubsystem {
return; return;
} }
setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); setTurretMotorRotation(pos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
} }
public void setTurretMotorRotation(Angle pos){ public void setTurretMotorRotation(double pos){
turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1))); turretMotor.setControl(turretControlMode.withPosition((pos + turretOffsetInRotations)));
turretSim.setRawRotorPosition(pos.plus(turretOffset)); turretSim.setRawRotorPosition(pos + turretOffsetInRotations);
} }
private Angle getRequestedPosTurretRelative() { private double getRequestedPosTurretRelative() {
return turretControlMode.getPositionMeasure().minus(turretOffset).div(Constants.ShooterConstants.TURRET_GEAR_RATIO); return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
} }
@PostTest() @PostTest()