From 7dc6105145fd789a8f33cb4d5535c59335736050 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 4 Mar 2025 19:46:52 -0700 Subject: [PATCH] Swerve calibration, L2, Autos --- .../pathplanner/autos/3 Coral Cage 5.auto | 8 +++- .../pathplanner/paths/Cage 5 to Reef.path | 12 ++--- src/main/java/frc4388/robot/Constants.java | 48 ++++--------------- .../java/frc4388/robot/RobotContainer.java | 4 +- .../frc4388/robot/subsystems/Elevator.java | 8 +++- .../frc4388/robot/subsystems/SwerveDrive.java | 2 + .../frc4388/utility/ReefPositionHelper.java | 2 + 7 files changed, 34 insertions(+), 50 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto index 1145cb6..a04fbe0 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, { "type": "path", "data": { @@ -13,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-right-l4" + "name": "place-coral-left-l4" } }, { diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index e015f20..44a5838 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.585795454545455, - "y": 1.9413510101010092 + "x": 7.129687499999999, + "y": 1.9409375 }, "prevControl": null, "nextControl": { - "x": 7.044375, - "y": 1.8921875 + "x": 6.072698863636364, + "y": 1.0434943181818186 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.61125 }, "prevControl": { - "x": 5.3503125, - "y": 2.1603125 + "x": 5.504318181818181, + "y": 2.1902272727272725 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ceeeda9..a24aa67 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -113,7 +113,7 @@ public final class Constants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.4794921875+0.25); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.22705078125+0.5); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -121,7 +121,7 @@ public final class Constants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.07666015625); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -129,7 +129,7 @@ public final class Constants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(-0.867431640625+0.25); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.37646484375); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -137,7 +137,7 @@ public final class Constants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.425537109375-0.25+0.25); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.219970703125+0.5); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -145,40 +145,6 @@ public final class Constants { private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); } - /* private static final class ModuleSpecificConstants { // 2024 - //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; - private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - - //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - } */ - public static final class IDs { public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); @@ -328,10 +294,11 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + public static final Gains XY_GAINS = new Gains(5,0.4,0.0); // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); - public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); @@ -355,6 +322,7 @@ public final class Constants { public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 990fd31..f214584 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -90,7 +90,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.basinLimitSwitch, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -250,6 +250,7 @@ public class RobotContainer { private Command AprilLidarAlignL2Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT), new LidarAlign(m_robotSwerveDrive, m_lidar), @@ -265,6 +266,7 @@ public class RobotContainer { private Command AprilLidarAlignL2Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT), new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index bc41f4b..9f5cf56 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -306,6 +306,10 @@ public class Elevator extends Subsystem { } else if (currentState == CoordinationState.Ready) { periodicReady(); } + + if(!intakeIR.get()){ + led.setMode(LEDConstants.DOWN_PATTERN); + } // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring(); // } @@ -324,8 +328,8 @@ public class Elevator extends Subsystem { } public boolean readyToMove() { - // return !intakeIR.get() || hasCoral(); - return hasCoral(); + return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); + // return hasCoral(); } public void armShuffle(){ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 54855a9..6f7ef5e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -184,10 +184,12 @@ public class SwerveDrive extends Subsystem { public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { stopped = false; // Create robot-relative speeds. + if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index a3ab63e..7c020b7 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -56,6 +56,8 @@ public class ReefPositionHelper { minDistance = dist; } } + + System.out.println(minPos.getRotation().getDegrees()); return minPos; }