diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path index 4bbb930..e922079 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path @@ -151,16 +151,16 @@ }, { "anchorPoint": { - "x": 1.834230493113764, + "x": 1.82, "y": 1.1 }, "prevControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 + "x": 3.314237054793555, + "y": 0.798088337496119 }, "nextControl": { - "x": 3.328467547907319, - "y": 0.7980883374961188 + "x": 3.314237054793555, + "y": 0.798088337496119 }, "holonomicAngle": 180.0, "isReversal": true, diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path index bc5cfc4..15e341a 100644 --- a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path @@ -152,15 +152,15 @@ { "anchorPoint": { "x": 1.834230493113764, - "y": 1.1 + "y": 1.5 }, "prevControl": { "x": 3.328467547907319, - "y": 0.7980883374961188 + "y": 1.1980883374961189 }, "nextControl": { "x": 3.328467547907319, - "y": 0.7980883374961188 + "y": 1.1980883374961189 }, "holonomicAngle": 180.0, "isReversal": true, @@ -178,11 +178,11 @@ }, { "anchorPoint": { - "x": 7.080923415319053, + "x": 6.0, "y": 0.9977988827505433 }, "prevControl": { - "x": 3.665490370911298, + "x": 2.5845669555922446, "y": 0.32913967911269143 }, "nextControl": null, @@ -204,7 +204,7 @@ "isReversed": null, "markers": [ { - "position": 2.015234375, + "position": 1.4254545454545942, "names": [ "PickupCube" ] diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path new file mode 100644 index 0000000..bd17d85 --- /dev/null +++ b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path @@ -0,0 +1,225 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.85, + "y": 0.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.614284574055259, + "y": 0.5526832772415535 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreConeHigh" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7075088130204783, + "y": 0.6 + }, + "prevControl": { + "x": 3.0844342575951473, + "y": 0.6 + }, + "nextControl": { + "x": 4.099143568158317, + "y": 0.6 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.694433640603393, + "y": 0.7 + }, + "prevControl": { + "x": 5.743020330548507, + "y": 0.6049491252537939 + }, + "nextControl": { + "x": 5.743020330548507, + "y": 0.6049491252537939 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.6960733550526776, + "y": 0.6 + }, + "prevControl": { + "x": 4.227107333054388, + "y": 0.6 + }, + "nextControl": { + "x": 3.104340782909878, + "y": 0.6 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.834230493113764, + "y": 1.1 + }, + "prevControl": { + "x": 2.472479352948291, + "y": 0.6632565618911479 + }, + "nextControl": { + "x": 2.472479352948291, + "y": 0.6632565618911479 + }, + "holonomicAngle": 180.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "ScoreCubeHigh" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.741815186911401, + "y": 0.7089983937457166 + }, + "prevControl": { + "x": 3.4444932798567436, + "y": 0.7089983937457166 + }, + "nextControl": { + "x": 4.04263511909015, + "y": 0.7089983937457166 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.726469715422112, + "y": 1.9897696856769331 + }, + "prevControl": { + "x": 5.6227746574520605, + "y": 0.2234883748754959 + }, + "nextControl": { + "x": 5.6227746574520605, + "y": 0.2234883748754959 + }, + "holonomicAngle": 59.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.741815186911401, + "y": 0.6289501880002328 + }, + "prevControl": { + "x": 4.222104421384305, + "y": 0.6746920198547948 + }, + "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": 1.1272727272727252, + "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 fecb67b..0ab2eb9 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 @@ -77,15 +77,15 @@ { "anchorPoint": { "x": 6.707654896212571, - "y": 1.9 + "y": 1.5 }, "prevControl": { "x": 6.036038306451613, - "y": 2.1836771159653425 + "y": 1.7836771159653428 }, "nextControl": { "x": 6.036038306451613, - "y": 2.1836771159653425 + "y": 1.7836771159653428 }, "holonomicAngle": 0, "isReversal": true, diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index d1cc0e6..ec87b43 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(40); //30, 40 + public static final double DRIVE_SPEED = Units.inchesToMeters(20); //30, 40 } @@ -213,6 +213,12 @@ public static class VisionConstants { new Transform2d( new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)), Rotation2d.fromDegrees(180))); + + public static final Camera BACK_LEFT = new Camera("BACK_LEFT", true, 0, 0, 0, + new Transform2d( + new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(18.89)), + Rotation2d.fromDegrees(180))); + public static final double POSE_THRESH = 100; public static final Matrix SVR_STATE_STD = VecBuilder.fill(0.1,0.1,Units.degreesToRadians(3)); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index a945c18..c8ef359 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -26,7 +26,7 @@ private void initAutoSelector() { final String[] autoStrings = new String[] { //Blue Autos //Cable - "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1.5Cube+Climb", + "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1Cube_V2", "cable_1Cone+1.5Cube+Climb", //Mid "mid_1Cone+Climb","mid_1Cone+0.5Cube+Climb", "mid_1Cone+1Cube+Climb", //Hp diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index 81dbaec..c2591d8 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -45,9 +45,9 @@ public static void initTrajectories() { final String[] trajectoryNames = { //Blue Autos //Cable - "b_cable_1Cone+1Cube","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb", + "b_cable_1Cone+1Cube", "b_cable_1Cone+1Cube_V2","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb", //Mid - "b_mid_1Cone+Climb","b_mid_1Cone+1Cube+Climb", + "b_mid_1Cone+Climb","b_mid_1Cone+0.5Cube+Climb", "b_mid_1Cone+1Cube+Climb", //Hp "b_hp_1Cone+1Cube","b_cable_1Cone+2Cube", @@ -55,12 +55,12 @@ public static void initTrajectories() { //Cable "r_cable_1Cone+1Cube","r_cable_1Cone+2Cube", //Mid - "r_mid_1Cone+Climb","r_mid_1Cone+1Cube+Climb", + "r_mid_1Cone+Climb","r_mid_1Cone+0.5Cube+Climb","r_mid_1Cone+1Cube+Climb", //Hp "r_hp_1Cone+1Cube","r_cable_1Cone+2Cube", }; - CommandEventMap.put("ScoreConeHigh", score(Position.HIGH_CONE, true)); + CommandEventMap.put("ScoreConeHigh", sequence(score(Position.HIGH_CONE, true))); CommandEventMap.put("ScoreCubeHigh", score(Position.HIGH_CUBE, true)); @@ -68,7 +68,7 @@ public static void initTrajectories() { CommandEventMap.put("PickupCube", pickup(Position.GROUND_CUBE, true)); - CommandEventMap.put("Neutral", retract(Position.NEUTRAL)); + CommandEventMap.put("Neutral", sequence(retract(Position.NEUTRAL))); CommandEventMap.put("Balance", new ScheduleCommand(new CmdAutoBalance(true))); @@ -110,6 +110,7 @@ public static CommandBase resetAuto() { resetLeds(), resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180), runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()), + runOnce(()-> Manipulator.getInstance().isCone = true), resetAll(), retract(Position.NEUTRAL) ); diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java index db4f2a9..4a69090 100644 --- a/src/main/java/frc/team3128/commands/CmdAutoBalance.java +++ b/src/main/java/frc/team3128/commands/CmdAutoBalance.java @@ -49,12 +49,12 @@ public void execute() { // final double angleVelocity = (advAngle - prevAngle) / 0.02; final double angleVelocity = getAngleVelocity(advAngle); - if (advAngle > RAMP_THRESHOLD) onRamp = true; + if (Math.abs(advAngle) > RAMP_THRESHOLD) onRamp = true; if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) { swerve.xlock(); if (direction == 1) { - Manipulator.getInstance().set(-1); + // Manipulator.getInstance().set(-1); } return; } diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 411b5a9..d083a39 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -62,7 +62,7 @@ public void execute() { rotation = -zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; - if (Math.abs(rotation) > maxAngularVelocity * swerve.throttle / 2.0) { + if (Math.abs(rotation) > maxAngularVelocity * swerve.throttle / 4.0) { enabled = false; } if (enabled) { diff --git a/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java b/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java index 5312416..cf5c615 100644 --- a/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java +++ b/src/main/java/frc/team3128/common/hardware/camera/NAR_Camera.java @@ -464,7 +464,7 @@ private Pose2d getPos(PhotonTrackedTarget target) { private Pose2d getPosApril(PhotonTrackedTarget tag) { if(!hasValidTarget() || !AprilTags.containsKey(targetId(tag))) return new Pose2d(); Transform2d transform = getProcessedTarget(tag); - if (!AprilTags.containsKey(targetId(tag)) || transform.getX() > 5 || Math.abs(transform.getRotation().getDegrees()) < 150) return new Pose2d(); + if (!AprilTags.containsKey(targetId(tag)) || transform.getX() > 4 || Math.abs(transform.getRotation().getDegrees()) < 150) return new Pose2d(); Pose2d target = AprilTags.get(targetId()); if (target == null) return new Pose2d(); Translation2d coord = target.getTranslation().plus(transform.getTranslation().rotateBy(target.getRotation())); diff --git a/src/main/java/frc/team3128/subsystems/Vision.java b/src/main/java/frc/team3128/subsystems/Vision.java index 3186788..d3b157f 100644 --- a/src/main/java/frc/team3128/subsystems/Vision.java +++ b/src/main/java/frc/team3128/subsystems/Vision.java @@ -33,9 +33,10 @@ public Vision() { NAR_Camera.setAprilTags(APRIL_TAG_POS); NAR_Camera.multipleTargets = false; 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(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_LEFT.hostname, new NAR_Camera(BACK_LEFT)); } public Pose2d targetPos(String name, Pose2d robotPos) { @@ -122,8 +123,8 @@ public void periodic(){ } public void initShuffleboard() { - NAR_Camera cam = cameras.get(FRONT_LEFT.hostname); - NAR_Camera cam2 = cameras.get(FRONT_RIGHT.hostname); + 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_Shuffleboard.addData("Vision","HasTarget", ()->cam.hasValidTarget(), 0, 0);