From 9aa79ead47c401e261603c118cc8a73faf237edd Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 09:32:51 -0400 Subject: [PATCH 01/16] Add 3 piece with low first piece --- .../deploy/pathplanner/3 Piece W Low.path | 199 +++++++++++ .../com/stuypulse/robot/RobotContainer.java | 1 + .../robot/commands/auton/ThreePieceWLow.java | 321 ++++++++++++++++++ .../robot/constants/ArmTrajectories.java | 4 +- 4 files changed, 523 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/3 Piece W Low.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece W Low.path new file mode 100644 index 00000000..45e1f455 --- /dev/null +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -0,0 +1,199 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.19, + "y": 4.46 + }, + "prevControl": null, + "nextControl": { + "x": 3.5735287054512384, + "y": 4.46 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.187741628152297, + "y": 4.440796639600396 + }, + "prevControl": { + "x": 5.435011671065698, + "y": 4.440796639600396 + }, + "nextControl": { + "x": 6.396944039439681, + "y": 4.440796639600396 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.62, + "y": 4.44 + }, + "prevControl": { + "x": 6.497482928507895, + "y": 4.44 + }, + "nextControl": { + "x": 6.497482928507895, + "y": 4.44 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.26, + "y": 4.6 + }, + "prevControl": { + "x": 3.5298747164225914, + "y": 4.6 + }, + "nextControl": { + "x": 3.5298747164225914, + "y": 4.6 + }, + "holonomicAngle": 6.538707501667168, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.58, + "y": 3.53 + }, + "prevControl": { + "x": 5.084308630155721, + "y": 5.127206493729227 + }, + "nextControl": { + "x": 6.708196571169469, + "y": 3.393102508930692 + }, + "holonomicAngle": -30.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.9, + "y": 3.25 + }, + "prevControl": { + "x": 6.842860523327884, + "y": 3.3115587598696563 + }, + "nextControl": { + "x": 6.842860523327884, + "y": 3.3115587598696563 + }, + "holonomicAngle": -30.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.75, + "y": 4.5 + }, + "prevControl": { + "x": 5.893518753704175, + "y": 5.298494015791451 + }, + "nextControl": { + "x": 5.893518753704175, + "y": 5.298494015791451 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.317578789003732, + "y": 5.355589856478859 + }, + "prevControl": { + "x": 5.392626169867724, + "y": 5.21855983882908 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 95534570..5de4ec6c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -237,6 +237,7 @@ public void configureAutons() { autonChooser.addOption("One Piece Dock", new OnePieceDock()); autonChooser.addOption("One Piece Mobility Dock", new OnePieceMobilityDock()); autonChooser.setDefaultOption("Three Piece", new ThreePiece()); + autonChooser.setDefaultOption("Three Piece W Low", new ThreePieceWLow()); autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java new file mode 100644 index 00000000..757d9789 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -0,0 +1,321 @@ +/************************ 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.auton; + +import com.stuypulse.robot.commands.arm.routines.*; +import com.stuypulse.robot.commands.intake.*; +import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.manager.*; +import com.stuypulse.robot.commands.swerve.*; +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.ArmTrajectories.Ready; +import com.stuypulse.robot.constants.Settings.Arm.Wrist; +import com.stuypulse.robot.subsystems.Manager; +import com.stuypulse.robot.subsystems.Manager.*; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.util.ArmState; +import com.stuypulse.robot.util.ArmTrajectory; +import com.stuypulse.robot.util.DebugSequentialCommandGroup; +import com.stuypulse.robot.util.LEDColor; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; + +public class ThreePieceWLow extends DebugSequentialCommandGroup { + + static class ConeAutonReady extends ArmRoutine { + public ConeAutonReady() { + super(Manager.getInstance()::getReadyTrajectory); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + + return new ArmTrajectory() + .addState(new ArmState(dest.getShoulderState(), src.getWristState()) + .setShoulderTolerance(20).setWristLimp(true).setWristTolerance(360)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()).setWristTolerance(7) + .setShoulderTolerance(15)); + } + } + + static class AutonMidCubeReady extends ArmRoutine { + public AutonMidCubeReady() { + super(() -> Ready.Mid.kAutonCubeBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(7).setShoulderTolerance(20)); + } + } + + static class AutonHighCubeReady extends ArmRoutine { + public AutonHighCubeReady() { + super(() -> Ready.High.kCubeAutonBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(30).setShoulderTolerance(20)); + } + } + + static class ArmIntakeFirst extends ArmRoutine { + public ArmIntakeFirst() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 11); + // 8.37); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + // .addState( + // new ArmState(intermediateShoulderDegrees, wristSafeAngle) + // .setShoulderTolerance(15) + // .setWristTolerance(360)) + + // .addState( + // new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + // .setWristTolerance(360)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + static class ArmIntakeSecond extends ArmRoutine { + public ArmIntakeSecond() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 10); + // 8.37); + double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + .addState( + new ArmState(intermediateShoulderDegrees, wristSafeAngle) + .setShoulderTolerance(15) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + .setWristTolerance(20)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + + private static final double INTAKE_DEACQUIRE_TIME = 0.3; + 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; + private static final double WIGGLE_VEL_AMPLITUDE = 0.6; + + private static final PathConstraints INTAKE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.5, 2); + private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(2.2, 1.2); + + private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.5, 3.5); + private static final PathConstraints THIRD_SCORE_PIECE_CONSTRAINTS = new PathConstraints(3, 2); + + private static final PathConstraints BACK_AWAY_CONSTRAINTS = new PathConstraints(2.5, 2); + + public ThreePieceWLow() { + + var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( + PathPlanner.loadPathGroup("3 Piece W Low", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, THIRD_SCORE_PIECE_CONSTRAINTS, BACK_AWAY_CONSTRAINTS), + "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" + ); + + var arm = Arm.getInstance(); + + // initial setup + addCommands( + new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), + new ManagerSetNodeLevel(NodeLevel.LOW), + new ManagerSetGamePiece(GamePiece.CONE_TIP_IN), + new ManagerSetScoreSide(ScoreSide.BACK) + ); + + // score first piece + addCommands( + new LEDSet(LEDColor.BLUE), + // new ConeAutonReady() + // .withTimeout(1.5) + new IntakeScore(), + new WaitCommand(INTAKE_DEACQUIRE_TIME) + ); + + // intake second piece + addCommands( + + new LEDSet(LEDColor.GREEN), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) + .robotRelative() + .withStop(), + // .until(Intake.getInstance()::hasGamePiece), // interrupting IntakeScore? idk one time the intake just stopped early + + new IntakeScore() + .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) + .andThen(new IntakeStop()) + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) + .andThen(new IntakeAcquire()), + + new ArmIntakeFirst() + .withTolerance(4, 10) + ), + + new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) + .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))), + + // new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) + // .until(Intake.getInstance()::hasGamePiece) + // .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) + // .withTimeout(ACQUIRE_WAIT_TIME), + + arm.runOnce(() -> arm.setWristVoltage(0)) + ); + + // drive to grid and score second piece :: TODO: make custom arm setpoint for this + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new AutonMidCubeReady(), + new WaitCommand(0.5).andThen(new IntakeStop()) + ), + + new ManagerSetGridNode(1), + // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + + // intake third piece + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + + new LEDSet(LEDColor.GREEN), + + new ParallelDeadlineGroup( + new SwerveDriveFollowTrajectory(paths.get("Intake Third Piece")) + .fieldRelative() + .withStop() + .until(Intake.getInstance()::hasGamePiece), + + new WaitCommand(INTAKE_STOP_WAIT_TIME) + .andThen(new IntakeStop()) + .andThen(new WaitCommand(INTAKE_WAIT_TIME)) + .andThen(new IntakeAcquire()), + + new ArmIntakeSecond() + // .setShoulderVelocityTolerance(10) + // .withTolerance(4, 10) + ), + + new SwerveDriveWiggle(WIGGLE_PERIOD, WIGGLE_VEL_AMPLITUDE) + .until(Intake.getInstance()::hasGamePiece) + .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))) + .withTimeout(ACQUIRE_WAIT_TIME), + + arm.runOnce(() -> arm.setWristVoltage(0)) + ); + + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(5); + arm.setShoulderVelocityFeedbackDebounce(0.2); + }) + ); + + // drive to grid and score third piece + addCommands( + new ManagerSetGamePiece(GamePiece.CUBE), + new ManagerSetNodeLevel(NodeLevel.HIGH), + new ManagerSetScoreSide(ScoreSide.BACK), + + new LEDSet(LEDColor.RED), + + new SwerveDriveFollowTrajectory( + paths.get("Score Third Piece")) + .fieldRelative() + .withStop() + .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), + + new ManagerSetGridNode(1), + // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), + new IntakeDeacquire(), + new WaitCommand(INTAKE_DEACQUIRE_TIME), + new IntakeStop() + ); + + // back away + addCommands( + new LEDSet(LEDColor.RAINBOW), + + new SwerveDriveFollowTrajectory( + paths.get("Back Away")) + .fieldRelative() + .withStop() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 051f1664..6d194932 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -77,7 +77,7 @@ public interface Mid { ArmState kAutonCubeBack = new ArmState( new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Shoulder", -165), - new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133)); + new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133 + 10)); ArmState kCubeFront = new ArmState( new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -26), @@ -99,7 +99,7 @@ public interface High { new SmartNumber("Arm Trajectories/Ready High Tip Up Back Wrist", 136)); ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", 3), + new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", -2), new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 50)); ArmState kCubeFront = new ArmState( From 760d05cfcc3b6c57f1cfc0bdfd31a700e6558e3c Mon Sep 17 00:00:00 2001 From: BenG49 Date: Mon, 10 Apr 2023 09:57:02 -0400 Subject: [PATCH 02/16] Remove NYC function, add theoretical y values --- .../com/stuypulse/robot/constants/Field.java | 64 ++++++++++++------- 1 file changed, 42 insertions(+), 22 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index c86e20e0..5e7bbdd5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -84,10 +84,6 @@ 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); @@ -99,6 +95,18 @@ public interface Back { SmartNumber EIGHT = new SmartNumber("Alignment/Y Poses/Red 8", 3.557); SmartNumber NINE = new SmartNumber("Alignment/Y Poses/Red 9", 2.89); + /* + * Theoretical values: + * 295.07in | 7.494778m + * 273.07in | 6.935978m + * 251.07in | 6.377178m + * 230.07in | 5.843778m + * 207.07in | 5.259578m + * 185.07in | 4.700778m + * 163.07in | 4.141978m + * 141.07in | 3.583178m + * 119.07in | 3.024378m + */ Number RED_Y_POSES[] = { Back.ONE, Back.TWO, @@ -111,16 +119,28 @@ public interface Back { Back.NINE }; + /* + * Theoretical values: + * 196in | 4.9784m + * 174in | 4.4196m + * 152in | 3.8608m + * 130in | 3.3020m + * 108in | 2.7432m + * 86in | 2.1844m + * 64in | 1.6256m + * 42in | 1.0668m + * 20in | 0.5080m + */ Number BLUE_Y_POSES[] = { - 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)) + 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) }; } @@ -142,15 +162,15 @@ private static Number backToFront(Number backYPose) { }; Number BLUE_Y_POSES[] = { - 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))) + 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)) }; } } From aa0ca1c9ca88c25249a2486f45bd507c8afafed4 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Mon, 10 Apr 2023 10:02:30 -0400 Subject: [PATCH 03/16] Revert X alignment poses to pre-NYC values --- src/main/java/com/stuypulse/robot/constants/Field.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 5e7bbdd5..b58e13d6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -55,16 +55,16 @@ public static Pose2d getAprilTagFromId(int id) { 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 - 0.075); - SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/High/Cone Tip Out", 1.82 - 0.05); + SmartNumber CUBE_FR`ONT = 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); } 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 - 0.075); - SmartNumber CONE_TIP_OUT = new SmartNumber("Alignment/X Poses/Mid/Cone Tip Out", 2.1433 - 0.1); + 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); } public interface Low { From 18e62bffb162d0e2fea208f8fb88ebcf4941143d Mon Sep 17 00:00:00 2001 From: BenG49 Date: Mon, 10 Apr 2023 10:03:26 -0400 Subject: [PATCH 04/16] Fix typo --- src/main/java/com/stuypulse/robot/constants/Field.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index b58e13d6..3add2da9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -55,7 +55,7 @@ public static Pose2d getAprilTagFromId(int id) { public interface ScoreXPoses { public interface High { SmartNumber CUBE_BACK = new SmartNumber("Alignment/X Poses/High/Cube Back", 1.98); - SmartNumber CUBE_FR`ONT = new SmartNumber("Alignment/X Poses/High/Cube Front", 1.830060); + 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); } From 9bc4e6cf3397ba57bb6b5bb4e75d1958d3aaaed4 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 10:48:38 -0400 Subject: [PATCH 05/16] Three piece W Low changes --- .../robot/commands/auton/ThreePieceWLow.java | 35 ++++++++----------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 757d9789..1822b068 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -23,6 +23,7 @@ import com.stuypulse.robot.util.LEDColor; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -178,18 +179,8 @@ public ThreePieceWLow() { new ManagerSetScoreSide(ScoreSide.BACK) ); - // score first piece + // score first piece + intake second piece addCommands( - new LEDSet(LEDColor.BLUE), - // new ConeAutonReady() - // .withTimeout(1.5) - new IntakeScore(), - new WaitCommand(INTAKE_DEACQUIRE_TIME) - ); - - // intake second piece - addCommands( - new LEDSet(LEDColor.GREEN), new ParallelDeadlineGroup( @@ -201,11 +192,11 @@ public ThreePieceWLow() { new IntakeScore() .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) - .andThen(new IntakeAcquire()), + .andThen(new IntakeAcquire()) - new ArmIntakeFirst() - .withTolerance(4, 10) ), new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) @@ -226,14 +217,16 @@ public ThreePieceWLow() { new LEDSet(LEDColor.RED), - new ParallelDeadlineGroup( - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop(), + new ParallelCommandGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new WaitCommand(0.5).andThen(new IntakeStop()), - new AutonMidCubeReady(), - new WaitCommand(0.5).andThen(new IntakeStop()) + new AutonMidCubeReady() + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) ), new ManagerSetGridNode(1), From e1f0f9f191c8cc47664f52f8352b4e7e82f898c7 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 10:54:37 -0400 Subject: [PATCH 06/16] Increase robot score wrist voltage --- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 181a19ed..19868713 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -384,7 +384,7 @@ public interface Score { // Tip in scoring SmartNumber kForwardSpeed = new SmartNumber("Robot Score/Forward Speed (in per s)", 4); - SmartNumber kWristVoltage = new SmartNumber("Robot Score/Wrist Voltage", 2); + SmartNumber kWristVoltage = new SmartNumber("Robot Score/Wrist Voltage", 3); // Tip in release SmartNumber kBackwardsTipInSpeed = new SmartNumber("Robot Score/Tip In Backwards Speed (in per s)", 16); From 4d344534a4d5ceadaf33c3dd7751c12c5a9b8df9 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 10:59:30 -0400 Subject: [PATCH 07/16] Update pathplannerlib Co-authored-by: anivanchen --- vendordeps/PathplannerLib.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 69fddcd1..8e61586b 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.3", + "version": "2023.4.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" @@ -11,7 +11,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.3" + "version": "2023.4.4" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.3", + "version": "2023.4.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 57c3b12dd4caef4a6cc2ce02dec76b68b699f42c Mon Sep 17 00:00:00 2001 From: Prog694 Date: Mon, 10 Apr 2023 12:08:37 -0400 Subject: [PATCH 08/16] Adjust 3 piece w low path and prep for arm jig --- .../deploy/pathplanner/3 Piece W Low.path | 45 ++++++++++--------- .../robot/commands/auton/ThreePieceWLow.java | 2 +- .../stuypulse/robot/constants/Settings.java | 4 +- 3 files changed, 27 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece W Low.path index 45e1f455..f4920acc 100644 --- a/src/main/deploy/pathplanner/3 Piece W Low.path +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 2.19, - "y": 4.46 + "x": 1.8, + "y": 4.6 }, "prevControl": null, "nextControl": { - "x": 3.5735287054512384, - "y": 4.46 + "x": 3.1835287054512387, + "y": 4.6 }, "holonomicAngle": 0, "isReversal": false, @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 6.187741628152297, - "y": 4.440796639600396 + "y": 4.6 }, "prevControl": { "x": 5.435011671065698, - "y": 4.440796639600396 + "y": 4.6 }, "nextControl": { "x": 6.396944039439681, - "y": 4.440796639600396 + "y": 4.6 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 6.62, - "y": 4.44 + "y": 4.6 }, "prevControl": { "x": 6.497482928507895, - "y": 4.44 + "y": 4.6 }, "nextControl": { "x": 6.497482928507895, - "y": 4.44 + "y": 4.6 }, "holonomicAngle": 0, "isReversal": true, @@ -85,7 +85,7 @@ "x": 3.5298747164225914, "y": 4.6 }, - "holonomicAngle": 6.538707501667168, + "holonomicAngle": 0.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -99,16 +99,16 @@ }, { "anchorPoint": { - "x": 6.58, - "y": 3.53 + "x": 6.614477160578253, + "y": 3.6198762995816596 }, "prevControl": { - "x": 5.084308630155721, - "y": 5.127206493729227 + "x": 5.107146966430685, + "y": 5.035853148629374 }, "nextControl": { - "x": 6.708196571169469, - "y": 3.393102508930692 + "x": 6.751173163320354, + "y": 3.4914649030663525 }, "holonomicAngle": -30.0, "isReversal": false, @@ -153,12 +153,12 @@ "y": 4.5 }, "prevControl": { - "x": 5.893518753704175, - "y": 5.298494015791451 + "x": 5.929327072329358, + "y": 5.28707484765397 }, "nextControl": { - "x": 5.893518753704175, - "y": 5.298494015791451 + "x": 5.929327072329358, + "y": 5.28707484765397 }, "holonomicAngle": 0.0, "isReversal": true, @@ -195,5 +195,8 @@ } } ], + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 1822b068..3018e6d4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -226,7 +226,7 @@ public ThreePieceWLow() { new WaitCommand(0.5).andThen(new IntakeStop()), new AutonMidCubeReady() - .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 1) ), new ManagerSetGridNode(1), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 19868713..b1683f91 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -188,7 +188,7 @@ public interface Shoulder { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.695582 + 180.0/360.0).plus(Rotation2d.fromDegrees(+90)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(AAAA).plus(Rotation2d.fromDegrees()).plus(Rotation2d.fromDegrees(+90)); 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); @@ -237,7 +237,7 @@ public interface Wrist { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(60)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(0)); 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); From 47f89700b2a86192358091cfd24211b4dedd1028 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 12:42:24 -0400 Subject: [PATCH 09/16] Tune wrist and shoulder zero angle --- src/main/java/com/stuypulse/robot/constants/Settings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b1683f91..3dab8403 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -188,7 +188,7 @@ public interface Shoulder { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(AAAA).plus(Rotation2d.fromDegrees()).plus(Rotation2d.fromDegrees(+90)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.025333).plus(Rotation2d.fromDegrees(+90)); 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); @@ -237,7 +237,7 @@ public interface Wrist { MOI, RADIUS); - Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.66055).plus(Rotation2d.fromDegrees(0)); + Rotation2d ZERO_ANGLE = Rotation2d.fromRotations(0.662482).plus(Rotation2d.fromDegrees(180)); 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); From e0cbe159d7af7abed26e95f6e44d8a302164722b Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 14:53:19 -0400 Subject: [PATCH 10/16] Tune stuff --- .../robot/constants/ArmTrajectories.java | 24 +++++++++---------- .../com/stuypulse/robot/constants/Field.java | 18 +++++++------- .../stuypulse/robot/constants/Settings.java | 4 ++-- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java index 6d194932..8893f924 100644 --- a/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java +++ b/src/main/java/com/stuypulse/robot/constants/ArmTrajectories.java @@ -63,12 +63,12 @@ public interface Stow { public interface Ready { public interface Mid { ArmState kConeTipUpBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Shoulder", -161.7), - new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist",133.9)); + new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Shoulder", -172), + new SmartNumber("Arm Trajectories/Ready Mid Tip Up Back Wrist", 136)); ArmState kConeTipInBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -177), - new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -168)); + new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Shoulder", -171), + new SmartNumber("Arm Trajectories/Ready Mid Tip In Back Wrist", -175)); // -4, 0 ArmState kConeTipOutFront = new ArmState( @@ -77,10 +77,10 @@ public interface Mid { ArmState kAutonCubeBack = new ArmState( new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Shoulder", -165), - new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133 + 10)); + new SmartNumber("Arm Trajectories/Ready Auton Mid Cube Back/Wrist", -133)); ArmState kCubeFront = new ArmState( - new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -26), + new SmartNumber("Arm Trajectories/Mid Cube Front/Shoulder", -21), new SmartNumber("Arm Trajectories/Mid Cube Front/Wrist", 49)); ArmState kCubeBack = new ArmState( @@ -90,7 +90,7 @@ public interface Mid { public interface High { ArmState kConeTipInBack = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -193), + new SmartNumber("Arm Trajectories/Ready High Tip In Back Shoulder", -185), new SmartNumber("Arm Trajectories/Ready High Tip In Back Wrist", -180)); // -175, 128 @@ -99,11 +99,11 @@ public interface High { new SmartNumber("Arm Trajectories/Ready High Tip Up Back Wrist", 136)); ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", -2), - new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 50)); + new SmartNumber("Arm Trajectories/Ready High Tip Out Front Shoulder", 3), + new SmartNumber("Arm Trajectories/Ready High Tip Out Front Wrist", 37)); ArmState kCubeFront = new ArmState( - new SmartNumber("Arm Trajectories/High Cube Front/Shoulder", -10), + new SmartNumber("Arm Trajectories/High Cube Front/Shoulder", -5), new SmartNumber("Arm Trajectories/High Cube Front/Wrist", 46)); ArmState kCubeAutonBack = new ArmState( @@ -119,8 +119,8 @@ public interface High { public interface Score { public interface High { ArmState kConeTipOutFront = new ArmState( - new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -11 - 5), - new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 44)); + new SmartNumber("Arm Trajectories/Score High Tip Out Front Shoulder", -5), + new SmartNumber("Arm Trajectories/Score High Tip Out Front Wrist", 37)); } public interface Mid { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 3add2da9..ce06861c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -85,15 +85,15 @@ public static Number[] getYPoseArray(Alliance alliance, ScoreSide side) { } 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); - SmartNumber THREE = new SmartNumber("Alignment/Y Poses/Red 3", 6.3238); - SmartNumber FOUR = new SmartNumber("Alignment/Y Poses/Red 4", 5.822); - SmartNumber FIVE = new SmartNumber("Alignment/Y Poses/Red 5", 5.2947); - SmartNumber SIX = new SmartNumber("Alignment/Y Poses/Red 6", 4.6); - SmartNumber SEVEN = new SmartNumber("Alignment/Y Poses/Red 7", 4.1028); - SmartNumber EIGHT = new SmartNumber("Alignment/Y Poses/Red 8", 3.557); - SmartNumber NINE = new SmartNumber("Alignment/Y Poses/Red 9", 2.89); + SmartNumber ONE = new SmartNumber("Alignment/Y Poses/Red 1", 7.494778); + SmartNumber TWO = new SmartNumber("Alignment/Y Poses/Red 2", 6.935978); + SmartNumber THREE = new SmartNumber("Alignment/Y Poses/Red 3", 6.377178); + SmartNumber FOUR = new SmartNumber("Alignment/Y Poses/Red 4", 5.843778); + SmartNumber FIVE = new SmartNumber("Alignment/Y Poses/Red 5", 5.259578); + SmartNumber SIX = new SmartNumber("Alignment/Y Poses/Red 6", 4.700778); + SmartNumber SEVEN = new SmartNumber("Alignment/Y Poses/Red 7", 4.14197); + SmartNumber EIGHT = new SmartNumber("Alignment/Y Poses/Red 8", 3.583178); + SmartNumber NINE = new SmartNumber("Alignment/Y Poses/Red 9", 3.024378); /* * Theoretical values: diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3dab8403..48b4bff1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -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)", 15.0); + SmartNumber TELEOP_SHOULDER_VELOCITY_FEEDBACK_CUTOFF = new SmartNumber("Arm/Wrist/Teleop Shoulder Velocity Feedback Cutoff (deg per s)", 20.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); @@ -358,7 +358,7 @@ public static Vector2D vpow(Vector2D vec, double power) { public interface Alignment { - SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.3); + SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); SmartNumber ALIGNED_CUBE_THRESHOLD_X = new SmartNumber("Alignment/X Cube Threshold", 0.08); SmartNumber ALIGNED_CUBE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cube Threshold", 0.1); From 5a3f0a7f9c9fd87e4efec364cd9ca26011778e26 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 14:53:29 -0400 Subject: [PATCH 11/16] Tune one piece mobility dock --- .../stuypulse/robot/commands/auton/OnePieceMobilityDock.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java index e234fe5b..8c8fc521 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java @@ -67,7 +67,7 @@ public OnePieceMobilityDock() { .setWristVelocityTolerance(25) .setShoulderVelocityTolerance(45) .withTolerance(7, 9) - .withTimeout(6) + .withTimeout(3) ); addCommands( From 4b96190ec06def3b0a51c92812288e15e8d85057 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 14:53:43 -0400 Subject: [PATCH 12/16] Tune 2 piece wire --- src/main/deploy/pathplanner/2 Piece Wire.path | 40 +++++++++---------- .../robot/commands/auton/TwoPieceWire.java | 7 ++++ 2 files changed, 27 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/pathplanner/2 Piece Wire.path b/src/main/deploy/pathplanner/2 Piece Wire.path index c38f8ffc..eecab3a2 100644 --- a/src/main/deploy/pathplanner/2 Piece Wire.path +++ b/src/main/deploy/pathplanner/2 Piece Wire.path @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 4.025, - "y": 0.7036808379666587 + "y": 0.6 }, "prevControl": { - "x": 3.939417195382434, - "y": 0.7036808379666587 + "x": 3.9394171953824344, + "y": 0.6 }, "nextControl": { "x": 4.091564403591444, - "y": 0.7036808379666587 + "y": 0.6 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 7.5, - "y": 1.4200000000000002 + "y": 0.97 }, "prevControl": { - "x": 5.986422913016767, - "y": 0.7879226014862292 + "x": 6.083140422508604, + "y": 0.6941716374535951 }, "nextControl": { - "x": 7.519005814510022, - "y": 1.4279369236594843 + "x": 7.520216962619652, + "y": 0.9739357546673141 }, "holonomicAngle": 0, "isReversal": false, @@ -75,15 +75,15 @@ { "anchorPoint": { "x": 7.5, - "y": 1.3 + "y": 0.8500000000000001 }, "prevControl": { - "x": 7.524864538684214, - "y": 1.306041827491513 + "x": 7.525347858738666, + "y": 0.8534978594569578 }, "nextControl": { - "x": 6.542296228896253, - "y": 1.0672878211629957 + "x": 6.301852034309053, + "y": 0.6846624369405326 }, "holonomicAngle": 0, "isReversal": false, @@ -124,16 +124,16 @@ }, { "anchorPoint": { - "x": 2.0, - "y": 1.2 + "x": 2.15, + "y": 1.35 }, "prevControl": { - "x": 2.1225494577571427, - "y": 0.6936088327872868 + "x": 3.2377422608937754, + "y": 0.9963188456626031 }, "nextControl": { - "x": 2.1225494577571427, - "y": 0.6936088327872868 + "x": 3.2377422608937754, + "y": 0.9963188456626031 }, "holonomicAngle": 0, "isReversal": true, 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 d9226529..bd2619f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java @@ -100,6 +100,13 @@ public TwoPieceWire() { arm.runOnce(() -> arm.setWristVoltage(0)) ); + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + // drive to grid and score second piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), From bc671ba67aa5edf6c3f3130e48dc6324f41e7b7d Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 15:09:55 -0400 Subject: [PATCH 13/16] Comment out old autos --- src/main/java/com/stuypulse/robot/RobotContainer.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5de4ec6c..df8836c7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -229,16 +229,16 @@ private void configureOperatorBindings() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new MobilityAuton()); - autonChooser.addOption("1.5 Piece Dock", new OnePiecePickupDock()); - autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockWire()); + // autonChooser.addOption("1.5 Piece Dock", new OnePiecePickupDock()); + // autonChooser.addOption("1.5 Piece Dock + Wire", new OnePiecePickupDockWire()); autonChooser.addOption("Two Piece", new TwoPiece()); autonChooser.addOption("Two Piece Wire", new TwoPieceWire()); autonChooser.addOption("Two Piece Dock", new TwoPieceDock()); - autonChooser.addOption("One Piece Dock", new OnePieceDock()); + // autonChooser.addOption("One Piece Dock", new OnePieceDock()); autonChooser.addOption("One Piece Mobility Dock", new OnePieceMobilityDock()); - autonChooser.setDefaultOption("Three Piece", new ThreePiece()); + // autonChooser.addOption("Three Piece", new ThreePiece()); autonChooser.setDefaultOption("Three Piece W Low", new ThreePieceWLow()); - autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); + // autonChooser.addOption("Three Piece Wire", new ThreePieceWire()); SmartDashboard.putData("Autonomous", autonChooser); } From 05d62b21714b14ce719cc049646e46a30ab72c9b Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 15:10:08 -0400 Subject: [PATCH 14/16] BOOM THREE PIECE --- .../deploy/pathplanner/3 Piece W Low.path | 56 +++++++++---------- .../robot/commands/auton/ThreePieceWLow.java | 29 +++++----- 2 files changed, 44 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/3 Piece W Low.path b/src/main/deploy/pathplanner/3 Piece W Low.path index f4920acc..2d09f50b 100644 --- a/src/main/deploy/pathplanner/3 Piece W Low.path +++ b/src/main/deploy/pathplanner/3 Piece W Low.path @@ -25,15 +25,15 @@ { "anchorPoint": { "x": 6.187741628152297, - "y": 4.6 + "y": 4.44 }, "prevControl": { "x": 5.435011671065698, - "y": 4.6 + "y": 4.44 }, "nextControl": { "x": 6.396944039439681, - "y": 4.6 + "y": 4.44 }, "holonomicAngle": 0, "isReversal": false, @@ -50,15 +50,15 @@ { "anchorPoint": { "x": 6.62, - "y": 4.6 + "y": 4.4 }, "prevControl": { - "x": 6.497482928507895, - "y": 4.6 + "x": 6.501547629778108, + "y": 4.431296466187375 }, "nextControl": { - "x": 6.497482928507895, - "y": 4.6 + "x": 6.501547629778108, + "y": 4.431296466187375 }, "holonomicAngle": 0, "isReversal": true, @@ -74,18 +74,18 @@ }, { "anchorPoint": { - "x": 2.26, + "x": 2.1599999999999997, "y": 4.6 }, "prevControl": { - "x": 3.5298747164225914, + "x": 3.4298747164225913, "y": 4.6 }, "nextControl": { - "x": 3.5298747164225914, + "x": 3.4298747164225913, "y": 4.6 }, - "holonomicAngle": 0.0, + "holonomicAngle": 6.54, "isReversal": true, "velOverride": null, "isLocked": false, @@ -99,16 +99,16 @@ }, { "anchorPoint": { - "x": 6.614477160578253, - "y": 3.6198762995816596 + "x": 6.58, + "y": 3.53 }, "prevControl": { - "x": 5.107146966430685, - "y": 5.035853148629374 + "x": 4.903826385233523, + "y": 4.751390831153475 }, "nextControl": { - "x": 6.751173163320354, - "y": 3.4914649030663525 + "x": 6.7315776729384345, + "y": 3.419548684990858 }, "holonomicAngle": -30.0, "isReversal": false, @@ -149,18 +149,18 @@ }, { "anchorPoint": { - "x": 1.75, + "x": 1.7999999999999998, "y": 4.5 }, "prevControl": { - "x": 5.929327072329358, - "y": 5.28707484765397 + "x": 5.794373267068212, + "y": 4.905051768145495 }, "nextControl": { - "x": 5.929327072329358, - "y": 5.28707484765397 + "x": 5.794373267068212, + "y": 4.905051768145495 }, - "holonomicAngle": 0.0, + "holonomicAngle": 10.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -174,12 +174,12 @@ }, { "anchorPoint": { - "x": 6.317578789003732, - "y": 5.355589856478859 + "x": 7.494791213358649, + "y": 5.540372759067196 }, "prevControl": { - "x": 5.392626169867724, - "y": 5.21855983882908 + "x": 6.738011797701916, + "y": 5.2881129538482865 }, "nextControl": null, "holonomicAngle": 0, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 3018e6d4..3e2fd420 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -124,11 +124,14 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { -70.82, 10); // 8.37); - double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double intermediateShoulderDegrees = -60; double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); return new ArmTrajectory() - // .addState(src.getShoulderDegrees(), wristSafeAngle) + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setShoulderTolerance(694) + .setWristTolerance(15)) .addState( new ArmState(intermediateShoulderDegrees, wristSafeAngle) @@ -175,7 +178,7 @@ public ThreePieceWLow() { addCommands( new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), new ManagerSetNodeLevel(NodeLevel.LOW), - new ManagerSetGamePiece(GamePiece.CONE_TIP_IN), + new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); @@ -192,10 +195,10 @@ public ThreePieceWLow() { new IntakeScore() .andThen(new WaitCommand(INTAKE_STOP_WAIT_TIME)) .andThen(new IntakeStop()) - .andThen(new ArmIntakeFirst() - .withTolerance(4, 10)) .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) .andThen(new IntakeAcquire()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) ), @@ -210,6 +213,13 @@ public ThreePieceWLow() { arm.runOnce(() -> arm.setWristVoltage(0)) ); + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) + ); + // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), @@ -226,7 +236,7 @@ public ThreePieceWLow() { new WaitCommand(0.5).andThen(new IntakeStop()), new AutonMidCubeReady() - .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 1) + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) ), new ManagerSetGridNode(1), @@ -236,13 +246,6 @@ public ThreePieceWLow() { new IntakeStop() ); - addCommands( - arm.runOnce(() -> { - arm.setShoulderVelocityFeedbackCutoff(20); - arm.setShoulderVelocityFeedbackDebounce(0.0); - }) - ); - // intake third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), From e3a0924aedce67fd2dc43e3e9de46b7c6254e7b4 Mon Sep 17 00:00:00 2001 From: Vincent Wang Date: Mon, 10 Apr 2023 16:21:34 -0400 Subject: [PATCH 15/16] Three piece wire (sad) --- src/main/deploy/pathplanner/3 Piece Wire.path | 186 ++++++++++----- .../robot/commands/auton/ThreePieceWLow.java | 16 -- .../robot/commands/auton/ThreePieceWire.java | 219 ++++++++++++++---- 3 files changed, 297 insertions(+), 124 deletions(-) diff --git a/src/main/deploy/pathplanner/3 Piece Wire.path b/src/main/deploy/pathplanner/3 Piece Wire.path index 4cc58162..c7948301 100644 --- a/src/main/deploy/pathplanner/3 Piece Wire.path +++ b/src/main/deploy/pathplanner/3 Piece Wire.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.9941842018915368, - "y": 0.5 + "x": 1.8, + "y": 0.74 }, "prevControl": null, "nextControl": { - "x": 2.994184201891537, - "y": 0.5 + "x": 2.8000000000000003, + "y": 0.74 }, "holonomicAngle": 0, "isReversal": false, @@ -24,16 +24,16 @@ }, { "anchorPoint": { - "x": 3.943570307069441, - "y": 0.7036808379666587 + "x": 4.025, + "y": 0.74 }, "prevControl": { - "x": 3.8579875024518744, - "y": 0.7036808379666587 + "x": 3.9394171953824344, + "y": 0.74 }, "nextControl": { - "x": 4.010134710660885, - "y": 0.7036808379666587 + "x": 4.091564403591444, + "y": 0.74 }, "holonomicAngle": 0, "isReversal": false, @@ -49,16 +49,16 @@ }, { "anchorPoint": { - "x": 7.2, - "y": 0.9 + "x": 7.5, + "y": 0.8500000000000001 }, "prevControl": { - "x": 5.60314484579627, - "y": 0.9469663280648167 + "x": 7.525347858738666, + "y": 0.8534978594569578 }, "nextControl": { - "x": 5.60314484579627, - "y": 0.9469663280648167 + "x": 7.525347858738666, + "y": 0.8534978594569578 }, "holonomicAngle": 0, "isReversal": true, @@ -68,22 +68,22 @@ "stopEvent": { "names": [], "executionBehavior": "parallel", - "waitBehavior": "before", + "waitBehavior": "none", "waitTime": 0 } }, { "anchorPoint": { - "x": 3.945273556840507, - "y": 0.9472995877682691 + "x": 4.6000000000000005, + "y": 0.74 }, "prevControl": { - "x": 4.121236461957803, - "y": 0.9472995877682691 + "x": 4.707626879752849, + "y": 0.74 }, "nextControl": { - "x": 3.759125629434196, - "y": 0.9472995877682691 + "x": 3.249347993927497, + "y": 0.7399999999999999 }, "holonomicAngle": 0, "isReversal": false, @@ -99,16 +99,16 @@ }, { "anchorPoint": { - "x": 2.36, - "y": 1.05 + "x": 2.45, + "y": 1.2000000000000002 }, "prevControl": { - "x": 2.9550460021986837, - "y": 0.8334209671805778 + "x": 3.1424514319266095, + "y": 0.8002129793764304 }, "nextControl": { - "x": 2.9550460021986837, - "y": 0.8334209671805778 + "x": 3.1424514319266095, + "y": 0.8002129793764304 }, "holonomicAngle": 0, "isReversal": true, @@ -124,20 +124,45 @@ }, { "anchorPoint": { - "x": 3.9364500016977466, - "y": 0.9280457054566447 + "x": 4.33, + "y": 0.9 }, "prevControl": { - "x": 3.7975022038197768, - "y": 0.9280457054566447 + "x": 3.0442054627112523, + "y": 0.9 }, "nextControl": { - "x": 4.075397799575718, - "y": 0.9280457054566447 + "x": 4.776276695913049, + "y": 0.9 }, "holonomicAngle": 0, "isReversal": false, - "velOverride": 0.75, + "velOverride": 0.6, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.46706608098531, + "y": 0.9436385306337061 + }, + "prevControl": { + "x": 5.99141720254821, + "y": 0.779621676000223 + }, + "nextControl": { + "x": 7.008957514418527, + "y": 1.1304976456106774 + }, + "holonomicAngle": 15.0, + "isReversal": false, + "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { @@ -149,18 +174,18 @@ }, { "anchorPoint": { - "x": 6.982883890789484, - "y": 2.0618508255788215 + "x": 8.27, + "y": 2.2 }, "prevControl": { - "x": 5.70395524896903, - "y": 0.7829221837583678 + "x": 7.8756722931627685, + "y": 1.6405622825231674 }, "nextControl": { - "x": 5.70395524896903, - "y": 0.7829221837583678 + "x": 7.8756722931627685, + "y": 1.6405622825231674 }, - "holonomicAngle": 45.0, + "holonomicAngle": 30.0, "isReversal": true, "velOverride": null, "isLocked": false, @@ -174,20 +199,45 @@ }, { "anchorPoint": { - "x": 3.9254150735135243, - "y": 0.9488987206598537 + "x": 6.46706608098531, + "y": 0.9342955748848557 }, "prevControl": { - "x": 4.676603404598951, - "y": 0.9488987206598537 + "x": 7.205159585144346, + "y": 1.2706419818434043 }, "nextControl": { - "x": 3.2358275315495755, - "y": 0.9488987206598537 + "x": 5.901838611156348, + "y": 0.6767235633172274 }, "holonomicAngle": 0, "isReversal": false, - "velOverride": 0.6, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.73, + "y": 0.85 + }, + "prevControl": { + "x": 4.834901218567718, + "y": 0.8674835364279528 + }, + "nextControl": { + "x": 4.5618267965207275, + "y": 0.8219711327534546 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.5, "isLocked": false, "isStopPoint": false, "stopEvent": { @@ -199,12 +249,37 @@ }, { "anchorPoint": { - "x": 2.04, - "y": 1.05 + "x": 2.65, + "y": 1.2052412916014645 + }, + "prevControl": { + "x": 3.7898406013595243, + "y": 0.8221801058986732 + }, + "nextControl": { + "x": 3.7898406013595243, + "y": 0.8221801058986732 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9631539402938953, + "y": 0.803494194400977 }, "prevControl": { - "x": 2.894697148904932, - "y": 0.8719604723009351 + "x": 3.983786695704312, + "y": 0.7983367444252163 }, "nextControl": null, "holonomicAngle": 0, @@ -220,8 +295,5 @@ } } ], - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java index 3e2fd420..2a0b0e1e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWLow.java @@ -32,22 +32,6 @@ public class ThreePieceWLow extends DebugSequentialCommandGroup { - static class ConeAutonReady extends ArmRoutine { - public ConeAutonReady() { - super(Manager.getInstance()::getReadyTrajectory); - } - - @Override - protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { - - return new ArmTrajectory() - .addState(new ArmState(dest.getShoulderState(), src.getWristState()) - .setShoulderTolerance(20).setWristLimp(true).setWristTolerance(360)) - .addState(new ArmState(dest.getShoulderState(), dest.getWristState()).setWristTolerance(7) - .setShoulderTolerance(15)); - } - } - static class AutonMidCubeReady extends ArmRoutine { public AutonMidCubeReady() { super(() -> Ready.Mid.kAutonCubeBack); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java index 7c15feb3..23689c47 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ThreePieceWire.java @@ -10,100 +10,204 @@ import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.manager.*; import com.stuypulse.robot.commands.swerve.*; +import com.stuypulse.robot.constants.ArmTrajectories; +import com.stuypulse.robot.constants.ArmTrajectories.Ready; +import com.stuypulse.robot.constants.Settings.Arm.Wrist; import com.stuypulse.robot.subsystems.Manager.*; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.util.ArmState; +import com.stuypulse.robot.util.ArmTrajectory; import com.stuypulse.robot.util.DebugSequentialCommandGroup; import com.stuypulse.robot.util.LEDColor; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; public class ThreePieceWire extends DebugSequentialCommandGroup { + static class AutonMidCubeReady extends ArmRoutine { + public AutonMidCubeReady() { + super(() -> Ready.Mid.kAutonCubeBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(7).setShoulderTolerance(20)); + } + } + + static class AutonHighCubeReady extends ArmRoutine { + public AutonHighCubeReady() { + super(() -> Ready.High.kCubeAutonBack); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setWristTolerance(45)) + .addState(new ArmState(dest.getShoulderState(), dest.getWristState()) + .setWristTolerance(30).setShoulderTolerance(20)); + } + } + + static class ArmIntakeFirst extends ArmRoutine { + public ArmIntakeFirst() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 11); + // 8.37); + + return new ArmTrajectory() + // .addState(src.getShoulderDegrees(), wristSafeAngle) + + // .addState( + // new ArmState(intermediateShoulderDegrees, wristSafeAngle) + // .setShoulderTolerance(15) + // .setWristTolerance(360)) + + // .addState( + // new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + // .setWristTolerance(360)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } + + static class ArmIntakeSecond extends ArmRoutine { + public ArmIntakeSecond() { + super(() -> ArmTrajectories.Acquire.kBOOMCubeAuton); + } + + @Override + protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) { + dest = new ArmState( + -70.82, + 10); + // 8.37); + // double intermediateShoulderDegrees = Manager.getInstance().getIntakeIntermediateTrajectory().getShoulderDegrees(); + double intermediateShoulderDegrees = -60; + double wristSafeAngle = Wrist.WRIST_SAFE_ANGLE.get(); + + return new ArmTrajectory() + .addState(new ArmState(src.getShoulderDegrees(), wristSafeAngle) + .setShoulderTolerance(694) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, wristSafeAngle) + .setShoulderTolerance(15) + .setWristTolerance(15)) + + .addState( + new ArmState(intermediateShoulderDegrees, dest.getWristDegrees()) + .setWristTolerance(20)) + + .addState( + new ArmState(dest.getShoulderDegrees(), dest.getWristDegrees()) + .setShoulderTolerance(3) + .setWristTolerance(4)); + } + } private static final double INTAKE_DEACQUIRE_TIME = 0.5; private static final double INTAKE_STOP_WAIT_TIME = 0.5; private static final double INTAKE_WAIT_TIME = 2.0; private static final double ACQUIRE_WAIT_TIME = 0.4; - private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(4, 3.2); - private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(4.8, 4); + private static final PathConstraints INTAKE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.5, 2); + private static final PathConstraints SCORE_SECOND_PIECE_CONSTRAINTS = new PathConstraints(2.2, 1.2); + private static final PathConstraints INTAKE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(4, 3.5); + private static final PathConstraints SCORE_THIRD_PIECE_CONSTRAINTS = new PathConstraints(3, 2); public ThreePieceWire() { var paths = SwerveDriveFollowTrajectory.getSeparatedPaths( - PathPlanner.loadPathGroup("3 Piece Wire", INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS, INTAKE_PIECE_CONSTRAINTS, SCORE_PIECE_CONSTRAINTS), - "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece" + PathPlanner.loadPathGroup("3 Piece Wire", INTAKE_SECOND_PIECE_CONSTRAINTS, SCORE_SECOND_PIECE_CONSTRAINTS, INTAKE_THIRD_PIECE_CONSTRAINTS, SCORE_THIRD_PIECE_CONSTRAINTS), + "Intake Piece", "Score Piece", "Intake Third Piece", "Score Third Piece", "Back Away" ); var arm = Arm.getInstance(); // initial setup addCommands( - new ManagerSetNodeLevel(NodeLevel.MID), + new InstantCommand( () -> Arm.getInstance().setShoulderVelocityFeedbackCutoff(15)), + new ManagerSetNodeLevel(NodeLevel.LOW), new ManagerSetGamePiece(GamePiece.CONE_TIP_UP), new ManagerSetScoreSide(ScoreSide.BACK) ); - // score first piece - addCommands( - new LEDSet(LEDColor.RED), - new ArmReady() - .setWristVelocityTolerance(25) - .setShoulderVelocityTolerance(45) - .withTolerance(7, 9) - .withTimeout(4) - ); - + // score first piece + intake second piece addCommands( - new LEDSet(LEDColor.BLUE), - new IntakeScore(), - new WaitCommand(INTAKE_DEACQUIRE_TIME) - ); - - // intake second piece - addCommands( - new ManagerSetGamePiece(GamePiece.CUBE), - new LEDSet(LEDColor.GREEN), - new ParallelCommandGroup( + new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Piece")) .robotRelative() .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 WaitCommand(INTAKE_WAIT_TIME)) - .andThen(new IntakeAcquire()), + .andThen(new ManagerSetGamePiece(GamePiece.CUBE)) + .andThen(new IntakeAcquire()) + .andThen(new ArmIntakeFirst() + .withTolerance(4, 10)) + ) - new ArmIntake() - .withTolerance(7, 10) - .withTimeout(6.5) - ), + // new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) + // .alongWith(arm.runOnce(() -> arm.setWristVoltage(-3))), - new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) - .alongWith(arm.runOnce(() -> arm.setWristVoltage(-2))), + // arm.runOnce(() -> arm.setWristVoltage(0)) + ); - arm.runOnce(() -> arm.setWristVoltage(0)) + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(20); + arm.setShoulderVelocityFeedbackDebounce(0.0); + }) ); - // drive to grid and score second piece + // drive to grid and score second piece :: TODO: make custom arm setpoint for this addCommands( new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetScoreSide(ScoreSide.BACK), new LEDSet(LEDColor.RED), - new SwerveDriveFollowTrajectory( - paths.get("Score Piece")) - .fieldRelative() - .withStop() - .alongWith(new ArmReady() - .withTolerance(17, 9).alongWith(new WaitCommand(0.1).andThen(new IntakeStop()))), + new ParallelCommandGroup( + new SwerveDriveFollowTrajectory( + paths.get("Score Piece")) + .fieldRelative() + .withStop(), + + new WaitCommand(0.5).andThen(new IntakeStop()), + + new AutonMidCubeReady() + .withTimeout(paths.get("Score Piece").getTotalTimeSeconds() + 0.5) + ), new ManagerSetGridNode(1), // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), @@ -118,19 +222,20 @@ public ThreePieceWire() { new LEDSet(LEDColor.GREEN), - new ParallelCommandGroup( + new ParallelDeadlineGroup( new SwerveDriveFollowTrajectory(paths.get("Intake Third Piece")) .fieldRelative() - .withStop(), + .withStop() + .until(Intake.getInstance()::hasGamePiece), new WaitCommand(INTAKE_STOP_WAIT_TIME) .andThen(new IntakeStop()) .andThen(new WaitCommand(INTAKE_WAIT_TIME)) .andThen(new IntakeAcquire()), - new ArmIntake() - .withTolerance(7, 10) - .withTimeout(6.5) + new ArmIntakeSecond() + // .setShoulderVelocityTolerance(10) + // .withTolerance(4, 10) ), new WaitCommand(ACQUIRE_WAIT_TIME).until(Intake.getInstance()::hasGamePiece) @@ -139,7 +244,14 @@ public ThreePieceWire() { arm.runOnce(() -> arm.setWristVoltage(0)) ); - // drive to grid and score second piece + addCommands( + arm.runOnce(() -> { + arm.setShoulderVelocityFeedbackCutoff(5); + arm.setShoulderVelocityFeedbackDebounce(0.2); + }) + ); + + // drive to grid and score third piece addCommands( new ManagerSetGamePiece(GamePiece.CUBE), new ManagerSetNodeLevel(NodeLevel.HIGH), @@ -151,8 +263,7 @@ public ThreePieceWire() { paths.get("Score Third Piece")) .fieldRelative() .withStop() - .alongWith(new ArmReady() - .withTolerance(17, 9).alongWith(new WaitCommand(0.1).andThen(new IntakeStop()))), + .alongWith(new AutonHighCubeReady().alongWith(new WaitCommand(0.5).andThen(new IntakeStop()))), new ManagerSetGridNode(1), // new SwerveDriveToScorePose().withTimeout(ALIGNMENT_TIME), @@ -161,8 +272,14 @@ public ThreePieceWire() { new IntakeStop() ); + // back away addCommands( - new LEDSet(LEDColor.RAINBOW) + new LEDSet(LEDColor.RAINBOW), + + new SwerveDriveFollowTrajectory( + paths.get("Back Away")) + .fieldRelative() + .withStop() ); } } From 36bd6cfee0046c89efd605d7dba693bb8960a9ce Mon Sep 17 00:00:00 2001 From: vincentw Date: Mon, 10 Apr 2023 16:42:13 -0400 Subject: [PATCH 16/16] Align against grid detection (#183) * Add against grid debouncer in AlignThenScore * Copy derivative into util and add getOutput * Fixes --------- Co-authored-by: BenG49 --- .../robot/commands/RobotAlignThenScore.java | 25 ++++++++++-- .../stuypulse/robot/constants/Settings.java | 3 ++ .../com/stuypulse/robot/util/Derivative.java | 40 +++++++++++++++++++ 3 files changed, 64 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/util/Derivative.java diff --git a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java index f6eeab56..0531cc16 100644 --- a/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java +++ b/src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java @@ -9,7 +9,7 @@ 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.stuylib.streams.filters.IFilter; import com.stuypulse.robot.constants.ArmTrajectories; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.LEDController; @@ -23,6 +23,7 @@ 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.Derivative; import com.stuypulse.robot.util.HolonomicController; import com.stuypulse.robot.util.LEDColor; @@ -48,10 +49,14 @@ public class RobotAlignThenScore extends CommandBase { private final BStream aligned; private boolean movingWhileScoring; // when we have aligned and ready to score and move back + // Against grid debounce + private final Derivative xErrorChange; + private final BStream stoppedByGrid; + // Logging private final FieldObject2d targetPose2d; - public RobotAlignThenScore(){ + public RobotAlignThenScore() { this.swerve = SwerveDrive.getInstance(); this.arm = Arm.getInstance(); this.intake = Intake.getInstance(); @@ -66,18 +71,26 @@ public RobotAlignThenScore(){ aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + xErrorChange = new Derivative(); + stoppedByGrid = BStream.create(this::isAgainstGrid).filtered(new BDebounceRC.Both(Alignment.AGAINST_GRID_DEBOUNCE)); + targetPose2d = Odometry.getInstance().getField().getObject("Target Pose"); + addRequirements(swerve, arm, intake); } + private boolean isAgainstGrid() { + return xErrorChange.getOutput() < Alignment.AGAINST_GRID_VEL_X.get(); + } + private boolean isAligned() { if (Manager.getInstance().getGamePiece().isCone()) { - return controller.isDone( + return stoppedByGrid.get() || controller.isDone( Alignment.ALIGNED_CONE_THRESHOLD_X.get(), Alignment.ALIGNED_CONE_THRESHOLD_Y.get(), Alignment.ALIGNED_CONE_THRESHOLD_ANGLE.get()); } else { - return controller.isDone( + return stoppedByGrid.get() || controller.isDone( Alignment.ALIGNED_CUBE_THRESHOLD_X.get(), Alignment.ALIGNED_CUBE_THRESHOLD_Y.get(), Alignment.ALIGNED_CUBE_THRESHOLD_ANGLE.get()); @@ -90,6 +103,8 @@ public void initialize() { intake.enableBreak(); Odometry.USE_VISION_ANGLE.set(true); + xErrorChange.reset(); + LEDController.getInstance().setColor(LEDColor.BLUE, 694000000); } @@ -99,6 +114,8 @@ public void execute() { Pose2d targetPose = Manager.getInstance().getScorePose(); targetPose2d.setPose(targetPose); + xErrorChange.get(targetPose.getX() - currentPose.getX()); + controller.update(targetPose, currentPose); if (aligned.get() || movingWhileScoring) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 48b4bff1..22deba4b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -368,6 +368,9 @@ public interface Alignment { SmartNumber ALIGNED_CONE_THRESHOLD_Y = new SmartNumber("Alignment/Y Cone Threshold", 0.05); SmartNumber ALIGNED_CONE_THRESHOLD_ANGLE = new SmartNumber("Alignment/Angle Cone Threshold", 1); + SmartNumber AGAINST_GRID_VEL_X = new SmartNumber("Alignment/Against Grid X Velocity (m per s)", 0.02); + SmartNumber AGAINST_GRID_DEBOUNCE = new SmartNumber("Alignment/Against Grid Debounce", 0.3); + public interface Translation { SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2); SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); diff --git a/src/main/java/com/stuypulse/robot/util/Derivative.java b/src/main/java/com/stuypulse/robot/util/Derivative.java new file mode 100644 index 00000000..31ba4f7b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/Derivative.java @@ -0,0 +1,40 @@ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved. */ +/* This work is licensed under the terms of the MIT license */ +/* found in the root directory of this project. */ + +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.streams.filters.IFilter; +import com.stuypulse.stuylib.util.StopWatch; + +/** + * This class takes an IStream and gives you the derivative with respect to time. + * + * @author Sam (sam.belliveau@gmail.com) + */ +public class Derivative implements IFilter { + + private final StopWatch mTimer; + private double mLastValue; + private double mOutput; + + public Derivative() { + mTimer = new StopWatch(); + mLastValue = 0.0; + } + + public double get(double next) { + double diff = next - mLastValue; + mLastValue = next; + mOutput = diff / mTimer.reset(); + return mOutput; + } + + public double getOutput() { + return mOutput; + } + + public void reset() { + mOutput = Double.POSITIVE_INFINITY; + } +}