From 4747f6233b1bf91ebac4649beb5ef1ba672e305b Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 18 Mar 2025 11:02:39 -0600 Subject: [PATCH] end of day one --- .../pathplanner/autos/3 Coral Cage 5.auto | 14 ++- .../paths/Bottom Feed to Reef 2.path | 2 +- .../pathplanner/paths/Cage 5 to Reef.path | 31 ++---- .../paths/Reef 2 to Bottom Feed.path | 2 +- .../paths/Reef 3 to Bottom Feed.path | 16 +-- .../paths/Reef 4 to Bottom Feed.path | 2 +- src/main/java/frc4388/robot/Constants.java | 43 +++++--- .../java/frc4388/robot/RobotContainer.java | 102 +++++++++++++++++- src/main/java/frc4388/robot/RobotMap.java | 5 +- .../robot/commands/DriveUntilLiDAR.java | 6 +- .../frc4388/robot/commands/GotoLastApril.java | 21 +++- .../robot/commands/MoveUntilSuply.java | 47 ++++++++ .../frc4388/robot/subsystems/Elevator.java | 8 +- 13 files changed, 243 insertions(+), 56 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/MoveUntilSuply.java 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 034400a..a09d168 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "place-coral-left-l4" + "name": "place-coral-right-l4" } }, { @@ -46,6 +46,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { @@ -76,6 +82,12 @@ "pathName": "Bottom Feed to Reef 2" } }, + { + "type": "named", + "data": { + "name": "await-coral" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index 7118995..521f2b1 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": 59.28109573597083 }, "reversed": false, 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 6401f60..87801e6 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.959532828282828, - "y": 1.5754419191919191 + "x": 6.670197826108406, + "y": 2.0396270358261894 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.349684343434343, - "y": 2.2869318181818175 + "x": 5.754837328767123, + "y": 2.492380136986301 }, "prevControl": { - "x": 5.747433712121213, - "y": 1.9321212121212112 + "x": 6.077263063386753, + "y": 2.432264238502225 }, "nextControl": null, "isLocked": false, @@ -30,18 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.8, - "y": 3.0 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -52,14 +41,14 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, - "rotation": 122.79876573246848 + "velocity": 1.0, + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Barge to Reef", "idealStartingState": { "velocity": 0, - "rotation": 126.34745820888533 + "rotation": 123.5866498700801 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index e7219b4..92cd109 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 55.37584492005105 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 098c67d..0d5a745 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.703267045454546, - "y": 1.075625 + "x": 4.106435503617635, + "y": 1.8391383607824139 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.3559375 }, "prevControl": { - "x": 2.0014457070707063, - "y": 2.5936426767676766 + "x": 2.9441094150144833, + "y": 1.668218251495801 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 55.05578949900953 }, "reversed": false, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 121.42956561483854 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index 38a1be6..dab21be 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 52.857102599919905 }, "reversed": false, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f44d1d1..404dbc9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -54,6 +54,7 @@ import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; import frc4388.utility.ReefPositionHelper; import frc4388.utility.Trim; +import frc4388.utility.configurable.ConfigurableDouble; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -193,7 +194,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(2.5, 0.2, 1); + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); } public static final class Configurations { @@ -298,7 +299,11 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + public static final Gains XY_GAINS = new Gains(8,0,0.0); + // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); + // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); + // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); // 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.007); @@ -316,24 +321,37 @@ public final class Constants { public static final double MIN_XY_PID_OUTPUT = 0.0; public static final double MIN_ROT_PID_OUTPUT = 0.0; - public static final double VELOCITY_THRESHHOLD = 0.005; + public static final double VELOCITY_THRESHHOLD = 0.01; // X is tangent to reef side // Y is normal to reef side public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); + public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); - public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); - // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; + // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); - public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); - 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(0.5); + public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); + public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5); - public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); + public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; + + // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); + // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + + // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + + // 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(0.5); + + // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); public static final int L4_DRIVE_TIME = 250; //Milliseconds // public static final int L3_DRIVE_TIME = 500; @@ -437,7 +455,8 @@ public final class Constants { public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value + public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8fbc18b..c5086ea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -49,6 +50,7 @@ import frc4388.robot.commands.DriveUntilLiDAR; import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.MoveUntilSuply; import frc4388.robot.commands.WhileTrueCommand; import frc4388.robot.commands.waitElevatorRefrence; import frc4388.robot.commands.waitEndefectorRefrence; @@ -148,6 +150,48 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + new ParallelDeadlineGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator), + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), @@ -204,6 +248,53 @@ public class RobotContainer { + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); */ + + private Command AprilLidarAlignL4Left = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + waitDebuger.asProxy(), + new ParallelDeadlineGroup( + new WaitCommand(1), + new MoveUntilSuply( + m_robotSwerveDrive, + new Translation2d(0,-0.5), + new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + ), + new InstantCommand(m_robotSwerveDrive::softStop, m_robotElevator), + + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + + + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + new ConditionalCommand( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + () -> m_robotElevator.hasCoral()), + + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -408,7 +499,7 @@ public class RobotContainer { new Translation2d(0, 1), new Translation2d(), 300, true - ), new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), + ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); @@ -421,7 +512,7 @@ public class RobotContainer { NamedCommands.registerCommand("prepare-l4", new SequentialCommandGroup( // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Hovering)), // new waitElevatorRefrence(m_robotElevator), - new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)), + // new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(1.5)), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour)) )); @@ -545,14 +636,15 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, + new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); // ? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 72bafdd..577f6bf 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -55,11 +55,14 @@ public class RobotMap { /* Elevator Subsystem */ public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - void configureDriveMotorControllers() {} + void configureDriveMotorControllers() { + // endeffector.saf + } } diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java index 937f02c..cda04a7 100644 --- a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java @@ -45,6 +45,10 @@ public class DriveUntilLiDAR extends Command { @Override public boolean isFinished() { - return Math.abs(m_lidar.getDistance()) < mindistance; + if (Math.abs(m_lidar.getDistance()) < mindistance) { + swerveDrive.softStop(); + return true; + } + return false; } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index b11f4d0..0686440 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -50,8 +50,8 @@ public class GotoLastApril extends Command { this.vision = vision; this.distance = distance; this.side = side; - this.waitVelocityZero = waitVelocityZero; - // addRequirements(swerveDrive); + this.waitVelocityZero = waitVelocityZero && false; + addRequirements(swerveDrive); } @@ -62,6 +62,23 @@ public class GotoLastApril extends Command { @Override public void initialize() { + // double kP = AutoConstants.P_XY_GAINS.get(); + // double kI = AutoConstants.I_XY_GAINS.get(); + // double kD = AutoConstants.D_XY_GAINS.get(); + // xPID = new PID(new Gains( + // kP, + // kI, + // kD), + // 0); + // yPID = new PID(new Gains( + // kP, + // kI, + // kD), + // 0); + + // System.out.println("kP: "+kP); + // System.out.println("kI: "+kI); + // System.out.println("kD: "+kD); xPID.initialize(); yPID.initialize(); this.targetpos = ReefPositionHelper.getNearestPosition( diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java new file mode 100644 index 0000000..dd0d3e2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -0,0 +1,47 @@ +package frc4388.robot.commands; + +import java.time.Instant; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.TimesNegativeOne; + +// Command to repeat a joystick movement for a specific time. +public class MoveUntilSuply extends Command { + private final SwerveDrive swerveDrive; + private final Translation2d leftStick; + private final Translation2d rightStick; + private final Supplier truth; + private final boolean robotRelative; + + public MoveUntilSuply( + SwerveDrive swerveDrive, + Translation2d leftStick, + Translation2d rightStick, + Supplier truth, + boolean robotRelative) { + addRequirements(swerveDrive); + + this.swerveDrive = swerveDrive; + this.leftStick = leftStick; + this.rightStick = rightStick; + this.truth = truth; + this.robotRelative = robotRelative; + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); + } + + @Override + public boolean isFinished() { + return truth.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 4526bcd..cbd10bb 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -162,7 +162,7 @@ public class Elevator extends Subsystem { case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } @@ -255,6 +255,10 @@ public class Elevator extends Subsystem { // } // } + public boolean getEndeffectorLimit() { + return endeffectorLimitSwitch.get(); + } + private void periodicWaiting() { if (!basinBeamBreak.get()) transitionState(CoordinationState.Ready); @@ -363,7 +367,7 @@ public class Elevator extends Subsystem { } public boolean hasCoral() { - return elevatorAtReference() && currentState == CoordinationState.Hovering && !endeffectorLimitSwitch.get(); + return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); } public boolean readyToMove() {