diff --git a/src/main/deploy/pathplanner/BumpAuto04Balance.path b/src/main/deploy/pathplanner/BumpAuto04Balance.path new file mode 100644 index 0000000..e1f641e --- /dev/null +++ b/src/main/deploy/pathplanner/BumpAuto04Balance.path @@ -0,0 +1,84 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.073997477964574, + "y": 2.15586199878997 + }, + "prevControl": null, + "nextControl": { + "x": 6.644274052436994, + "y": 2.335755157621427 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.254650502356561, + "y": 2.52 + }, + "prevControl": { + "x": 6.220418304465077, + "y": 2.52 + }, + "nextControl": { + "x": 5.030300017255194, + "y": 2.52 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 0.25, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8007762890525787, + "y": 2.52 + }, + "prevControl": { + "x": 4.289432841687728, + "y": 2.52 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 3.25, + "maxAcceleration": 3.0, + "isReversed": null, + "markers": [ + { + "position": 0.05, + "names": [ + "GoHome" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/NoBumpAuto04Balance.path b/src/main/deploy/pathplanner/NoBumpAuto04Balance.path new file mode 100644 index 0000000..108e9dd --- /dev/null +++ b/src/main/deploy/pathplanner/NoBumpAuto04Balance.path @@ -0,0 +1,90 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.7699638687309065, + "y": 3.5991968997826884 + }, + "prevControl": null, + "nextControl": { + "x": 6.7699638687309065, + "y": 3.358082547089816 + }, + "holonomicAngle": 145.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.435283007718793, + "y": 2.52 + }, + "prevControl": { + "x": 6.369127568086373, + "y": 2.52 + }, + "nextControl": { + "x": 4.976865268451309, + "y": 2.52 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 0.25, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8991238746366386, + "y": 2.52 + }, + "prevControl": { + "x": 4.1691650440779355, + "y": 2.52 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 3.25, + "maxAcceleration": null, + "isReversed": null, + "markers": [ + { + "position": 1.25, + "names": [ + "ConeScorePrep" + ] + }, + { + "position": 0.03, + "names": [ + "GoHome" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8a52e4..9b3029c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { - public static final ArmProfile ARM_A = new ArmProfile(1012, -322.037); // +1 + public static final ArmProfile ARM_A = new ArmProfile(786, -322.037); // +1 public static final ArmProfile ARM_B = new ArmProfile(687, -201); // +1.412 public static ArmProfile INSTALLED_ARM = true ? ARM_A : ARM_B; @@ -97,7 +97,8 @@ public static class PositionConfigs { new PositionConfig(1, 83, 1850 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); public static final PositionConfig BACK_CONE_FLOOR = - new PositionConfig(1, 82.7, 1735 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); + new PositionConfig(1, 75.1, 2130 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); // 1880 + // new PositionConfig(1, 81.7, 1880 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); //1880 public static final PositionConfig BACK_CONE_FLOOR_TIPPED = // not a good angle with new LL mounts @@ -110,7 +111,7 @@ public static class PositionConfigs { new PositionConfig(38, 49.5, 2800 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); public static final PositionConfig BACK_CUBE_FLOOR = - new PositionConfig(4, 98.2, 2731 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); + new PositionConfig(4, 98.2, 2746 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); public static final PositionConfig BACK_CUBE_FLOOR_LONG = new PositionConfig(27, 88, 2076 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); @@ -139,13 +140,13 @@ public static class PositionConfigs { new PositionConfig(20.7, -43.5, 300 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); public static final PositionConfig FRONT_CONE_MEDIUM_AUTO = - new PositionConfig(18, -40, 125 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); + new PositionConfig(22, -40, 225 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); public static final PositionConfig FRONT_CONE_TOP = new PositionConfig(37.5, -45.6, 433 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); public static final PositionConfig FRONT_CONE_TOP_AUTO = - new PositionConfig(37.5, -43.6, 340 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); + new PositionConfig(38.5, -43.0, 450 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); public static final PositionConfig FRONT_CUBE_FLOOR = new PositionConfig( @@ -161,13 +162,14 @@ public static class PositionConfigs { new PositionConfig(5, -47, 1016 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); public static final PositionConfig FRONT_CUBE_TOP = - new PositionConfig(22, -57.8, 2045 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); + new PositionConfig(23, -57.8, 2045 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); public static final PositionConfig FRONT_CUBE_TOP_AUTO = new PositionConfig(24, -57.8, 2045 + INSTALLED_ARM.getWristOffset(), GamePiece.CUBE); public static final PositionConfig BACK_DOUBLE_SUBSTATION = - new PositionConfig(22.25, 27, 2390 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); + new PositionConfig( + 23.25, 24.22, 2575 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); // 2575 public static final PositionConfig FRONT_SINGLE_SUBSTATION = new PositionConfig(0, -72, 2330 + INSTALLED_ARM.getWristOffset(), GamePiece.CONE); diff --git a/src/main/java/frc/robot/DriveControls.txt b/src/main/java/frc/robot/DriveControls.txt new file mode 100644 index 0000000..2db2bb9 --- /dev/null +++ b/src/main/java/frc/robot/DriveControls.txt @@ -0,0 +1,21 @@ +Driver Controls: +BackButton - Zero Gyroscope +RightBumper - Collect Cube Floor +RightTrigger - Reduce Speed +LeftTrigger - Reduce Speed, Align Straight +LeftBumper - Eject / Score +B - Collect Double Substation +StartButton - Go home +A - Score +Y - Collect Cone Floor + +CoDriver Controls: +RightBumper - Correctional Grab GamePiece +LeftBumper - Eject (No home) +LeftTrigger - Home Wrist +StartButton - Increase Wrist Offset +BackButton - Decrease Wrist Offset +A - Set Height Floor +B - Set Height Medium +Y - Set Height High +X - Zero Arm Extension \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 29e292d..a3bb93c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -125,7 +125,6 @@ public void autonomousInit() { // schedule the autonomous command if (autonomousCommand != null) { - robotContainer.setShouldUseVision(false); autonomousCommand.schedule(); } } @@ -145,7 +144,6 @@ public void teleopInit() { } robotContainer.enableFieldRelative(); robotContainer.disableXstance(); - robotContainer.setShouldUseVision(false); } /** This method is invoked at the start of the test period. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1fe25c..ddb6535 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,10 +27,13 @@ import frc.robot.commands.arm.SetArm; import frc.robot.commands.auto.AutoPathHelper; import frc.robot.commands.auto.AutoScoreSequenceNoHome; +import frc.robot.commands.auto.AutoScoreSequenceNoHomeWait; import frc.robot.commands.auto.Balance; import frc.robot.commands.auto.BumpAuto2Cubes; +import frc.robot.commands.auto.BumpAuto2CubesBalance; import frc.robot.commands.auto.DriveToScore; import frc.robot.commands.auto.NoBumpAuto2Cubes; +import frc.robot.commands.auto.NoBumpAuto2CubesBalance; import frc.robot.commands.auto.PreBalance; import frc.robot.commands.wrist.HomeWrist; import frc.robot.configs.CompRobotConfig; @@ -60,8 +63,6 @@ public class RobotContainer { private Wrist wrist; private Claw claw; private Height height = Height.HIGH; - // private MultiLimelight multiLimelight; - private boolean shouldUseVision = false; private boolean doubleSubstation = false; @@ -146,14 +147,6 @@ public RobotContainer() { wrist = new Wrist(new WristIOTalonSRX(config.getWristRotatorID())); claw = new Claw(new ClawIOSparkMAX(config.getClawMotorID())); claw.setDefaultCommand(new DefaultClawRollersSpin(claw)); - // multiLimelight = - // new MultiLimelight( - // () -> - // (Math.abs(drivetrain.getCharacterizationVelocity()) < 0.025 - // // velocity can accept pose - // && shouldUseVision), // see how high this can go - // "limelight-fl", - // "limelight-fr"); arm = new Arm( @@ -383,11 +376,11 @@ private void configureButtonBindings() { new Trigger(coDriverController::getStartButton) .onTrue( new InstantCommand( - () -> Constants.INSTALLED_ARM.addMatchOffset(29))); // Increase wrist Offset + () -> Constants.INSTALLED_ARM.addMatchOffset(22))); // Increase wrist Offset new Trigger(coDriverController::getBackButton) .onTrue( new InstantCommand( - () -> Constants.INSTALLED_ARM.addMatchOffset(-29))); // Decrease wrist offset + () -> Constants.INSTALLED_ARM.addMatchOffset(-22))); // Decrease wrist offset new Trigger(coDriverController::getAButton) .onTrue(new InstantCommand(() -> this.setHeight(Height.FLOOR))); @@ -433,14 +426,6 @@ private void configureButtonBindings() { claw, () -> Constants.PositionConfigs.BACK_CONE_FLOOR, driverController)); - new Trigger(driverController::getXButton) - .onTrue( - new CollectSequence( - arm, - wrist, - claw, - () -> Constants.PositionConfigs.BACK_CONE_FLOOR_TIPPED, - driverController)); } /** Use this method to define your commands for autonomous mode. */ @@ -455,11 +440,6 @@ private void configureAutoCommands() { autoEventMap.put("Auto Balance", new Balance(drivetrain)); - // eventMap01.put("GoHome", new GoHome(arm, wrist)); - // eventMap02.put("CubeScorePrep", new ParallelCommandGroup(new SetArm(arm, () -> -49.5, () -> - // 4, () -> true), new SetWristPosition(Wrist.ANGLE_STRAIGHT, wrist))); - // eventMap02.put("GoHome", new GoHome(arm, wrist).withTimeout(0.5)); - autoEventMap.put("Balance", new PreBalance(drivetrain)); autoEventMap.put( @@ -526,87 +506,19 @@ private void configureAutoCommands() { autoEventMap.put("Score Cube Prep Medium", new SetArm(arm, () -> -49.5, () -> 1, () -> true)); - // autoChooser.addOption( - // "TwoPieceBalanceCable", - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO) - // .andThen( - // AutoPathHelper.followPath( - // drivetrain, "TwoPieceBalanceCable", autoEventMap, 3.25, 2.5))); - - // autoChooser.addOption( - // "ThreePieceNoCable", - // new InstantCommand(() -> claw.setCone(), claw) - // .andThen( - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO)) - // .andThen(new GoHome(arm, wrist).withTimeout(1.0)) - // .andThen(new InstantCommand(() -> arm.setExtensionNominal())) - // .andThen( - // AutoPathHelper.followPath(drivetrain, "ThreePieceNoCable", autoEventMap, 2.75, - // 2.5)) - // .andThen(new EjectGamePiece(claw).withTimeout(0.3)) - // .andThen(new GoHome(arm, wrist))); - - // autoChooser.addOption( - // "ThreePieceCable", - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO) - // .andThen( - // AutoPathHelper.followPath(drivetrain, "ThreePieceCable", autoEventMap, 3.5, - // 2.25))); - - // autoChooser.addOption( - // "Barker3PieceTwisty", - // new InstantCommand(() -> setShouldUseVision(true)) - // .andThen( - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO)) - // .andThen( - // AutoPathHelper.followPath( - // drivetrain, "Barker3PieceTwisty", autoEventMap, 3.25, 2.35)) - // .andThen(new GoHome(arm, wrist))); - - // autoChooser.addOption( - // "2910 Red", - // (new InstantCommand(() -> setShouldUseVision(true))) - // .andThen( - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO)) - // .andThen(AutoPathHelper.followPath(drivetrain, "2910 Red", autoEventMap, 3.25, 2.75)) - // .andThen(new GoHome(arm, wrist))); - - // autoChooser.addOption( - // "2910 Blue", - // new InstantCommand(() -> setShouldUseVision(true)) - // .andThen( - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO)) - // .andThen(AutoPathHelper.followPath(drivetrain, "2910 Blue", autoEventMap, 3.25, - // 2.75)) - // .andThen(new GoHome(arm, wrist))); - - // autoChooser.addOption( - // "TwoPieceBalanceNoCable", - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO) - // .andThen(new GoHome(arm, wrist).withTimeout(1)) - // .andThen( - // AutoPathHelper.followPath(drivetrain, "TwoPieceNoCable", autoEventMap, 3.25, - // 2.5)) - // .andThen(new Balance(drivetrain))); - - // autoChooser.addOption( - // "ThreePieceBalance", - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO) - // .andThen( - // AutoPathHelper.followPath(drivetrain, "ThreePieceBalance", autoEventMap, 3.75, - // 2.7)) - // .andThen(new PreBalance(drivetrain))); + autoChooser.addOption( + "Thursday Test", + new SequentialCommandGroup( + new InstantCommand(() -> claw.setCone(), claw), + new AutoScoreSequenceNoHomeWait( + arm, + wrist, + claw, + () -> Constants.PositionConfigs.BACK_DOUBLE_SUBSTATION), // FRONT_CONE_MEDIUM + new GoHome(arm, wrist))); autoChooser.addOption( - "MobilityConeNoBump", + "TaxiConeNoBumpSideBalance", new WaitCommand(0) // Max of 2 seconds .andThen( new AutoScoreSequenceNoHome( @@ -616,20 +528,8 @@ private void configureAutoCommands() { .andThen(new GoHome(arm, wrist)) .andThen(new Balance(drivetrain))); - // autoChooser.addOption( - // "MobilityConeTest1NO BUMP", - // new WaitCommand(0) // Max of 2 seconds - // .andThen( - // new AutoScoreSequenceNoHome( - // arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO)) - // .andThen(new GoHome(arm, wrist)) - // .andThen( - // AutoPathHelper.followPath(drivetrain, "MobilityConeTest1", autoEventMap, 1.5, 1)) - // .andThen(new GoHome(arm, wrist)) - // .andThen(new Balance(drivetrain))); - autoChooser.addOption( - "MobilityConeBump", + "TaxiConeBumpSideBalance", new WaitCommand(0) // Max of 2 seconds .andThen( new AutoScoreSequenceNoHome( @@ -640,8 +540,15 @@ private void configureAutoCommands() { .andThen(new GoHome(arm, wrist)) .andThen(new Balance(drivetrain))); - autoChooser.addOption("BumpAutoAlign", new BumpAuto2Cubes(arm, claw, drivetrain, wrist)); - autoChooser.addOption("NoBumpAutoAlign", new NoBumpAuto2Cubes(arm, claw, drivetrain, wrist)); + autoChooser.addOption("Bump3Piece", new BumpAuto2Cubes(arm, claw, drivetrain, wrist)); + autoChooser.addOption( + "Bump2PieceBalance", + new BumpAuto2CubesBalance(arm, claw, drivetrain, wrist).andThen(new Balance(drivetrain))); + + autoChooser.addOption("NoBump3Piece", new NoBumpAuto2Cubes(arm, claw, drivetrain, wrist)); + autoChooser.addOption( + "NoBump2PieceBalance", + new NoBumpAuto2CubesBalance(arm, claw, drivetrain, wrist).andThen(new Balance(drivetrain))); TAB_MATCH.add(autoChooser); @@ -649,7 +556,25 @@ private void configureAutoCommands() { TAB_MATCH.add("Re-Home Wrist", new HomeWrist(wrist)); TAB_MATCH.addBoolean("Cone", () -> claw.isCone()); TAB_MATCH.addBoolean("Cube", () -> claw.isCube()); - TAB_MATCH.add("Stop Wrist Motor", new InstantCommand(() -> wrist.setPercent(0))); + TAB_MATCH.add( + "Force Cone Eject", + new SequentialCommandGroup( + new InstantCommand(() -> claw.setGamePiece(GamePiece.CONE)), + new EjectGamePiece(claw), + new WaitCommand(1.5), + new InstantCommand(() -> claw.setGamePiece(GamePiece.NONE)), + new InstantCommand(() -> claw.setRollerSpeed(0.0)))); + TAB_MATCH.add( + "Force Cube Eject", + new SequentialCommandGroup( + new InstantCommand(() -> claw.setGamePiece(GamePiece.CUBE)), + new EjectGamePiece(claw), + new WaitCommand(1.5), + new InstantCommand(() -> claw.setGamePiece(GamePiece.NONE)), + new InstantCommand(() -> claw.setRollerSpeed(0.0)))); + TAB_MATCH.add( + "Stop Wrist Motor", + new InstantCommand(() -> wrist.setPercent(0)).andThen(new Balance(drivetrain))); TAB_TEST.add( "ScoreConeMid", @@ -691,10 +616,6 @@ public void disableXstance() { drivetrain.disableXstance(); } - public void setShouldUseVision(boolean shouldUse) { - shouldUseVision = shouldUse; - } - public void setLimelightPipeline(String limeLightName, int pipelineID) { LimelightHelpers.setPipelineIndex(limeLightName, pipelineID); } diff --git a/src/main/java/frc/robot/commands/CollectGamePiece.java b/src/main/java/frc/robot/commands/CollectGamePiece.java index b5aca0c..b0715ac 100644 --- a/src/main/java/frc/robot/commands/CollectGamePiece.java +++ b/src/main/java/frc/robot/commands/CollectGamePiece.java @@ -18,10 +18,10 @@ public class CollectGamePiece extends CommandBase { private Debouncer debouncer; private boolean isCollected; - private static final double COLLECTION_TIME = 0.1; + private static final double COLLECTION_TIME = 0.2; private static final double CLAW_ROLLER_SPEED = 0.95; - private static final double MAXIMUM_CURRENT = 38.5; - private static final double MAXIMUM_CURRENT_CUBE = 15.0; + private static final double MAXIMUM_CURRENT = 30.0; + private static final double MAXIMUM_CURRENT_CUBE = 12.0; private final GamePiece piece; diff --git a/src/main/java/frc/robot/commands/DefaultClawRollersSpin.java b/src/main/java/frc/robot/commands/DefaultClawRollersSpin.java index 04b6cee..2e5ca8e 100644 --- a/src/main/java/frc/robot/commands/DefaultClawRollersSpin.java +++ b/src/main/java/frc/robot/commands/DefaultClawRollersSpin.java @@ -11,6 +11,8 @@ public class DefaultClawRollersSpin extends CommandBase { private final Claw claw; + public static final double CONE_ROLLER_SPEED = 0.1; + public static final double CUBE_ROLLER_SPEED = -0.05; public static final double CLAW_ROLLER_SPEED = -0.05; public DefaultClawRollersSpin(Claw claw) { @@ -22,7 +24,7 @@ public DefaultClawRollersSpin(Claw claw) { public void execute() { // GamePiece gamePiece = claw.getGamePiece(); // double speed = claw.getSpeedPercent(); - claw.setRollerSpeed(claw.isCone() ? -CLAW_ROLLER_SPEED : CLAW_ROLLER_SPEED); + claw.setRollerSpeed(claw.isCone() ? CONE_ROLLER_SPEED : CUBE_ROLLER_SPEED); // if (gamePiece == GamePiece.CONE && speed != 0.0) { // claw.setRollerSpeed(0.0); diff --git a/src/main/java/frc/robot/commands/auto/AutoScoreSequenceNoHomeWait.java b/src/main/java/frc/robot/commands/auto/AutoScoreSequenceNoHomeWait.java index 3f97920..e5543c0 100644 --- a/src/main/java/frc/robot/commands/auto/AutoScoreSequenceNoHomeWait.java +++ b/src/main/java/frc/robot/commands/auto/AutoScoreSequenceNoHomeWait.java @@ -26,7 +26,7 @@ public AutoScoreSequenceNoHomeWait( final Supplier positionConfigSupplier) { addCommands( new ScoreSequence(arm, wrist, positionConfigSupplier), - new WaitCommand(.1), + new WaitCommand(.25), new EjectGamePiece(claw).withTimeout(0.16)); } } diff --git a/src/main/java/frc/robot/commands/auto/BumpAuto2Cubes.java b/src/main/java/frc/robot/commands/auto/BumpAuto2Cubes.java index 3db86cb..8693039 100644 --- a/src/main/java/frc/robot/commands/auto/BumpAuto2Cubes.java +++ b/src/main/java/frc/robot/commands/auto/BumpAuto2Cubes.java @@ -9,10 +9,8 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import frc.robot.RobotContainer; -import frc.robot.commands.AutoScoreSequence; import frc.robot.commands.CollectSequenceNoHome; import frc.robot.commands.GoHome; import frc.robot.commands.SetWristPosition; @@ -89,7 +87,7 @@ public BumpAuto2Cubes(final Arm arm, final Claw claw, final Drivetrain drive, fi () -> Constants.PositionConfigs.FRONT_CUBE_TOP.getArmLength() - 8, () -> true), new SetWristPosition(2045 + Constants.INSTALLED_ARM.getWristOffset(), wrist)) - .withTimeout(5)), + .withTimeout(1.5)), new AutoScoreSequenceNoHome( arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CUBE_TOP), new ParallelCommandGroup( @@ -97,7 +95,7 @@ public BumpAuto2Cubes(final Arm arm, final Claw claw, final Drivetrain drive, fi AutoPathHelper.followPathNoRotationReset( drive, "BumpAuto03", eventMap03, MAX_VELOCITY, MAX_ACCELERATION)), new ParallelRaceGroup( - new DriveToPiece(drive, () -> -1.875, GamePiece.CUBE), + new DriveToPiece(drive, () -> -1.7, GamePiece.CUBE), new CollectSequenceNoHome( arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR)), new ParallelCommandGroup( @@ -107,20 +105,20 @@ public BumpAuto2Cubes(final Arm arm, final Claw claw, final Drivetrain drive, fi drive, "BumpAuto04", eventMap04, MAX_VELOCITY, MAX_ACCELERATION)), // Step 9: Score the second game piece new ParallelRaceGroup( - new ParallelCommandGroup( - new SetArm( - arm, - () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getArmRotation(), - () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getArmLength(), - () -> true), - new SetWristPosition( - (int) Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getWristRotation() - + Constants.INSTALLED_ARM.getWristOffset(), - wrist), - new WaitCommand(10)), + // new ParallelCommandGroup( + // new SetArm( + // arm, + // () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getArmRotation(), + // () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getArmLength(), + // () -> true), + // new SetWristPosition( + // (int) Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getWristRotation() + // + Constants.INSTALLED_ARM.getWristOffset(), + // wrist), + // new WaitCommand(10)), new DriveToScore(drive, claw).withTimeout(5)), - new AutoScoreSequence( - arm, wrist, claw, () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE), + // new AutoScoreSequence( + // arm, wrist, claw, () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE), new InstantCommand(() -> RobotContainer.setAprilTagPipeline())); } } diff --git a/src/main/java/frc/robot/commands/auto/BumpAuto2CubesBalance.java b/src/main/java/frc/robot/commands/auto/BumpAuto2CubesBalance.java new file mode 100644 index 0000000..8262e1a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/BumpAuto2CubesBalance.java @@ -0,0 +1,125 @@ +// 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.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.commands.CollectSequenceNoHome; +import frc.robot.commands.GoHome; +import frc.robot.commands.SetWristPosition; +import frc.robot.commands.arm.SetArm; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.claw.Claw; +import frc.robot.subsystems.claw.Claw.GamePiece; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.wrist.Wrist; +import java.util.HashMap; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class BumpAuto2CubesBalance extends SequentialCommandGroup { + + public final double MAX_VELOCITY = 3.35; + public final double MAX_ACCELERATION = 3.25; + + public final HashMap eventMap01 = new HashMap<>(); + public final HashMap eventMap02 = new HashMap<>(); + public final HashMap eventMap03 = new HashMap<>(); + public final HashMap eventMap04 = new HashMap<>(); + + /** Creates a new BumpAuto2Cubes. */ + public BumpAuto2CubesBalance( + final Arm arm, final Claw claw, final Drivetrain drive, final Wrist wrist) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + eventMap01.put("GoHome", new GoHome(arm, wrist)); + eventMap02.put( + "CubeScorePrep", + new ParallelCommandGroup( + new SetArm(arm, () -> -49.5, () -> 8, () -> true), + new SetWristPosition(2045 + Constants.INSTALLED_ARM.getWristOffset(), wrist))); + eventMap02.put("GoHome", new GoHome(arm, wrist).withTimeout(0.5)); + eventMap03.put("GoHome", new GoHome(arm, wrist)); + eventMap03.put( + "CubeCollectPrep", + new ParallelCommandGroup( + new SetArm(arm, () -> 25, () -> 4, () -> true), + new SetWristPosition(2731 + Constants.INSTALLED_ARM.getWristOffset(), wrist))); + + eventMap04.put("GoHome", new GoHome(arm, wrist)); + eventMap04.put( + "CubeScorePrep", + new ParallelCommandGroup( + new SetArm(arm, () -> -65.4, () -> 6, () -> true), + new SetWristPosition(2045 + Constants.INSTALLED_ARM.getWristOffset(), wrist))); + + addCommands( + new InstantCommand(() -> claw.setCone(), claw), + new AutoScoreSequenceNoHomeWait( + arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_MEDIUM), + new ParallelCommandGroup( + new InstantCommand(() -> RobotContainer.setCubePipeline()), + AutoPathHelper.followPath( + drive, "BumpAuto01", eventMap01, MAX_VELOCITY, MAX_ACCELERATION)), + new ParallelRaceGroup( + new DriveToPiece(drive, () -> -2.375, GamePiece.CUBE), + new CollectSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR) + .andThen(new InstantCommand(() -> claw.setGamePiece(GamePiece.CUBE)))), + new ParallelCommandGroup( + new InstantCommand(() -> RobotContainer.setAprilTagPipeline()), + AutoPathHelper.followPathNoRotationReset( + drive, "BumpAuto02", eventMap02, MAX_VELOCITY, MAX_ACCELERATION)), + new ParallelRaceGroup( + new DriveToScore(drive, claw).withTimeout(1.5), + new ParallelCommandGroup( + new SetArm( + arm, + () -> Constants.PositionConfigs.FRONT_CUBE_MEDIUM_AUTO.getArmRotation(), + () -> Constants.PositionConfigs.FRONT_CUBE_MEDIUM_AUTO.getArmLength() - 8, + () -> true), + new SetWristPosition(2045 + Constants.INSTALLED_ARM.getWristOffset(), wrist)) + .withTimeout(5)), + new AutoScoreSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CUBE_MEDIUM_AUTO), + new ParallelCommandGroup( + new InstantCommand(() -> RobotContainer.setConePipeline()), + AutoPathHelper.followPathNoRotationReset( + drive, "BumpAuto03", eventMap03, MAX_VELOCITY, MAX_ACCELERATION)), + new ParallelRaceGroup( // Add short timeout and go home to ensure balance + new DriveToPiece(drive, () -> -1.5, GamePiece.CONE), + new CollectSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CONE_FLOOR)), + new ParallelCommandGroup( + new InstantCommand(() -> claw.setGamePiece(GamePiece.CONE)), + AutoPathHelper.followPathNoReset( + drive, "BumpAuto04Balance", eventMap04, MAX_VELOCITY, MAX_ACCELERATION)), + + // Step 9: Score the second game piece + // new ParallelRaceGroup( + // new ParallelCommandGroup( + // new SetArm( + // arm, + // () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getArmRotation(), + // () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getArmLength(), + // () -> true), + // new SetWristPosition( + // (int) Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE.getWristRotation() + // + Constants.INSTALLED_ARM.getWristOffset(), + // wrist), + // new WaitCommand(10)), + // new DriveToScore(drive, claw).withTimeout(5)), + // new AutoScoreSequence( + // arm, wrist, claw, () -> Constants.PositionConfigs.LAUNCH_LAST_AUTO_CUBE), + new InstantCommand(() -> RobotContainer.setAprilTagPipeline())); + } +} diff --git a/src/main/java/frc/robot/commands/auto/DriveToScore.java b/src/main/java/frc/robot/commands/auto/DriveToScore.java index 4afad78..0bf90f9 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToScore.java +++ b/src/main/java/frc/robot/commands/auto/DriveToScore.java @@ -33,7 +33,7 @@ public DriveToScore(final Drivetrain drive, final Claw claw) { m_xController.setTolerance(2.0); m_yController = new PIDController(0.075, 0, 0); - m_yController.setTolerance(3.0); + m_yController.setTolerance(4.0); m_yController.enableContinuousInput(-28.5, 28.5); m_yController.setSetpoint(0); diff --git a/src/main/java/frc/robot/commands/auto/NoBumpAuto2Cubes.java b/src/main/java/frc/robot/commands/auto/NoBumpAuto2Cubes.java index 2097278..57f0c91 100644 --- a/src/main/java/frc/robot/commands/auto/NoBumpAuto2Cubes.java +++ b/src/main/java/frc/robot/commands/auto/NoBumpAuto2Cubes.java @@ -74,9 +74,10 @@ public NoBumpAuto2Cubes( new InstantCommand(() -> RobotContainer.setCubePipeline())), // Step 3: Collect the first game piece new ParallelRaceGroup( - new DriveToPiece(drive, () -> -2.25, GamePiece.CUBE), - new CollectSequenceNoHome( - arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR)), + new DriveToPiece(drive, () -> -2.25, GamePiece.CUBE), + new CollectSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR)) + .withTimeout(3.25), new WaitCommand(0.06) .andThen( () -> { @@ -86,7 +87,7 @@ public NoBumpAuto2Cubes( // Step 4: Drive almost to the scoring location AutoPathHelper.followPathNoReset(drive, "NoBumpAuto02", eventMap02, MAX_V, MAX_A), // Step 5: Score the first game piece - new DriveToScore(drive, claw).withTimeout(2.5), + new DriveToScore(drive, claw).withTimeout(1.375), new AutoScoreSequenceNoHome( arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CUBE_TOP_AUTO), // Step 6: Drive almost to the second game piece @@ -96,9 +97,10 @@ public NoBumpAuto2Cubes( new InstantCommand(() -> RobotContainer.setCubePipeline())), // Shouldnt need // Step 7: Collect the second game piece new ParallelRaceGroup( - new DriveToPiece(drive, () -> -2.25, GamePiece.CUBE), - new CollectSequenceNoHome( - arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR)), + new DriveToPiece(drive, () -> -2.0, GamePiece.CUBE), + new CollectSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR)) + .withTimeout(3.25), new WaitCommand(0.04), // Step 8: Drive almost to the scoring location new ParallelCommandGroup( @@ -109,7 +111,7 @@ public NoBumpAuto2Cubes( }), AutoPathHelper.followPathNoReset(drive, "NoBumpAuto04Cube", eventMap04, MAX_V, MAX_A)), // Step 9: Score the second game piece - new DriveToScore(drive, claw).withTimeout(5), + new DriveToScore(drive, claw).withTimeout(1.375), new AutoScoreSequence( arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CUBE_MEDIUM_AUTO), new InstantCommand( diff --git a/src/main/java/frc/robot/commands/auto/NoBumpAuto2CubesBalance.java b/src/main/java/frc/robot/commands/auto/NoBumpAuto2CubesBalance.java new file mode 100644 index 0000000..0f367c5 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/NoBumpAuto2CubesBalance.java @@ -0,0 +1,117 @@ +// 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.auto; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; +import frc.robot.RobotContainer; +import frc.robot.commands.CollectSequenceNoHome; +import frc.robot.commands.GoHome; +import frc.robot.commands.SetWristPosition; +import frc.robot.commands.arm.SetArm; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.claw.Claw; +import frc.robot.subsystems.claw.Claw.GamePiece; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.wrist.Wrist; +import java.util.HashMap; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class NoBumpAuto2CubesBalance extends SequentialCommandGroup { + + public final double MAX_V = 3.5; + public final double MAX_A = 3.25; + + public final HashMap eventMap01 = new HashMap<>(); + public final HashMap eventMap02 = new HashMap<>(); + public final HashMap eventMap03 = new HashMap<>(); + public final HashMap eventMap04 = new HashMap<>(); + + private double startTime = 0; + + /** Creates a new NoBumpAuto2Cubes. */ + public NoBumpAuto2CubesBalance( + final Arm arm, final Claw claw, final Drivetrain drive, final Wrist wrist) { + + eventMap01.put("GoHome", new GoHome(arm, wrist)); + + eventMap02.put("GoHome", new GoHome(arm, wrist)); + eventMap02.put( + "CubeScorePrep", + new ParallelCommandGroup( + new SetArm(arm, () -> -49.5, () -> 4, () -> true), + new SetWristPosition(2045 + Constants.INSTALLED_ARM.getWristOffset(), wrist))); + eventMap04.put( + "CubeScorePrep", + new ParallelCommandGroup( + new SetArm(arm, () -> -65.4, () -> 6, () -> true), + new SetWristPosition(2045 + Constants.INSTALLED_ARM.getWristOffset(), wrist))); + eventMap03.put("GoHome", new GoHome(arm, wrist)); + eventMap04.put("GoHome", new GoHome(arm, wrist)); + + addCommands( + new InstantCommand(() -> startTime = Timer.getFPGATimestamp()), + // Step 1: Score preload + new InstantCommand(() -> claw.setCone(), claw), + // new WaitCommand(0.26), + new AutoScoreSequenceNoHomeWait( + arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CONE_TOP_AUTO), + // Step 2: Drive almost to the first game piece + new ParallelCommandGroup( + AutoPathHelper.followPath(drive, "NoBumpAuto01", eventMap01, MAX_V, MAX_A), + new InstantCommand(() -> RobotContainer.setCubePipeline())), + // Step 3: Collect the first game piece + new ParallelRaceGroup( + new DriveToPiece(drive, () -> -2.25, GamePiece.CUBE), + new CollectSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CUBE_FLOOR)), + new WaitCommand(0.06) + .andThen( + () -> { + RobotContainer.setAprilTagPipeline(); + claw.setGamePiece(GamePiece.CUBE); + }), + // Step 4: Drive almost to the scoring location + AutoPathHelper.followPathNoReset(drive, "NoBumpAuto02", eventMap02, MAX_V, MAX_A), + // Step 5: Score the first game piece + new DriveToScore(drive, claw).withTimeout(1.0), + new AutoScoreSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.FRONT_CUBE_TOP_AUTO), + // Step 6: Drive almost to the second game piece + new ParallelCommandGroup( + AutoPathHelper.followPathNoRotationReset( + drive, "NoBumpAuto03", eventMap03, MAX_V, MAX_A), + new InstantCommand(() -> RobotContainer.setConePipeline())), // Shouldnt need + // Step 7: Collect the second game piece + new ParallelRaceGroup( + new DriveToPiece(drive, () -> -1.5, GamePiece.CONE), + new CollectSequenceNoHome( + arm, wrist, claw, () -> Constants.PositionConfigs.BACK_CONE_FLOOR)) + .withTimeout(2.0), + new WaitCommand(0.04), + // Step 8: Drive almost to the scoring location + new ParallelCommandGroup( + new InstantCommand(() -> RobotContainer.setAprilTagPipeline()), // Shouldnt need + new InstantCommand( + () -> { + claw.setGamePiece(GamePiece.CONE); + }), + AutoPathHelper.followPathNoReset( + drive, "NoBumpAuto04Balance", eventMap04, MAX_V, MAX_A)), + // Step 10: Finish + new InstantCommand( + () -> SmartDashboard.putNumber("Auto Time", Timer.getFPGATimestamp() - startTime)), + new InstantCommand(() -> RobotContainer.setAprilTagPipeline())); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmProfile.java b/src/main/java/frc/robot/subsystems/arm/ArmProfile.java index 3032221..374170e 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmProfile.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmProfile.java @@ -4,8 +4,6 @@ package frc.robot.subsystems.arm; -import edu.wpi.first.wpilibj.DriverStation; - /** Add your docs here. */ public class ArmProfile { private int wristOffset; @@ -27,11 +25,11 @@ public int getWristOffset() { public void addMatchOffset(int incrementAmount) { MatchOffset += incrementAmount; - DriverStation.reportWarning("-------- Modified MatchOffset!: " + MatchOffset, false); + // DriverStation.reportWarning("-------- Modified MatchOffset!: " + MatchOffset, false); } public int getMatchOffset() { - DriverStation.reportWarning("- Returned MatchOffset!: " + MatchOffset, false); + // DriverStation.reportWarning("- Returned MatchOffset!: " + MatchOffset, false); return MatchOffset; } } diff --git a/src/main/java/frc/robot/subsystems/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/wrist/Wrist.java index f2e220d..259b798 100644 --- a/src/main/java/frc/robot/subsystems/wrist/Wrist.java +++ b/src/main/java/frc/robot/subsystems/wrist/Wrist.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.wrist; import static frc.robot.Constants.ADVANTAGE_KIT_ENABLED; +import static frc.robot.Constants.TAB_MAIN; import static frc.robot.Constants.TAB_WRIST; import edu.wpi.first.wpilibj.DigitalInput; @@ -16,7 +17,7 @@ public class Wrist extends SubsystemBase { private final DigitalInput wristSwitch = new DigitalInput(0); public static final int ANGLE_STRAIGHT = - 1279 + Constants.INSTALLED_ARM.getWristOffset(); // 2048 total is true straight + 1347 + Constants.INSTALLED_ARM.getWristOffset(); // 2048 total is true straight public static final int ANGLE_FRONT_MAX = 795; // when telescope extended public static final int ANGLE_FRONT_PERPENDICULAR = 447; public static final int ANGLE_BACK_PERPENDICULAR = 2439; @@ -43,12 +44,23 @@ public Wrist(WristIO io) { TAB_WRIST .add("Set Front Half Perpendicular", new SetWristPosition(ANGLE_FRONT_HALF, this)) .withSize(2, 1); + TAB_WRIST.addNumber("Current", this::getCurrent); + TAB_MAIN.addNumber("Wrist Position", this::getPositionWithOffset).withPosition(9, 0); + TAB_WRIST.addNumber("Position Error", this::getError); } public double getCurrent() { return io.getCurrentAmps(); } + public double getError() { + return io.getError(); + } + + public double getPositionWithOffset() { + return getPosition() - Constants.INSTALLED_ARM.getWristOffset(); + } + public void setRotation(boolean inverted) { io.setRotation(inverted); } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java index 7318362..17c1bf4 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIO.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.java @@ -19,6 +19,8 @@ default void updateInputs(WristIOInputs inputs) {} int getPosition(); + double getError(); + /** Return wrist degree */ double getDegrees(); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOTalonSRX.java b/src/main/java/frc/robot/subsystems/wrist/WristIOTalonSRX.java index 9a42a1b..5a9fb19 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOTalonSRX.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOTalonSRX.java @@ -15,12 +15,12 @@ public class WristIOTalonSRX implements WristIO { public WristIOTalonSRX(int wristMotorID) { wristMotor = new WPI_TalonSRX(wristMotorID); TalonSRXConfiguration wristConfig = new TalonSRXConfiguration(); - wristConfig.motionAcceleration = 4000; - wristConfig.motionCruiseVelocity = 6000; + wristConfig.motionAcceleration = 6000; + wristConfig.motionCruiseVelocity = 12000; wristConfig.feedbackNotContinuous = true; - wristConfig.slot0.kP = 8.0; - wristConfig.slot0.kD = 0.0; - wristConfig.slot0.allowableClosedloopError = 0; + wristConfig.slot0.kP = 4.9; + wristConfig.slot0.kD = 1.3; + wristConfig.slot0.allowableClosedloopError = 10; wristConfig.neutralDeadband = 0.001; wristMotor.configFactoryDefault(); wristMotor.configAllSettings(wristConfig); @@ -95,4 +95,9 @@ public void setPercent(final double percent) { public void incrementMidMatchOffset(final int ticks) { midMatchOffsetTicks += ticks; } + + @Override + public double getError() { + return wristMotor.getClosedLoopError(); + } }