From 93e3ca7a3baaf48d82947f322b5d8d7a2763afb1 Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Sun, 5 Nov 2023 07:43:11 -0800 Subject: [PATCH] Beach Blitz Day 1 Co-Authored-By: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> --- .../pathplanner/b_hp_1Cone+1.5Cube.path | 225 ++++++++++++++++++ .../b_mid_1Cone+0.5Cube+Climb.path | 12 +- .../pathplanner/r_hp_1Cone+1.5Cube.path | 225 ++++++++++++++++++ .../r_mid_1Cone+0.5Cube+Climb.path | 12 +- src/main/java/frc/team3128/Constants.java | 8 +- .../java/frc/team3128/PositionConstants.java | 4 +- .../frc/team3128/autonomous/AutoPrograms.java | 5 +- .../frc/team3128/autonomous/Trajectories.java | 6 +- .../frc/team3128/commands/CmdManager.java | 6 + .../frc/team3128/subsystems/Manipulator.java | 2 +- .../java/frc/team3128/subsystems/Vision.java | 18 +- 11 files changed, 490 insertions(+), 33 deletions(-) create mode 100644 src/main/deploy/pathplanner/b_hp_1Cone+1.5Cube.path create mode 100644 src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path diff --git a/src/main/deploy/pathplanner/b_hp_1Cone+1.5Cube.path b/src/main/deploy/pathplanner/b_hp_1Cone+1.5Cube.path new file mode 100644 index 0000000..5f1ecbe --- /dev/null +++ b/src/main/deploy/pathplanner/b_hp_1Cone+1.5Cube.path @@ -0,0 +1,225 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 4.85 + }, + "prevControl": null, + "nextControl": { + "x": 2.614287557554049, + "y": 4.797360022722226 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.71, + "y": 4.75 + }, + "prevControl": { + "x": 3.0869254445746694, + "y": 4.75 + }, + "nextControl": { + "x": 4.101634755137839, + "y": 4.75 + }, + "holonomicAngle": 0.01, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.69, + "y": 4.65 + }, + "prevControl": { + "x": 5.738594656932928, + "y": 4.745130586685033 + }, + "nextControl": { + "x": 5.738594656932928, + "y": 4.745130586685033 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.71, + "y": 4.75 + }, + "prevControl": { + "x": 4.2410339780017114, + "y": 4.75 + }, + "nextControl": { + "x": 3.1182674278572016, + "y": 4.75 + }, + "holonomicAngle": -0.01, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.83, + "y": 4.35 + }, + "prevControl": { + "x": 2.4682733218996464, + "y": 4.786707687546719 + }, + "nextControl": { + "x": 2.4682733218996464, + "y": 4.786707687546719 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.71, + "y": 4.75 + }, + "prevControl": { + "x": 3.4126780929453417, + "y": 4.75 + }, + "nextControl": { + "x": 4.010819932178748, + "y": 4.75 + }, + "holonomicAngle": 0.01, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.73, + "y": 3.55 + }, + "prevControl": { + "x": 5.6263049420299485, + "y": 5.316281310801438 + }, + "nextControl": { + "x": 5.6263049420299485, + "y": 5.316281310801438 + }, + "holonomicAngle": -59.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.71, + "y": 4.75 + }, + "prevControl": { + "x": 4.236034616958284, + "y": 4.830024869548394 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 2.3, + "isReversed": null, + "markers": [ + { + "position": 0.7636363636363053, + "names": [ + "PickupCube" + ] + }, + { + "position": 2.5090909090909514, + "names": [ + "Neutral" + ] + }, + { + "position": 5.141818181818192, + "names": [ + "PickupCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path index 96bb318..1cd2fce 100644 --- a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path @@ -76,16 +76,16 @@ }, { "anchorPoint": { - "x": 6.707654896212571, - "y": 1.5 + "x": 6.75, + "y": 2.0 }, "prevControl": { - "x": 6.036038306451613, - "y": 1.7836771159653428 + "x": 6.078383410239042, + "y": 2.283677115965343 }, "nextControl": { - "x": 6.036038306451613, - "y": 1.7836771159653428 + "x": 6.078383410239042, + "y": 2.283677115965343 }, "holonomicAngle": 0, "isReversal": true, diff --git a/src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path b/src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path new file mode 100644 index 0000000..67be288 --- /dev/null +++ b/src/main/deploy/pathplanner/r_hp_1Cone+1.5Cube.path @@ -0,0 +1,225 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 14.69, + "y": 4.85 + }, + "prevControl": null, + "nextControl": { + "x": 13.92571244244595, + "y": 4.797360022722226 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.83, + "y": 4.75 + }, + "prevControl": { + "x": 13.453074555425331, + "y": 4.75 + }, + "nextControl": { + "x": 12.438365244862162, + "y": 4.75 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.85, + "y": 4.65 + }, + "prevControl": { + "x": 10.801405343067072, + "y": 4.745130586685033 + }, + "nextControl": { + "x": 10.801405343067072, + "y": 4.745130586685033 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.83, + "y": 4.75 + }, + "prevControl": { + "x": 12.29896602199829, + "y": 4.75 + }, + "nextControl": { + "x": 13.4217325721428, + "y": 4.75 + }, + "holonomicAngle": 180.01, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 14.71, + "y": 4.35 + }, + "prevControl": { + "x": 14.071726678100354, + "y": 4.786707687546719 + }, + "nextControl": { + "x": 14.071726678100354, + "y": 4.786707687546719 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.83, + "y": 4.75 + }, + "prevControl": { + "x": 13.127321907054657, + "y": 4.75 + }, + "nextControl": { + "x": 12.529180067821251, + "y": 4.75 + }, + "holonomicAngle": 179.99, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.81, + "y": 3.55 + }, + "prevControl": { + "x": 10.913695057970052, + "y": 5.316281310801438 + }, + "nextControl": { + "x": 10.913695057970052, + "y": 5.316281310801438 + }, + "holonomicAngle": -121.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 12.83, + "y": 4.75 + }, + "prevControl": { + "x": 12.303965383041717, + "y": 4.830024869548394 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.5, + "maxAcceleration": 2.3, + "isReversed": null, + "markers": [ + { + "position": 0.7636363636363053, + "names": [ + "PickupCube" + ] + }, + { + "position": 2.5090909090909514, + "names": [ + "Neutral" + ] + }, + { + "position": 5.141818181818192, + "names": [ + "PickupCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path index 178ec17..e08c386 100644 --- a/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/r_mid_1Cone+0.5Cube+Climb.path @@ -76,16 +76,16 @@ }, { "anchorPoint": { - "x": 9.81, - "y": 1.5 + "x": 9.75, + "y": 2.0 }, "prevControl": { - "x": 10.481616589760959, - "y": 1.7836771159653426 + "x": 10.421616589760959, + "y": 2.283677115965343 }, "nextControl": { - "x": 10.481616589760959, - "y": 1.7836771159653426 + "x": 10.421616589760959, + "y": 2.283677115965343 }, "holonomicAngle": 180, "isReversal": true, diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 0774c4d..0871387 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -73,7 +73,7 @@ public static class AutoConstants { public static final double ANGLE_THRESHOLD = 8; //7, 9 public static final double VELOCITY_THRESHOLD = 4; //6, 3 public static final double RAMP_THRESHOLD = 9; //8, 10 - public static final double DRIVE_SPEED = Units.inchesToMeters(20); //30, 40 + public static final double DRIVE_SPEED = Units.inchesToMeters(30); //30, 40 } @@ -356,7 +356,7 @@ public static class WristConstants { public static final int MIN_ANGLE = -90; public static final int MAX_ANGLE = 90; - public static final double WRIST_TOLERANCE = 0.5; + public static final double WRIST_TOLERANCE = 1; public static final int WRIST_ID = 21; @@ -381,9 +381,9 @@ public static class ElevatorConstants { public static final double kI = 0; public static final double kD = 0; - public static final double kS = 0.2375; //0.975; //1.05; + public static final double kS = 0.975;//0.2375; //0.975; //1.05; public static final double kV = 0; - public static final double kG = 0.4125;//0.975; + public static final double kG = 0.975;//0.4125;//0.975; public static final double MIN_DIST = 2; //Ask Charlie public static final double MAX_DIST = 55; //Ask Charlie diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 570c757..c50cb36 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -13,8 +13,8 @@ public static enum Position { CHUTE_CONE(10, 55, true), CHUTE_CUBE(10, 45, false), - GROUND_CONE(10.5, -18, true), - GROUND_CUBE(2.5, 0, false), + GROUND_CONE(11.5, -18, true), + GROUND_CUBE(1, 5, false), NEUTRAL(3, 80, false); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 6c563ab..5eccb9c 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -30,7 +30,7 @@ private void initAutoSelector() { //Mid "mid_1Cone+Climb","mid_1Cone+0.5Cube+Climb", "mid_1Cone+1Cube+Climb", //Hp - "hp_1Cone+1Cube", + "hp_1Cone+1Cube", "hp_1Cone+1.5Cube", "scuffedClimb" }; @@ -43,9 +43,10 @@ public Command getAutonomousCommand() { if (selectedAutoName == null) { autoCommand = score(Position.HIGH_CONE, true); + // autoCommand = none(); } - else if (selectedAutoName == "scuffedClimb") { + else if (selectedAutoName.equals("scuffedClimb")) { autoCommand = sequence( score(Position.HIGH_CONE, true), new CmdAutoBalance(false) diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index af9751b..abbdfa7 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -49,7 +49,7 @@ public static void initTrajectories() { //Mid "b_mid_1Cone+Climb","b_mid_1Cone+0.5Cube+Climb", "b_mid_1Cone+1Cube+Climb", //Hp - "b_hp_1Cone+1Cube", + "b_hp_1Cone+1Cube", "b_hp_1Cone+1.5Cube", //Red Autos //Cable @@ -57,7 +57,7 @@ public static void initTrajectories() { //Mid "r_mid_1Cone+Climb","r_mid_1Cone+0.5Cube+Climb","r_mid_1Cone+1Cube+Climb", //Hp - "r_hp_1Cone+1Cube" + "r_hp_1Cone+1Cube", "r_hp_1Cone+1.5Cube" }; CommandEventMap.put("ScoreConeHigh", sequence(score(Position.HIGH_CONE, true))); @@ -109,7 +109,7 @@ public static CommandBase resetAuto() { runOnce(()-> Leds.getInstance().defaultColor = Colors.AUTO), resetLeds(), resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), - runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), + runOnce(()-> Manipulator.getInstance().set(-0.5), Manipulator.getInstance()), runOnce(()-> Manipulator.getInstance().isCone = true), resetAll(), retract(Position.NEUTRAL) diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 2ca9da1..011608b 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import static edu.wpi.first.wpilibj2.command.Commands.*; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -81,6 +82,11 @@ public static CommandBase pickup(Position position) { public static CommandBase extend(Position position) { return sequence ( moveElevator(position), + runOnce(()-> { + if (DriverStation.isAutonomous()) { + manipulator.stopRoller(); + } + }), moveWrist(position) ); } diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index b2ef096..dccbd2b 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -28,7 +28,7 @@ public Manipulator(){ } private void configMotor(){ m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); - m_roller.setInverted(true); + m_roller.setInverted(false); m_roller.setNeutralMode(NeutralMode.Brake); } diff --git a/src/main/java/frc/team3128/subsystems/Vision.java b/src/main/java/frc/team3128/subsystems/Vision.java index d3b157f..90c34b0 100644 --- a/src/main/java/frc/team3128/subsystems/Vision.java +++ b/src/main/java/frc/team3128/subsystems/Vision.java @@ -35,7 +35,7 @@ public Vision() { cameras = new HashMap(); // cameras.put(FRONT_LEFT.hostname, new NAR_Camera(FRONT_LEFT)); // cameras.put(FRONT_RIGHT.hostname, new NAR_Camera(FRONT_RIGHT)); - cameras.put(BACK_RIGHT.hostname, new NAR_Camera(BACK_RIGHT)); + // cameras.put(BACK_RIGHT.hostname, new NAR_Camera(BACK_RIGHT)); cameras.put(BACK_LEFT.hostname, new NAR_Camera(BACK_LEFT)); } @@ -124,8 +124,8 @@ public void periodic(){ public void initShuffleboard() { NAR_Camera cam = cameras.get(BACK_LEFT.hostname); - NAR_Camera cam2 = cameras.get(BACK_RIGHT.hostname); - NAR_Shuffleboard.addData("VisionComp", "HasTarget", ()->cam.hasValidTarget() || cam2.hasValidTarget(), 0, 0); + // NAR_Camera cam2 = cameras.get(BACK_RIGHT.hostname); + // NAR_Shuffleboard.addData("VisionComp", "HasTarget", ()->cam.hasValidTarget() || cam2.hasValidTarget(), 0, 0); NAR_Shuffleboard.addData("Vision","HasTarget", ()->cam.hasValidTarget(), 0, 0); NAR_Shuffleboard.addData("Vision","Distance",()->cam.getDistance(),1,0); @@ -136,12 +136,12 @@ public void initShuffleboard() { NAR_Shuffleboard.addData("Test", "Test", ()->SELECTED_GRID,0,0); NAR_Shuffleboard.addData("Test", "TESTING", ()->cam.getTest().toString(),0,1,3,1); - NAR_Shuffleboard.addData("Vision2","HasTarget", ()->cam2.hasValidTarget(), 0, 0); - NAR_Shuffleboard.addData("Vision2","Distance",()->cam2.getDistance(),1,0); - NAR_Shuffleboard.addData("Vision2","RawTarget",()->cam2.getTarget().toString(),0,1,4,1); - NAR_Shuffleboard.addData("Vision2", "Processed Target",()->cam2.getProcessedTarget().toString(),0,2,4,1); - NAR_Shuffleboard.addData("Vision2","EstimatedPose", ()-> cam2.getPos().toString(),0,3,4,1); - NAR_Shuffleboard.addData("Test", "TESTING", ()->cam2.getTest().toString(),0,2,3,1); + // NAR_Shuffleboard.addData("Vision2","HasTarget", ()->cam2.hasValidTarget(), 0, 0); + // NAR_Shuffleboard.addData("Vision2","Distance",()->cam2.getDistance(),1,0); + // NAR_Shuffleboard.addData("Vision2","RawTarget",()->cam2.getTarget().toString(),0,1,4,1); + // NAR_Shuffleboard.addData("Vision2", "Processed Target",()->cam2.getProcessedTarget().toString(),0,2,4,1); + // NAR_Shuffleboard.addData("Vision2","EstimatedPose", ()-> cam2.getPos().toString(),0,3,4,1); + // NAR_Shuffleboard.addData("Test", "TESTING", ()->cam2.getTest().toString(),0,2,3,1); } public void logCameraAll() {