diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path index 9597e5f..a2d7feb 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.5546611575005915, - "y": 2.13657272729177 + "x": 5.96969696969697, + "y": 1.128219696969696 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6833333333333327 }, "prevControl": { - "x": 5.563570773660507, - "y": 2.6306173444056773 + "x": 5.75625, + "y": 2.256439393939394 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -62.35402463626126 + "rotation": -60.52411099675423 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3ff474b..2e2cc3a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -250,8 +250,8 @@ public final class Constants { public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help. - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help. + public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. public static final double NEUTRAL_DEADBAND = 0.04; // POWER! (limiting) @@ -283,7 +283,7 @@ public final class Constants { // new ClosedLoopRampsConfigs() // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); - private static final double SLIP_CURRENT = 100; // TODO: Tune??? + public static final double SLIP_CURRENT = 60; // TODO: Tune??? } // No mans land @@ -355,8 +355,9 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = .2794 - ; + // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + ;;;;;;;;;;;;;;;;;;;;;;;;;;;; // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b73b7e5..710dd27 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; -import frc4388.robot.commands.GotoPositionCommand; +import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -106,7 +106,12 @@ public class RobotContainer { () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); - private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); + private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); + private Command AprilLidarAlign = new SequentialCommandGroup( + new GotoLastApril(m_robotSwerveDrive, m_vision), + new InstantCommand(() -> System.out.println("Soup")), + new LidarAlign(m_robotSwerveDrive, m_lidar, true) + ); private Command AprilLidarLeft = new SequentialCommandGroup( AutoGotoPosition.asProxy(), new LidarAlign(m_robotSwerveDrive, m_lidar, false) @@ -114,6 +119,7 @@ public class RobotContainer { private Command AprilLidarRight = new SequentialCommandGroup( AutoGotoPosition.asProxy(), + new InstantCommand(() -> System.out.println("Soup")), new LidarAlign(m_robotSwerveDrive, m_lidar, true) ); @@ -141,7 +147,7 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); - NamedCommands.registerCommand("april-allign", AutoGotoPosition); + NamedCommands.registerCommand("april-allign", AprilLidarAlign); NamedCommands.registerCommand("place-coral", placeCoral); NamedCommands.registerCommand("grab-coral", grabCoral); @@ -218,6 +224,15 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java similarity index 97% rename from src/main/java/frc4388/robot/commands/GotoPositionCommand.java rename to src/main/java/frc4388/robot/commands/GotoLastApril.java index 593e812..168055e 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -15,7 +15,7 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.VirtualController; -public class GotoPositionCommand extends Command { +public class GotoLastApril extends Command { // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); @@ -34,7 +34,7 @@ public class GotoPositionCommand extends Command { * @param SwerveDrive m_robotSwerveDrive */ - public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { + public GotoLastApril(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; // addRequirements(swerveDrive); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 8b96ad9..7525718 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -6,6 +6,7 @@ package frc4388.robot.commands; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; @@ -56,8 +57,10 @@ public class LidarAlign extends Command { headedRight = !headedRight; currentFinderTick *= -1; } - - double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef + double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; + double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef + SmartDashboard.putNumber("Rel Angle", relAngle); + SmartDashboard.putNumber("heading", currentHeading); if (!headedRight) { swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); } else { diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index f7fa030..78c7588 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -24,9 +24,14 @@ public class Elevator extends SubsystemBase { public enum CoordinationState { Waiting, // for coral into the though Ready, // Has coral in enefector + PrimedThree, // Arm and elevator Waiting to score in the level 3 position ScoringThree, // Arm and elevator in the level three position - ScoringFour // Arm and elevator in the level four position - + PrimedFour, // Arm and elevator Waiting to score in the level 4 position + ScoringFour, // Arm and elevator in the level four position + BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef. + BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef. + BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef. + BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef. } private CoordinationState currentState; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a16246f..812f16a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -8,9 +8,11 @@ import java.util.Optional; import java.util.function.DoubleSupplier; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; @@ -227,6 +229,26 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.setControl(ctrl); } + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + public void activateLuigiMode() { + setLimits(20); + } + + public void deactivateLuigiMode() { + setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + } + public boolean rotateToTarget(double angle) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(0) @@ -258,7 +280,11 @@ public class SwerveDrive extends Subsystem { } public double getGyroAngle() { - return swerveDriveTrain.getRotation3d().getAngle(); + return getPose2d().getRotation().getRadians(); + } + + public Pose2d getPose2d() { + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); } public void resetGyro() { @@ -283,7 +309,7 @@ public class SwerveDrive extends Subsystem { @Override public void periodic() { // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); SmartDashboard.putNumber("RotTartget", rotTarget); double time = Vision.getTime();