diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ee22e6..39b2ea8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -99,7 +99,7 @@ public final class Constants { public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); public static final double HOMING_SPEED = .35; - public static final double TURRET_GEAR_RATIO = 1; + public static final double TURRET_GEAR_RATIO = 6; public static final int HOOD_SERVO_CHANNEL = 0; public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22b554e..847174d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutoShootCommandChassisTurret; +import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.ManualHopperCommand; import frc.robot.commands.ManualIntake; import frc.robot.commands.ManualShootCommand; @@ -42,6 +43,10 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // turretSubsystem.homeTurret(); + // Configure the trigger bindings configureBindings(); } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 15fe417..d7ca341 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -40,7 +40,7 @@ public class AutoShootCommandChassisTurret extends Command{ private int[] tagFilters; - private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); private Pose3d closestTag; @@ -91,6 +91,7 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); + // Add 180 because we want the back of the robot to face the HUB deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); } @@ -105,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this + driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); } else // We have an april tag, center it to the camera frame { diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java new file mode 100644 index 0000000..da975ef --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -0,0 +1,94 @@ +package frc.robot.commands; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class AutoTurretCommand extends Command { + + private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736); + + private TurretSubsystem turretSubsystem; + private DriveSubsystem driveSubsystem; + + private int[] shootingTagFilters; + + private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + + private Pose3d closestTag; + private Pose3d scoringHubPose; + + public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) { + this.turretSubsystem = turretSubsystem; + this.driveSubsystem = driveSubsystem; + + addRequirements(turretSubsystem); // Not adding drive as we don't wan't the drive command to be de-scheduled + } + + @Override + public void initialize() { + // Perform auto-home + turretSubsystem.homeTurret(); + + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; + + scoringHubPose = fieldTags.getTagPose(26).get(); + scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + } else { + shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; + + scoringHubPose = fieldTags.getTagPose(10).get(); + scoringHubPose = scoringHubPose.transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero))); + } + Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose); + }); + + } + + @Override + public void execute() { + + Pose3d botPose = driveSubsystem.getBotPose(); + Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), + new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngle().in(Units.Rotations))))); + Logger.recordOutput("AutoTurret-TurretPose", turretPose); + + Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); + Logger.recordOutput("AutoTurret-Translation", targetTranslation); + + + if(turretSubsystem.isTurretReady()){ + turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).plus(Rotation2d.k180deg).getMeasure()); + // turretSubsystem.setTurretRotation(Angle.ofRelativeUnits(-180, Units.Degrees)); + } + + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + } + +} diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java new file mode 100644 index 0000000..7481d3c --- /dev/null +++ b/src/main/java/frc/robot/commands/ExampleCommand.java @@ -0,0 +1,43 @@ +// 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.commands; + +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class ExampleCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ExampleSubsystem m_subsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ExampleCommand(ExampleSubsystem subsystem) { + m_subsystem = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index dbe750c..cb397b0 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -26,17 +26,17 @@ public class ManualShootCommand extends Command { public void execute() { if(m_controller.getRightTriggerAxis()>.5 && m_controller.getLeftTriggerAxis()==0) { - m_subsystem.startRotate(-.5); + m_subsystem.turretRotateDir(-.5); m_controller.setRumble(RumbleType.kRightRumble, 1); } else if(m_controller.getLeftTriggerAxis()>.5 && m_controller.getRightTriggerAxis()==0) { - m_subsystem.startRotate(.5); + m_subsystem.turretRotateDir(.5); m_controller.setRumble(RumbleType.kLeftRumble, 1); } else { - m_subsystem.startRotate(0); + m_subsystem.turretRotateDir(0); m_controller.setRumble(RumbleType.kBothRumble, 0); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 9a2838e..b12b5a6 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -11,12 +11,18 @@ 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; @@ -28,6 +34,8 @@ 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; @@ -38,20 +46,24 @@ public class TurretSubsystem extends TestableSubsystem { public enum TurretState { INIT, HOMING, - MANUAL; + READY; } private final boolean LOGGING = true; - private TurretState currentState; + private TurretState turretState; private DigitalInput turretLimitSwitch; private TalonFX turretMotor; + private PositionVoltage turretControlMode = new PositionVoltage(0); + private TalonFXSimState turretSim; + private StatusSignal turretMotorPosition; + private StatusSignal turretMotorForwardLimit; + private StatusSignal turretMotorReverseLimit; private SparkMax launcherMotor; private SparkClosedLoopController launcherControler; + private Angle turretOffset = Angle.ofBaseUnits(0, Units.Degrees); + private Servo hood; - private Angle limitPos; - private Angle limitNeg; - private boolean turretGoingPos; public TurretSubsystem() { @@ -62,9 +74,10 @@ public class TurretSubsystem extends TestableSubsystem { new TalonFXConfiguration() .withSlot0( new Slot0Configs() - .withKP(1.0) - .withKI(0) + .withKP(2) + .withKI(1) .withKD(0) + .withKS(0) ) .withMotorOutput( new MotorOutputConfigs() @@ -72,8 +85,19 @@ public class TurretSubsystem extends TestableSubsystem { ) .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); @@ -92,10 +116,7 @@ public class TurretSubsystem extends TestableSubsystem { // Hood configuration hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL); - currentState = TurretState.INIT; - limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; - limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS; - turretGoingPos = false; + turretState = TurretState.INIT; } public void setLauncher(double speed) { @@ -141,95 +162,89 @@ public class TurretSubsystem extends TestableSubsystem { public void startHoming() { turretMotor.set(Constants.ShooterConstants.HOMING_SPEED); - currentState = TurretState.HOMING; + turretState = TurretState.HOMING; } - public void startRotate(double speed) + public boolean isTurretReady(){ + return turretState == TurretState.READY; + } + + public void turretRotateDir(double speed) { - turretGoingPos=speed>0; - - if(!passedLimit()) - { - turretMotor.set(speed); - } - else - { - turretMotor.set(0); - } - } - - public boolean passedLimit(){ - return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos); + turretMotor.set(speed); } public Angle getTurretAngle() { - return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO); + return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); + } + + private Angle getTurretMotorPosition(){ + return turretMotorPosition.refresh().getValue().minus(turretOffset); } @Override public void periodic() { - // This method will be called once per scheduler run - switch (currentState) { - case HOMING: - periodicHoming(); - break; - case MANUAL: - periodicManual(); - break; - default: - break; + 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-Launcher Volatge", launcherMotor.getOutputCurrent()); + 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()); } } - private void periodicHoming() + public void setTurretRotation(Angle pos) { - if(turretLimitSwitch.get()) + if(turretState != TurretState.READY) { - currentState = TurretState.MANUAL; - turretMotor.set(0); - turretMotor.setPosition(0); - goTo(limitPos); + // Not ready to set angle + return; } + + setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } - private void goTo(Angle pos) - { - if(pos.gt(limitNeg) && pos.lt(limitPos)) - { - while(getTurretAngle()!=pos) - { - if(getTurretAngle().gt(pos)) - { - turretMotor.set(.5); - } - else - { - turretMotor.set(-.5); - } - } - } - } - - private void periodicManual() - { - if(passedLimit()) - { - turretMotor.set(0); - } + public void setTurretMotorRotation(Angle pos){ + turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset))); + turretSim.setRawRotorPosition(pos.plus(turretOffset)); } @PostTest() public void TurretSubsystem_CANDevicesConnected(){ - assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0); + assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse(); assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue(); }