diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 62a15ef..dfcf13b 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 397d7ad..54fa268 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,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,9 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); + // Configure the trigger bindings configureBindings(); } @@ -57,7 +61,6 @@ public class RobotContainer { */ private void configureBindings() { - turretSubsystem.startHoming(); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index fc9a56b..f28a7b0 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -105,7 +105,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState", "Look for tag"); 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..c30401b --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -0,0 +1,87 @@ +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.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.k2026RebuiltAndymark); + + 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 + + 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()); + + turretSubsystem.setTurretRotation(botPose.getRotation().toRotation2d().minus(targetTranslation.getAngle()).getMeasure()); + + } + + @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 87af396..a172c06 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.sim.TalonFXSimState; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; @@ -43,6 +44,7 @@ public class TurretSubsystem extends TestableSubsystem { private TurretState currentState; private DigitalInput turretLimitSwitch; private TalonFX turretMotor; + private TalonFXSimState turretSim; private SparkMax launcherMotor; private SparkClosedLoopController launcherControler; @@ -73,6 +75,8 @@ public class TurretSubsystem extends TestableSubsystem { ) ); + turretSim = turretMotor.getSimState(); + // Launcher configuration launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); @@ -119,7 +123,7 @@ public class TurretSubsystem extends TestableSubsystem { currentState = TurretState.HOMING; } - public void startRotate(double speed) + public void turretRotateDir(double speed) { turretGoingPos=speed>0; @@ -139,23 +143,24 @@ public class TurretSubsystem extends TestableSubsystem { public Angle getTurretAngle() { - return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO); + return turretMotor.getPosition().refresh().getValue().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); } + @Override public void periodic() { // This method will be called once per scheduler run - switch (currentState) { - case HOMING: - periodicHoming(); - break; - case MANUAL: - periodicManual(); - break; + // switch (currentState) { + // case HOMING: + // periodicHoming(); + // break; + // case MANUAL: + // periodicManual(); + // break; - default: - break; - } + // default: + // break; + // } if(LOGGING) { @@ -171,26 +176,14 @@ public class TurretSubsystem extends TestableSubsystem { currentState = TurretState.MANUAL; turretMotor.set(0); turretMotor.setPosition(0); - goTo(limitPos); + setTurretRotation(limitPos); } } - private void goTo(Angle pos) + public void setTurretRotation(Angle pos) { - if(pos.gt(limitNeg) && pos.lt(limitPos)) - { - while(getTurretAngle()!=pos) - { - if(getTurretAngle().gt(pos)) - { - turretMotor.set(.5); - } - else - { - turretMotor.set(-.5); - } - } - } + turretMotor.setControl(new PositionVoltage(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO))); + turretSim.setRawRotorPosition(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } private void periodicManual() @@ -203,7 +196,7 @@ public class TurretSubsystem extends TestableSubsystem { @PostTest() public void TurretSubsystem_CANDevicesConnected(){ - assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0); + assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isTrue(); assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue(); }