diff --git a/src/main/deploy/pathplanner/2 Piece + Dock.path b/src/main/deploy/pathplanner/2 Piece + Dock.path index 159bdb71..db9edeab 100644 --- a/src/main/deploy/pathplanner/2 Piece + Dock.path +++ b/src/main/deploy/pathplanner/2 Piece + Dock.path @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 2.18, - "y": 4.06 + "y": 4.26 }, "prevControl": { "x": 2.1355417497355624, - "y": 4.0781910527110625 + "y": 4.278191052711063 }, "nextControl": { "x": 2.990760937761192, - "y": 3.7282596623312907 + "y": 3.928259662331291 }, "holonomicAngle": 0, "isReversal": false, @@ -74,11 +74,11 @@ }, { "anchorPoint": { - "x": 3.6, + "x": 3.9000000000000004, "y": 2.9668705600756398 }, "prevControl": { - "x": 1.9600286499779123, + "x": 2.2600286499779125, "y": 2.866211202507875 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/2 Piece Wire.path b/src/main/deploy/pathplanner/2 Piece Wire.path index 21f5f02d..c38f8ffc 100644 --- a/src/main/deploy/pathplanner/2 Piece Wire.path +++ b/src/main/deploy/pathplanner/2 Piece Wire.path @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 7.5, - "y": 0.8240000000000001 + "y": 1.4200000000000002 }, "prevControl": { - "x": 5.811700742388826, - "y": 0.756067081855683 + "x": 5.986422913016767, + "y": 0.7879226014862292 }, "nextControl": { - "x": 7.520579845087765, - "y": 0.824828081233506 + "x": 7.519005814510022, + "y": 1.4279369236594843 }, "holonomicAngle": 0, "isReversal": false, @@ -75,15 +75,15 @@ { "anchorPoint": { "x": 7.5, - "y": 1.004 + "y": 1.3 }, "prevControl": { "x": 7.524864538684214, - "y": 1.010041827491513 + "y": 1.306041827491513 }, "nextControl": { "x": 6.542296228896253, - "y": 0.7712878211629955 + "y": 1.0672878211629957 }, "holonomicAngle": 0, "isReversal": false, diff --git a/src/main/deploy/pathplanner/3 Piece.path b/src/main/deploy/pathplanner/3 Piece.path index 9e485104..9c94864f 100644 --- a/src/main/deploy/pathplanner/3 Piece.path +++ b/src/main/deploy/pathplanner/3 Piece.path @@ -153,12 +153,12 @@ "y": 4.5 }, "prevControl": { - "x": 4.838784774793751, - "y": 4.858894672949488 + "x": 6.043518753704174, + "y": 5.298494015791451 }, "nextControl": { - "x": 4.838784774793751, - "y": 4.858894672949488 + "x": 6.043518753704174, + "y": 5.298494015791451 }, "holonomicAngle": 10.0, "isReversal": true, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 96374637..95534570 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,4 +1,4 @@ -/************************ PROJECT JIM *************************/ + /************************ PROJECT JIM *************************/ /* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ /* This work is licensed under the terms of the MIT license. */ /**************************************************************/ @@ -126,14 +126,16 @@ private void configureDriverBindings() { driver.getBottomButton() .whileTrue(new RobotScore()); driver.getLeftBumper() - .whileTrue(new RobotRelease()); + .whileTrue(new RobotRelease()) + .onFalse(new WaitCommand(0.5).andThen(new IntakeStop())); driver.getRightTriggerButton() - .whileTrue(new RobotRelease()); + .whileTrue(new RobotRelease()) + .onFalse(new WaitCommand(0.5).andThen(new IntakeStop())); driver.getTopButton() .onTrue(new ManagerValidateState()) .onTrue(new ManagerChooseScoreNode()) - .whileTrue(new RobotAlignThenScore()); + .whileTrue(new RobotAlignThenScoreCubes()); // swerve driver.getLeftButton().whileTrue(new SwerveDriveAlignThenBalance()); diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java index 0549ebd7..f6eeab56 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.constants.ArmTrajectories; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.LEDController; import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Alignment.Rotation; import com.stuypulse.robot.constants.Settings.Alignment.Translation; @@ -23,6 +24,7 @@ import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -87,6 +89,8 @@ public void initialize() { movingWhileScoring = false; intake.enableBreak(); Odometry.USE_VISION_ANGLE.set(true); + + LEDController.getInstance().setColor(LEDColor.BLUE, 694000000); } @Override @@ -146,6 +150,8 @@ public void end(boolean interupted) { swerve.stop(); intake.stop(); targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); + + LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0); } } diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java new file mode 100644 index 00000000..68f53635 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java @@ -0,0 +1,140 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands; + +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.LEDController; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.subsystems.Manager.GamePiece; +import com.stuypulse.robot.subsystems.Manager.NodeLevel; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.*; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RobotAlignThenScoreCubes extends CommandBase { + + // Subsystems + private final SwerveDrive swerve; + private final Arm arm; + private final Intake intake; + + private final Manager manager; + + // Holonomic control + private final HolonomicController controller; + private final BStream aligned; + private boolean movingWhileScoring; // when we have aligned and ready to score and move back + + // Logging + private final FieldObject2d targetPose2d; + + public RobotAlignThenScoreCubes(){ + this.swerve = SwerveDrive.getInstance(); + this.arm = Arm.getInstance(); + this.intake = Intake.getInstance(); + manager = Manager.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); + + SmartDashboard.putData("Alignment/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + + targetPose2d = Odometry.getInstance().getField().getObject("Target Pose"); + addRequirements(swerve, arm, intake); + } + + private boolean isAligned() { + if (Manager.getInstance().getGamePiece().isCone()) { + return controller.isDone( + Alignment.ALIGNED_CONE_THRESHOLD_X.get(), + Alignment.ALIGNED_CONE_THRESHOLD_Y.get(), + Alignment.ALIGNED_CONE_THRESHOLD_ANGLE.get()); + } else { + return controller.isDone( + Alignment.ALIGNED_CUBE_THRESHOLD_X.get(), + Alignment.ALIGNED_CUBE_THRESHOLD_Y.get(), + Alignment.ALIGNED_CUBE_THRESHOLD_ANGLE.get()); + } + } + + @Override + public void initialize() { + movingWhileScoring = false; + intake.enableBreak(); + Odometry.USE_VISION_ANGLE.set(true); + + LEDController.getInstance().setColor(LEDColor.BLUE, 694000000); + } + + @Override + public void execute() { + Pose2d currentPose = Odometry.getInstance().getPose(); + Pose2d targetPose = Manager.getInstance().getScorePose(); + targetPose2d.setPose(targetPose); + + controller.update(targetPose, currentPose); + + if (aligned.get() || movingWhileScoring) { + // simply outtake when low + if (manager.getNodeLevel() == NodeLevel.LOW) { + intake.deacquire(); + } + + + // or do scoring motion based on the game piece + else { + + // only score for cubes + if (manager.getGamePiece().isCube()) { + intake.deacquire(); + } + } + + } else { + swerve.setChassisSpeeds(controller.getOutput()); + } + } + + @Override + public boolean isFinished(){ + return false; + } + + public void end(boolean interupted) { + intake.enableBreak(); + Odometry.USE_VISION_ANGLE.set(false); + swerve.stop(); + intake.stop(); + targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); + + LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/RobotRelease.java b/src/main/java/com/stuypulse/robot/commands/RobotRelease.java index 40b84a95..9e785f4d 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotRelease.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotRelease.java @@ -66,7 +66,7 @@ public boolean isFinished() { @Override public void end(boolean i) { - intake.stop(); + // intake.stop(); intake.enableBreak(); swerve.stop(); } diff --git a/src/main/java/com/stuypulse/robot/commands/RobotScore.java b/src/main/java/com/stuypulse/robot/commands/RobotScore.java index 976da275..b560913e 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotScore.java @@ -31,7 +31,7 @@ public RobotScore() { arm = Arm.getInstance(); intake = Intake.getInstance(); manager = Manager.getInstance(); - + addRequirements(swerve, arm, intake); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java index a444112e..cc2804dd 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePiece.java @@ -56,7 +56,7 @@ public AutonReady() { } }); } - + @Override protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); @@ -65,7 +65,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) .setWristTolerance(45)) .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) - .setWristTolerance(30).setShoulderTolerance(20)); + .setWristTolerance(7).setShoulderTolerance(25)); } } static class ArmIntakeFirst extends ArmRoutine { @@ -134,7 +134,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final double INTAKE_DEACQUIRE_TIME = 0.3; - private static final double INTAKE_STOP_WAIT_TIME = 0.5; + private static final double INTAKE_STOP_WAIT_TIME = 1; private static final double INTAKE_WAIT_TIME = 1.0; private static final double ACQUIRE_WAIT_TIME = 0.02; private static final double WIGGLE_PERIOD = 0.6; @@ -174,12 +174,11 @@ public ThreePiece() { addCommands( new LEDSet(LEDColor.BLUE), new IntakeScore(), - new WaitCommand(INTAKE_DEACQUIRE_TIME) + new WaitCommand(0.8) ); // intake second piece addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), new LEDSet(LEDColor.GREEN), @@ -189,8 +188,10 @@ public ThreePiece() { .withStop(), // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early - new WaitCommand(INTAKE_STOP_WAIT_TIME) + new IntakeScore() + .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) .andThen(new IntakeAcquire()), new ArmIntakeFirst() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java index 8d885ee3..92e29cdb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceDock.java @@ -89,7 +89,7 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { private static final double INTAKE_DEACQUIRE_TIME = 0.2; private static final double CUBE_DEACQUIRE_TIME = 0.2; - private static final double INTAKE_STOP_WAIT_TIME = 0.1; + private static final double INTAKE_STOP_WAIT_TIME = 1; private static final double INTAKE_WAIT_TIME = 0.2; private static final double ACQUIRE_WAIT_TIME = 0.1; private static final double ENGAGE_TIME = 10.0; @@ -139,7 +139,6 @@ public TwoPieceDock() { // intake second piece addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), new LEDSet(LEDColor.GREEN), @@ -150,6 +149,7 @@ public TwoPieceDock() { new WaitCommand(INTAKE_STOP_WAIT_TIME) .andThen(new IntakeStop()) .andThen(new WaitCommand(INTAKE_WAIT_TIME)) + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) .andThen(new IntakeAcquire()), new ArmIntakeBOOM() @@ -177,12 +177,16 @@ public TwoPieceDock() { new LEDSet(LEDColor.RED), + arm.runOnce(() -> arm.setShoulderVelocityFeedbackCutoff(10.0)), + new ParallelCommandGroup( new SwerveDriveFollowTrajectory( paths.get("Score Piece")) .fieldRelative().withStop(), - new WaitCommand(0.2).andThen(new AutonReady()), + new WaitCommand(0.2) + .andThen(new AutonReady() + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5)), new SequentialCommandGroup( new WaitCommand(0.4), @@ -212,7 +216,7 @@ public TwoPieceDock() { new LEDSet(LEDColor.GREEN), new SwerveDriveBalanceBlay() - .withMaxSpeed(0.6) + .withMaxSpeed(0.75) .withTimeout(ENGAGE_TIME) .alongWith(new FastStow().withTolerance(15, 10)), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java index 6afc0a37..d9226529 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java @@ -26,7 +26,7 @@ public class TwoPieceWire extends DebugSequentialCommandGroup { private static final double INTAKE_DEACQUIRE_TIME = 0.5; - private static final double INTAKE_STOP_WAIT_TIME = 0.5; + private static final double INTAKE_STOP_WAIT_TIME = 1; private static final double INTAKE_ACQUIRE_TIME = 0.5; private static final double INTAKE_WAIT_TIME = 2.0; private static final double ACQUIRE_WAIT_TIME = 0.4; @@ -77,8 +77,6 @@ public TwoPieceWire() { // intake second piece addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), new ParallelDeadlineGroup( @@ -88,6 +86,7 @@ public TwoPieceWire() { new WaitCommand(INTAKE_STOP_WAIT_TIME) .andThen(new IntakeStop()) .andThen(new WaitCommand(INTAKE_WAIT_TIME)) + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) .andThen(new IntakeAcquire()), new ArmIntakeBOOM() diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 8a22b893..051f1664 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -22,7 +22,7 @@ public interface Acquire { new SmartNumber("Arm Trajectories/Acquire Cube Wrist", 12)); ArmState kHPCone = new ArmState( - new SmartNumber("Arm Trajectories/Acquire HP Cone Shoulder", -4), + new SmartNumber("Arm Trajectories/Acquire HP Cone Shoulder", 0), new SmartNumber("Arm Trajectories/Acquire HP Cone Wrist", 0)); ArmState kIntermediate = new ArmState( @@ -67,7 +67,7 @@ public interface Mid { new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist",133.9)); ArmState kConeTipInBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -175), + new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -177), new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -168)); // -4, 0 @@ -83,15 +83,15 @@ public interface Mid { new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -26), new SmartNumber("Arm Trajectories/Mid Cube Front/Wrist", 49)); - ArmState kCubeBack = new ArmState( + ArmState kCubeBack = new ArmState( new SmartNumber("Arm Trajectories/Mid Cube Back/Shoulder", -175), new SmartNumber("Arm Trajectories/Mid Cube Back/Wrist", -62)); } public interface High { ArmState kConeTipInBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -190), - new SmartNumber("Arm Trajectories/Ready High Tip In Back Wrist", -173)); + new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -193), + new SmartNumber("Arm Trajectories/Ready High Tip In Back Wrist", -180)); // -175, 128 ArmState kConeTipUpBack = new ArmState( @@ -119,13 +119,13 @@ public interface High { public interface Score { public interface High { ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -11), + new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -11 - 5), new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 44)); } public interface Mid { ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Shoulder", -28), + new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Shoulder", -28 - 5), new SmartNumber("Arm Trajectories/Score Mid Tip Out Front Wrist", 44)); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index a50ed358..c86e20e0 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -56,15 +56,15 @@ public interface ScoreXPoses { public interface High { SmartNumber CUBE_BACK = new SmartNumber("Alignment/X Poses/High/Cube Back", 1.98); SmartNumber CUBE_FRONT = new SmartNumber("Alignment/X Poses/High/Cube Front", 1.830060); - SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/High/Cone Tip In", 1.894); - SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/High/Cone Tip Out", 1.82); + SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/High/Cone Tip In", 1.894 - 0.075); + SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/High/Cone Tip Out", 1.82 - 0.05); } public interface Mid { SmartNumber CUBE_BACK = new SmartNumber("Alignment/X Poses/Mid/Cube Back", 1.868); SmartNumber CUBE_FRONT = new SmartNumber("Alignment/X Poses/Mid/Cube Front", 2.083577); - SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/Mid/Cone Tip In", 2.275); - SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/Mid/Cone Tip Out", 2.1433); + SmartNumber CONE_TIP_IN = new SmartNumber("Alignment/X Poses/Mid/Cone Tip In", 2.275 - 0.075); + SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/Mid/Cone Tip Out", 2.1433 - 0.1); } public interface Low { @@ -84,6 +84,10 @@ public static Number[] getYPoseArray(Alliance alliance, ScoreSide side) { return alliance == Alliance.Red ? Back.RED_Y_POSES : Back.BLUE_Y_POSES; } + public static Number redToBlueNYC(Number yPose) { + return IStream.create(() -> yPose.doubleValue() - Units.inchesToMeters(3.0)).number(); + } + public interface Back { SmartNumber ONE = new SmartNumber("Alignment/Y Poses/Red 1", 7.4376); SmartNumber TWO = new SmartNumber("Alignment/Y Poses/Red 2", 6.905); @@ -108,15 +112,15 @@ public interface Back { }; Number BLUE_Y_POSES[] = { - AllianceUtil.getMirroredYPose(Back.NINE), - AllianceUtil.getMirroredYPose(Back.EIGHT), - AllianceUtil.getMirroredYPose(Back.SEVEN), - AllianceUtil.getMirroredYPose(Back.SIX), - AllianceUtil.getMirroredYPose(Back.FIVE), - AllianceUtil.getMirroredYPose(Back.FOUR), - AllianceUtil.getMirroredYPose(Back.THREE), - AllianceUtil.getMirroredYPose(Back.TWO), - AllianceUtil.getMirroredYPose(Back.ONE) + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.NINE)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.EIGHT)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.SEVEN)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.SIX)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.FIVE)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.FOUR)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.THREE)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.TWO)), + redToBlueNYC(AllianceUtil.getMirroredYPose(Back.ONE)) }; } @@ -138,15 +142,15 @@ private static Number backToFront(Number backYPose) { }; Number BLUE_Y_POSES[] = { - AllianceUtil.getMirroredYPose(backToFront(Back.NINE)), - AllianceUtil.getMirroredYPose(backToFront(Back.EIGHT)), - AllianceUtil.getMirroredYPose(backToFront(Back.SEVEN)), - AllianceUtil.getMirroredYPose(backToFront(Back.SIX)), - AllianceUtil.getMirroredYPose(backToFront(Back.FIVE)), - AllianceUtil.getMirroredYPose(backToFront(Back.FOUR)), - AllianceUtil.getMirroredYPose(backToFront(Back.THREE)), - AllianceUtil.getMirroredYPose(backToFront(Back.TWO)), - AllianceUtil.getMirroredYPose(backToFront(Back.ONE)) + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.NINE))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.EIGHT))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.SEVEN))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.SIX))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.FIVE))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.FOUR))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.THREE))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.TWO))), + redToBlueNYC(AllianceUtil.getMirroredYPose(backToFront(Back.ONE))) }; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index dc0a34e3..181a19ed 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -110,13 +110,13 @@ public interface Turn { } public interface Drive { - double kP = 0.50121; + double kP = 0.8; double kI = 0.0; double kD = 0.0; - double kS = 0.17475; - double kV = 2.5728; - double kA = 0.33987; + double kS = 0.22304; + double kV = 2.4899; + double kA = 0.41763; } public interface FrontRight { @@ -135,7 +135,7 @@ public interface FrontLeft { public interface BackLeft { String ID = "Back Left"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(118.741436) // recalibrated 3/24 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(125.371964) // recalibrated 4/6 .plus(Rotation2d.fromDegrees(180)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5); } @@ -170,7 +170,7 @@ public interface Turn { public interface Arm { public interface Shoulder { - SmartNumber MAX_SHOULDER_ANGLE = new SmartNumber("Arm/Shoulder/Max Angle (deg)", 10.0); + SmartNumber MAX_SHOULDER_ANGLE = new SmartNumber("Arm/Shoulder/Max Angle (deg)", 15.0); SmartNumber OVER_BUMPER_ANGLE = new SmartNumber("Arm/Shoulder/Over Bumper Angle (deg)", 25.0); int MOTORS = 2; @@ -190,8 +190,8 @@ public interface Shoulder { Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.695582 + 180.0/360.0).plus(Rotation2d.fromDegrees(+90)); - SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Shoulder/Teleop Max Velocity (deg)", 270); - SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Shoulder/Teleop Max Acceleration (deg)", 360); + SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Shoulder/Teleop Max Velocity (deg)", 315); + SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Shoulder/Teleop Max Acceleration (deg)", 420); SmartNumber AUTON_MAX_VELOCITY = new SmartNumber("Arm/Shoulder/Auton Max Velocity (deg)", 360); SmartNumber AUTON_MAX_ACCELERATION = new SmartNumber("Arm/Shoulder/Auton Max Acceleration (deg)", 480); @@ -237,7 +237,7 @@ public interface Wrist { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(180)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(60)); SmartNumber TELEOP_MAX_VELOCITY = new SmartNumber("Arm/Wrist/Teleop Max Velocity (deg)", 480.0); SmartNumber TELEOP_MAX_ACCELERATION = new SmartNumber("Arm/Wrist/Teleop Max Acceleration (deg)", 480.0); @@ -245,7 +245,7 @@ public interface Wrist { SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Auton Shoulder Velocity Feedback Cutoff (deg per s)", 5.0); SmartNumber AUTON_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Auton Feedback Enabled Debounce", 0.15); - SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 10.0); + SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 15.0); SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_DEBOUNCE = new SmartNumber("Arm/Wrist/Teleop Feedback Enabled Debounce", 0.0); SmartNumber WRIST_SAFE_ANGLE = new SmartNumber("Arm/Wrist/Safe Angle (deg)", 80); @@ -323,7 +323,7 @@ public interface Operator { public interface Driver { public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.08); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.10); SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.125); SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); diff --git a/src/main/java/com/stuypulse/robot/subsystems/Manager.java b/src/main/java/com/stuypulse/robot/subsystems/Manager.java index 5c2b7f75..910909b7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Manager.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Manager.java @@ -200,7 +200,7 @@ public int getNearestScoreIndex() { double gridDistance = getSelectedScoreX().doubleValue(); Number[] positions = Field.ScoreYPoses.getYPoseArray(RobotContainer.getCachedAlliance(), scoreSide); - int nearest = 0; + int nearest = getPossibleScoringIndices()[0]; double nearestDistance = robot.getDistance(new Translation2d(gridDistance, positions[nearest].doubleValue()));