diff --git a/simgui-ds.json b/simgui-ds.json index 23ded49..3fef02d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -93,5 +93,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/simgui.json b/simgui.json index 8214de6..4ff00d6 100644 --- a/simgui.json +++ b/simgui.json @@ -1,15 +1,57 @@ { + "HALProvider": { + "Other Devices": { + "CANCoder (v6)[9]": { + "header": { + "open": true + } + }, + "Talon FX (v6)[18]": { + "header": { + "open": true + } + }, + "Talon FX (v6)[27]/Fwd Limit": { + "header": { + "open": true + } + }, + "Talon FX (v6)[27]/Rev Limit": { + "header": { + "open": true + } + }, + "Talon FX (v6)[28]": { + "header": { + "open": true + } + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/SmartDashboard/Auton Chooser": "String Chooser", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/PIDController[3]": "PIDController" }, "windows": { "/FMSInfo": { "window": { "visible": true } + }, + "/SmartDashboard/PIDController[3]": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "TEAM 1072": { + "open": true } } }, @@ -48,5 +90,31 @@ "open": true }, "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.05999999865889549, + 0.05999999865889549, + 0.05999999865889549, + 0.9399999976158142 + ], + "height": 338, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/TEAM 1072/Shooter/Shooter Index Occupied" + } + ] + } + ] + } } } diff --git a/src/main/deploy/pathplanner/paths/FourNotePath.path b/src/main/deploy/pathplanner/paths/FourNotePath.path index e084a2a..124da77 100644 --- a/src/main/deploy/pathplanner/paths/FourNotePath.path +++ b/src/main/deploy/pathplanner/paths/FourNotePath.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 2.398033988749899, - "y": 5.413895591680073 + "x": 1.7164528753779988, + "y": 4.869096333680088 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.565241563695676, - "y": 4.153622036288668 + "x": 2.573413830101682, + "y": 4.4600922416554205 }, "prevControl": { - "x": 2.5089519071853434, - "y": 4.132111461906811 + "x": 2.603543675107459, + "y": 4.407905819281239 }, "nextControl": { - "x": 3.0471837503016173, - "y": 4.33779180912854 + "x": 2.315447370819469, + "y": 4.906903255780861 }, "isLocked": false, "linkedName": null @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 1.79, - "y": 5.430145569998894 + "x": 1.7150425771027837, + "y": 4.810667177680737 }, "prevControl": { - "x": 1.7970951627420186, - "y": 6.020001367599977 + "x": 1.7221377398448023, + "y": 5.40052297528182 }, "nextControl": null, "isLocked": false, @@ -127,7 +127,7 @@ "rotationTargets": [ { "waypointRelativePos": 1, - "rotationDegrees": 180.0, + "rotationDegrees": 133.12669653785858, "rotateFast": false }, { @@ -145,11 +145,6 @@ "rotationDegrees": -143.15527430224782, "rotateFast": false }, - { - "waypointRelativePos": 0, - "rotationDegrees": 180.0, - "rotateFast": false - }, { "waypointRelativePos": 3, "rotationDegrees": 180.0, @@ -177,7 +172,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -1.473689633364434, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..1316498 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.5482137470971375, + "y": 6.513777708545001 + }, + "prevControl": null, + "nextControl": { + "x": 6.548213747097137, + "y": 6.013777708545001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612227993615214, + "y": 6.028951994659942 + }, + "prevControl": { + "x": 4.7296587217802415, + "y": 6.138704806241458 + }, + "nextControl": { + "x": 2.204786894958409, + "y": 5.89071468489254 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.798129930709267, + "y": 1.7608511984137063 + }, + "prevControl": { + "x": 3.548129930709267, + "y": 3.2608511984137065 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -2.2217127139514274, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OneNotePath.path b/src/main/deploy/pathplanner/paths/OneNotePath.path new file mode 100644 index 0000000..a6881f9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OneNotePath.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8984446913140571, + "y": 6.446683545798883 + }, + "prevControl": null, + "nextControl": { + "x": 1.8984446913140567, + "y": 6.446683545798883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2252231911871232, + "y": 6.750016462995888 + }, + "prevControl": { + "x": 0.9931733443713804, + "y": 6.462274652944366 + }, + "nextControl": { + "x": 1.8529756423768975, + "y": 7.528429502471214 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.613150954696971, + "y": 7.37191005246208 + }, + "prevControl": { + "x": 3.870591444886594, + "y": 7.223398150500005 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": -119.7448812969422, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -108.97040780848653, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ThreeNotePath.path b/src/main/deploy/pathplanner/paths/ThreeNotePath.path index 7fedba8..c608828 100644 --- a/src/main/deploy/pathplanner/paths/ThreeNotePath.path +++ b/src/main/deploy/pathplanner/paths/ThreeNotePath.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.51, - "y": 1.3588927224150746 + "x": 1.570379985362581, + "y": 1.3536087807644175 }, "prevControl": null, "nextControl": { - "x": 6.9530533212918275, - "y": 0.7031704202297295 + "x": 7.013433306654408, + "y": 0.6978864785790724 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9930962948430917, - "y": 3.4618237659134143 + "x": 1.9793840773921174, + "y": 3.47653478225106 }, "prevControl": { - "x": 3.518485190594432, - "y": 2.7165654778797874 + "x": 1.8625257653836784, + "y": 3.233079965566812 }, "nextControl": { - "x": 2.7147240100272763, - "y": 3.856691746747238 + "x": 2.188448208451587, + "y": 3.9120850552916213 }, "isLocked": false, "linkedName": null @@ -33,15 +33,15 @@ { "anchor": { "x": 5.182268647489855, - "y": 1.8719319735981343 + "y": 1.52 }, "prevControl": { "x": 3.989225197806464, - "y": 2.4868913206357983 + "y": 2.134959347037664 }, "nextControl": { "x": 5.582490574055011, - "y": 1.6656358703874938 + "y": 1.3137038967893595 }, "isLocked": false, "linkedName": null @@ -65,31 +65,31 @@ { "anchor": { "x": 5.182268647489855, - "y": 1.8719319735981343 + "y": 1.5191580561097076 }, "prevControl": { "x": 6.271447434262405, - "y": 1.628337605806952 + "y": 1.2755636883185253 }, "nextControl": { "x": 4.093089860717305, - "y": 2.1155263413893164 + "y": 1.7627524239008894 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9930962948430917, - "y": 3.4618237659134143 + "x": 1.9793840773921174, + "y": 3.47653478225106 }, "prevControl": { - "x": 3.3796657049805376, - "y": 3.1725788667136037 + "x": 2.135195160070036, + "y": 3.0772688828888937 }, "nextControl": { - "x": 2.6065268847056466, - "y": 3.751068665113225 + "x": 1.8038647561047902, + "y": 3.926303043049836 }, "isLocked": false, "linkedName": null @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 2.9930962948430917, - "y": 3.4618237659134143 + "x": 1.9793840773921174, + "y": 3.47653478225106 }, "prevControl": { - "x": 3.2565818269753746, - "y": 3.757857413165056 + "x": 3.5959240601755234, + "y": 3.233079965566812 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index c17b3f2..d1f2d0f 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -3,23 +3,21 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.RobotMap.Pivot.Goal; import frc.robot.commands.CommandGroups; import frc.robot.commands.drivetrain.AlignToStage; //import frc.robot.commands.CommandGroups; // import frc.robot.commands.drivetrain.AlignToStage; -import frc.robot.commands.elevator.ElevatorManual; -import frc.robot.commands.elevator.ZeroElevator; -import frc.robot.commands.indexer.IndexToShooter; -import frc.robot.commands.intake.IntakeNote; -import frc.robot.commands.intake.OuttakeNote; -import frc.robot.commands.intake.ZeroIntake; +// import frc.robot.commands.elevator.ElevatorManual; +import frc.robot.commands.elevator.MoveToPosition; +import frc.robot.commands.intake.OuttakeStuckNote; import frc.robot.commands.pivot.PivotToAngle; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; -import frc.robot.commands.shooter.ShootNote; +import frc.robot.commands.shooter.ShootSpeakerNote; import frc.robot.subsystems.Pivot; import frc.robot.subsystems.swerve.Drivetrain; import frc.robot.util.Flip; @@ -49,32 +47,30 @@ public XboxGamepad getOperator() { } private void initBindings() { - // driver.getLeftBumper().onTrue(CommandGroups.FULL_SHOOT_AMP); - driver.getRightBumper().onTrue(CommandGroups.FULL_SHOOT_SPEAKER); - driver.getButtonB().whileTrue(new ZeroPivot()); - driver.getButtonA().whileTrue(new PivotToAngle(RobotMap.Pivot.Goal.AMP)); - // driver.getButtonA().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(1))); - driver.getButtonA().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); - // driver.getButtonA().onTrue(new AlignToStage("left")); - - driver.getButtonSelect().onTrue(new InstantCommand(() -> { - Drivetrain.getInstance().setYaw(180); - })); + driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); + driver.getLeftBumper().onTrue(CommandGroups.getFullIntakeCommand()); + driver.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); + driver.getUpDPadButton().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); + driver.getButtonA().onTrue(CommandGroups.getFullRetractClimb()); + driver.getButtonY().onTrue(CommandGroups.getFullClimb()); + driver.getButtonB().onTrue(CommandGroups.getFullShootAmp()); + + operator.getButtonY().onTrue(CommandGroups.getFullClimb()); + operator.getButtonA().whileTrue(new MoveToPosition(0)); + operator.getLeftBumper().onTrue(CommandGroups.getFullZeroCommand()); + driver.getButtonX().onTrue(CommandGroups.getFullShootNoAlign()); + + driver.getRightDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); + driver.getLeftDPadButton().onTrue(new PivotToAngle(Goal.AMP)); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); })); - driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(new Translation2d(Units.inchesToMeters(14), Units.inchesToMeters(121.25)), new Rotation2d(Math.toRadians(180)))))); - - driver.getUpDPadButton().onTrue(CommandGroups.FULL_SHOOT_NO_ALIGN); - - operator.getDownDPadButton().onTrue(CommandGroups.FULL_ZERO); - operator.getRightBumper().onTrue(CommandGroups.FULL_INTAKE); - operator.getLeftBumper().whileTrue(new OuttakeNote()); - - operator.getButtonY().whileTrue(new ElevatorManual()); - operator.getButtonX().whileTrue(new ZeroElevator()); + //TESTING + // driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); + // driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); + // driver.getButtonA().onTrue(new AlignToStage("left")); // operator.getUpDPadButton().onTrue(CommandGroups.PRE_ALIGN_CLIMB); // operator.getDownDPadButton().onTrue(CommandGroups.POST_ALIGN_CLIMB); // operator.getRightDPadButton().onTrue(CommandGroups.FULL_SHOOT_TRAP); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6730455..c7409cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -23,7 +24,10 @@ // import frc.robot.commands.CommandGroups; //import frc.robot.commands.CommandGroups; import frc.robot.commands.drivetrain.SwerveManual; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; // import frc.robot.commands.shooter.ShooterManual; // import frc.robot.subsystems.Intake; // import frc.robot.subsystems.Shooter; @@ -58,7 +62,7 @@ public void robotInit() { // DriverStation.startDataLog(DataLogManager.getLog()); SmartDashboard.putData(RobotMap.Field.FIELD); - Limelight.setCameraPose(RobotMap.Camera.FORWARD, RobotMap.Camera.UP, RobotMap.Camera.PITCH); + // Limelight.setCameraPose(RobotMap.Camera.FORWARD, RobotMap.Camera.UP, RobotMap.Camera.PITCH); CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); @@ -80,6 +84,7 @@ public void robotInit() { autonChooser.setDefaultOption("Four Note Path", "Four Note Path"); autonChooser.addOption("Six Note Path", "Six Note Path"); autonChooser.addOption("Three Note Path", "Three Note Path"); + autonChooser.addOption("One Note Path", "One Note Path"); SmartDashboard.putData("Auton Chooser", autonChooser); } @@ -91,9 +96,10 @@ public void robotPeriodic() { telemetry.autons("Current Auton", autonChooser.getSelected()); telemetry.publish(); + SmartDashboard.putBoolean("Alliance Red", Flip.isFlipped()); - // NetworkTableInstance.().flushLocal(); - // NetworkTableInstance.getDefault().flush(); + NetworkTableInstance.getDefault().flushLocal(); + NetworkTableInstance.getDefault().flush(); } @Override @@ -104,12 +110,17 @@ public void autonomousInit() { Autons.fourNotePath.schedule(); break; case "Three Note Path": - Drivetrain.getInstance().setPose(new Pose2d(1.51, 1.36, Rotation2d.fromDegrees(180))); + Drivetrain.getInstance().setPose(new Pose2d(1.51, 1.36 + Units.inchesToMeters(3), Rotation2d.fromDegrees(180))); Autons.threeNotePath.schedule(); break; case "Six Note Path": Drivetrain.getInstance().setPose(new Pose2d(1.72, 5.56, Rotation2d.fromDegrees(180))); Autons.sixNotePath.schedule(); + break; + case "One Note Path": + Drivetrain.getInstance().setPose(new Pose2d(0.90, 6.45 + , Rotation2d.fromDegrees(-119.49))); + Autons.oneNote.schedule(); // default: // Drivetrain.getInstance().setPose(Flip.apply(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180)))); // Autons.fourNotePath.schedule(); @@ -125,15 +136,13 @@ public void teleopInit() { Autons.fourNotePath.cancel(); Autons.threeNotePath.cancel(); Autons.sixNotePath.cancel(); - Drivetrain.getInstance().setYaw(0); + Autons.oneNote.cancel(); } @Override public void teleopPeriodic() { // Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_SPEED); // Shooter.getInstance().setShooter(RobotMap.Shooter.SHOOTING_SPEED); - Telemetry.putBoolean("pivot", "Is Stalling", Pivot.getInstance().isStalling()); - if (Pivot.getInstance().isStalling()) System.out.println("pivot stallinggggg"); } @Override @@ -142,7 +151,12 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + Intake.getInstance().setRollerPower(0); + Intake.getInstance().setDeployPos(0); + Indexer.getInstance().setPower(0); + Shooter.getInstance().setIndexer(0); + } @Override public void testInit() {} diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 4df3c72..3ed90bb 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -2,6 +2,9 @@ import com.ctre.phoenix6.signals.InvertedValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -50,7 +53,8 @@ public static final class SwerveModule { public static final int[] TRANSLATION_IDS = {22, 5, 28, 10}; // translation motors inverted - public static final InvertedValue[] TRANSLATION_INVERTS = {InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive}; + public static final InvertedValue[] TRANSLATION_INVERTS = (FIRST_BOT) ? new InvertedValue[]{InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive} + : new InvertedValue[]{InvertedValue.Clockwise_Positive, InvertedValue.Clockwise_Positive, InvertedValue.Clockwise_Positive, InvertedValue.Clockwise_Positive}; // ids for rotation motors public static final int[] ROTATION_IDS = {15, 18, 27, 25}; @@ -63,9 +67,9 @@ public static final class SwerveModule { // offsets of cancoders of each swerve module (in rotations) public static final double[] CAN_CODER_OFFSETS = (FIRST_BOT) ? new double[]{-0.155518, -0.069092, -0.360596-0.299805, -0.402588-0.193848} - : new double[]{0.403320, 0.367188, 0.099609, -0.030518}; + : new double[]{-0.141357+0.5, -0.115479+0.5, 0.099609, -0.030518}; // current limit constants for translation motors - public static final double TRANS_CURRENT_LIMIT = 30; + public static final double TRANS_CURRENT_LIMIT = 40; public static final double TRANS_THRESHOLD_CURRENT = 55; public static final double TRANS_THRESHOLD_TIME= 0.1; @@ -92,14 +96,12 @@ public static final class SwerveModule { // Translation FF Values public static final double TRANSLATION_kS = 0.13561; // TODO public static final double TRANSLATION_kV = 2.1051; // TODO - public static final double TRANSLATION_kA = 0.2737; // TODO + public static final double TRANSLATION_kA = 1.5737; // TODO // pid - public static final double TRANSLATION_kP = 0.27581; // TODO - public static final double TRANSLATION_kI = 0.00; // TODO + public static final double TRANSLATION_kP = 1.7; // TODO + public static final double TRANSLATION_kI = 0.5; // TODO public static final double TRANSLATION_kD = 0.00; // TODO - - public static final double MAX_SPEED = 5.0; } public static final class Drivetrain { @@ -111,14 +113,16 @@ public static final class Drivetrain { public static final double MIN_OUTPUT = 0.05; // PID for omega (turning) control - public static final double OMEGA_kP = 5; // TODO + public static final double OMEGA_kP = 10; // TODO public static final double OMEGA_kI = 0.0; public static final double OMEGA_kD = 0.1; - public static final double MAX_ERROR_SPEAKER = 0.5; //TODO + public static final double MAX_ERROR_SPEAKER = Math.toRadians(5); - public static final double VX_AMP_kP = 1; + public static final double VX_AMP_kP = 0.5; + public static final double VY_AMP_kP = 0.5; public static final double MAX_ERROR_VX_AMP = Units.inchesToMeters(1.0); - public static final double OMEGA_AMP_KP = 5; + public static final double MAX_ERROR_VY_AMP = Units.inchesToMeters(3.0); + public static final double OMEGA_AMP_KP = 2.5; public static final double MAX_ERROR_AMP_DEG = 1; public static final double VX_STAGE_kP = 0.19; @@ -129,27 +133,29 @@ public static final class Drivetrain { public static final double VERTICAL_DEG_STAGE = 10; // Robot Dimensions - public static final double ROBOT_LENGTH = Units.inchesToMeters(28); - public static final double ROBOT_WIDTH = Units.inchesToMeters(28); + public static final double ROBOT_LENGTH = Units.inchesToMeters(22.75); + public static final double ROBOT_WIDTH = Units.inchesToMeters(22.75); public static final double MAX_DRIVING_SPEED = 5.0; // m/s //TODO - public static final double MAX_ACCELERATION = 7; + public static final double EXTENDED_MAX_DRIVING_SPEED = 0.3; + public static final double MAX_ACCELERATION = 8.5; public static final double MAX_ANGLE_VELOCITY = Math.PI; + public static final double EXTENDED_MAX_ANGLE_VELOCITY = Math.PI/10; public static final double MAX_ANGLE_ACCELERATION = MAX_ANGLE_VELOCITY / 2.0; /** * PID values for X, Y, and Rotation (THETA) */ - public static double X_kP = 0.0; // TODO + public static double X_kP = 1.0; // TODO public static double X_kI = 0.0; public static double X_kD = 0.0; - public static double Y_kP = 0.0; // TODO + public static double Y_kP = 1.0; // TODO public static double Y_kI = 0.0; public static double Y_kD = 0.0; - public static double THETA_kP = 0.0; // TODO + public static double THETA_kP = 5.3; // TODO public static double THETA_kI = 0.0; public static double THETA_kD = 0.0; @@ -160,7 +166,7 @@ public static final class Drivetrain { } public static final class Shooter { - public static final int MASTER_ID = 3; + public static final int MASTER_ID = 38; public static final int FOLLOWER_ID = 6; public static final int INDEXER_ID = 4; @@ -175,56 +181,65 @@ public static final class Shooter { public static final double SHOOTER_CURRENT_LIMIT_TIME = 0.1; public static final int INDEXER_CURRENT_LIMIT = 80; - public static final double INDEXING_SPEED = 0.4; - public static final double SHOOTING_SPEED = 0.8; + public static final double INDEXING_SPEED = 0.25; + public static final double SHOOTING_SPEED = 1.0; - public static final double REVVED_RPS = 1200.0 / 60.0; - public static final double REVVED_AMP_RPS = 500.0 / 60.0; + public static final double AUTON_REVVED_RPS = 370.0 / 60.0; + public static final double REVVED_RPS = 1000.0 / 60.0; + public static final double REVVED_AMP_RPS = 60.0 / 60.0; public static enum Goal { AMP, - SPEAKER, + SPEAKER } } public static final class Pivot { public static final int MASTER_ID = 24; public static final int LIMIT_SWITCH_ID = 2; + public static final int CAN_CODER_ID = 30; - public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; + public static final double CAN_CODER_OFFSET = 0.032717; + + public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; - public static final double ZERO_SPEED = -0.08; + public static final double ZERO_SPEED = -0.35; + + public static final double PIVOT_AMP_kP = 25; // 11 + public static final double PIVOT_AMP_kS = 0; + public static final double PIVOT_AMP_kD = 3.1; + public static final double PIVOT_AMP_kG = 0.4; - public static final double PIVOT_kP = 41.939; - public static final double PIVOT_kG = 0; - public static final double PIVOT_kS = 0; //0.10574; - public static final double PIVOT_kV = 0; //69.706; - public static final double PIVOT_kA = 0; //82.618; + public static final double PIVOT_kG = 0.6; + public static final double PIVOT_kP = 62; // 13 + public static final double PIVOT_kI = 0; //2.7 + public static final double PIVOT_kD = 6.5; //0; + public static final double PIVOT_kS = 0; // 0.69673; + public static final double OFFSET_ANGLE = 21; - public static final double TRAP1_ANGLE = 50; - public static final double TRAP2_ANGLE = 40; - public static final double TRAP_SCORE_ANGLE = 30; - public static final double AMP_ANGLE = 58; + public static final double CLIMB_ANGLE = 100; + public static final double AMP_ANGLE = 80; + public static final double QUICK_ANGLE = 30; - public static final double PIVOT_GEAR_RATIO = 106.0 + 2.0/3.0; + public static final double PIVOT_GEAR_RATIO = 37.5; + public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees - public static final double STALLING_CURRENT = 80; + public static final double STALLING_CURRENT = 50; // stalls when current is 50 - public static final double MAX_ERROR = 1; // degrees + public static final double MAX_ERROR_SPEAKER = 1.5; // degrees + public static final double MAX_ERROR_AMP = 2; - public static final double PIVOT_FORWARD_SOFT_LIMIT = 65; + public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; - public static final double MAX_CRUISE_ACCLERATION = 344.53125 / 2; - public static final double MAX_CRUISE_VElOCITY = 344.53125; + public static final double MAX_CRUISE_ACCLERATION = 817.03 / 2; + public static final double MAX_CRUISE_VElOCITY = 817.03 * 0.8; public static enum Goal { AMP, SPEAKER, - TRAP1, - TRAP2, - TRAP_SCORE + CLIMB, } } @@ -235,19 +250,21 @@ public static final class Elevator { public static final int LIMIT_SWITCH_ID = 1; public static final double ELEVATOR_kP = 1; - public static final double ELEVATOR_kG = 0; + public static final double ELEVATOR_kG = 0.01; public static final double TRAP_HEIGHT = 0; // motor rotations - public static final double STAGE_HEIGHT = 20; + public static final double STAGE_HEIGHT = 89; public static final double ELEVATOR_FORWARD_SOFT_LIMIT = 0; public static final double ELEVATOR_REVERSE_SOFT_LIMIT = 0; + public static final double ELEVATOR_STALLING_CURRENT = 80; + public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; - public static final double ZERO_SPEED = -0.3; + public static final double EXTEND_SPEED = 0.5; - public static final double MAX_ERROR = 1; // motor rotations + public static final double MAX_ERROR = 2; // motor rotations } public static final class Intake { @@ -255,35 +272,41 @@ public static final class Intake { public static final int ROLLER_ID = 7; public static final int LIMIT_SWITCH_ID = 0; - public static final boolean DEPLOY_INVERT = false; + public static final boolean DEPLOY_INVERT = true; public static final boolean ROLLER_INVERT = false; public static final double ZERO_SPEED = -0.5; - public static final double ROLLER_SPEED = 0.6; + public static final double ROLLER_SPEED = 0.7; public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; - public static final double INTAKE_DEPLOY = 23; // rotations + public static final double INTAKE_DEPLOY = 23.5; // rotations - public static final int ROLLER_CURRENT_LIMIT = 90; + public static final double INTAKE_STALLING_CURRENT = 70; + + public static final int ROLLER_CURRENT_LIMIT = 70; } public static final class Indexer { - public static final int MASTER_ID = 5; + public static final int MASTER_ID = 43; - public static final boolean MASTER_INVERT = false; + public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; - public static final double INDEXING_SPEED = 0.8; + public static final double INDEXING_SPEED = 0.795; - public static final int CURRENT_LIMIT = 90; + public static final int CURRENT_LIMIT = 50; + public static final int CURRENT_LIMIT_THRESHOLD = 70; + public static final double CURRENT_LIMIT_TIME = 0.2; } public static final class Camera { public static final double FORWARD = Units.inchesToMeters(11.823); // TODO - public static final double UP = Units.inchesToMeters(11.823); // meters + public static final double UP = Units.inchesToMeters(21.47); // meters public static final double PITCH = -30.494; // degrees + public static final Pose3d CAMERA_POSE_ROBOT_SPACE = new Pose3d(FORWARD, 0, UP, new Rotation3d(0, PITCH, 0)); + public static final int[] ID_SPEAKER_BLUE = {7, 8}; public static final int[] ID_SPEAKER_RED = {3, 4}; public static final int ID_AMP_BLUE = 6; @@ -291,7 +314,7 @@ public static final class Camera { public static final int[] ID_STAGE_BLUE = {14, 15, 16}; public static final int[] ID_STAGE_RED = {11, 12, 13}; - public static final double MAX_ERROR_VISION_POSE = 1.0; // meters + public static final double MAX_ERROR_VISION_POSE = 5; // meters } } \ No newline at end of file diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 822b3e2..2174790 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -1,46 +1,74 @@ package frc.robot.auton; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.RobotMap; +import frc.robot.commands.CommandGroups; +import frc.robot.commands.elevator.MoveToPosition; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.swerve.Drivetrain; // import frc.robot.commands.CommandGroups; // import frc.robot.commands.indexer.IndexToShooter; // import frc.robot.commands.shooter.MoveNoteToShooter; // import frc.robot.commands.shooter.ShooterManual; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; public class Autons { public static final SequentialCommandGroup fourNotePath = new SequentialCommandGroup( - new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false), - new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true) + CommandGroups.getFullZeroCommand(), + CommandGroups.getFullShootSpeaker(), + new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(133), false).alongWith(CommandGroups.getFullIntakeCommand()), + new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), + new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-133), false).alongWith(CommandGroups.getFullIntakeCommand()), + CommandGroups.getFullShootSpeaker() + // new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()) ); public static final SequentialCommandGroup threeNotePath = new SequentialCommandGroup( - new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(140.1), false), - new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false), + CommandGroups.getFullZeroCommand(), + new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(140.1), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(180), false), + new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.noteToMiddle2_three, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(138.59), false), - new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(180), true) + new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(140.1), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), + new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(140.1), true).alongWith(CommandGroups.getFullShootSpeaker()) + + ); + + public static final SequentialCommandGroup oneNote = new SequentialCommandGroup( + CommandGroups.getFullZeroCommand(), + CommandGroups.getFullShootSpeaker(), + new WaitCommand(10), + new SwervePositionController(Trajectories.note1_one, () -> Rotation2d.fromDegrees(180), false) ); + public static final SequentialCommandGroup sixNotePath = new SequentialCommandGroup ( - new SwervePositionController(Trajectories.startToNote1_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note1ToShoot1_six, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot1ToNote2_six, () -> Rotation2d.fromDegrees(158.62), false), - new SwervePositionController(Trajectories.note2ToShoot2_six, () -> Rotation2d.fromDegrees(178.45), true), - new SwervePositionController(Trajectories.note5ToShooter_six, () -> Rotation2d.fromDegrees(-160.76), true), - new SwervePositionController(Trajectories.shoot2ToNote3_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note3ToShoot3_six, () -> Rotation2d.fromDegrees(-156.72), true), - new SwervePositionController(Trajectories.shoot3ToNote4_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note4ToShoot4_six, () -> Rotation2d.fromDegrees(-164.18), true), - new SwervePositionController(Trajectories.shoot4ToNote5_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note5ToShoot5_six, () -> Rotation2d.fromDegrees(-162.5), true) + new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(133.13), false), + new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true), + new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false), + new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true), + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false), + new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true) + // new SwervePositionController(Trajectories.startToNote1_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note1ToShoot1_six, () -> Rotation2d.fromDegrees(180), true), + // new SwervePositionController(Trajectories.shoot1ToNote2_six, () -> Rotation2d.fromDegrees(158.62), false), + // new SwervePositionController(Trajectories.note2ToShoot2_six, () -> Rotation2d.fromDegrees(178.45), true), + // new SwervePositionController(Trajectories.note5ToShooter_six, () -> Rotation2d.fromDegrees(-160.76), true), + // new SwervePositionController(Trajectories.shoot2ToNote3_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note3ToShoot3_six, () -> Rotation2d.fromDegrees(-156.72), true), + // new SwervePositionController(Trajectories.shoot3ToNote4_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note4ToShoot4_six, () -> Rotation2d.fromDegrees(-164.18), true), + // new SwervePositionController(Trajectories.shoot4ToNote5_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note5ToShoot5_six, () -> Rotation2d.fromDegrees(-162.5), true) ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/auton/SwervePositionController.java b/src/main/java/frc/robot/auton/SwervePositionController.java index c5f50cd..2b53098 100644 --- a/src/main/java/frc/robot/auton/SwervePositionController.java +++ b/src/main/java/frc/robot/auton/SwervePositionController.java @@ -35,7 +35,7 @@ public SwervePositionController( this.trajectory = trajectory; this.referenceHeading = referenceHeading; this.alignToSpeaker = alignToSpeaker; - thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.enableContinuousInput(-0.5, 0.5); addRequirements(Drivetrain.getInstance()); } @@ -75,8 +75,8 @@ public void execute() { double xFF = desiredState.velocityMetersPerSecond * desiredState.poseMeters.getRotation().getCos(); // meters per second double yFF = desiredState.velocityMetersPerSecond * desiredState.poseMeters.getRotation().getSin(); // meters per second - double thetaFF = MathUtil.clamp((alignToSpeaker) ? Drivetrain.getInstance().alignToSpeaker() - : thetaController.calculate(currentRotation.getRotations(), referenceAngle.getRotations()), + double thetaFF = MathUtil.clamp((alignToSpeaker) ? -Drivetrain.getInstance().alignToSpeaker() + : -thetaController.calculate(currentRotation.getRotations(), referenceAngle.getRotations()), -clampAdd, clampAdd); // radians per second /** @@ -89,7 +89,7 @@ public void execute() { * Send values to Drivetrain */ ChassisSpeeds adjustedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF, - currentPose.getRotation()); + Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation()); Drivetrain.getInstance().setAngleAndDrive(adjustedSpeeds); } diff --git a/src/main/java/frc/robot/auton/Trajectories.java b/src/main/java/frc/robot/auton/Trajectories.java index d3b79e6..b49c3e4 100644 --- a/src/main/java/frc/robot/auton/Trajectories.java +++ b/src/main/java/frc/robot/auton/Trajectories.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; @@ -17,15 +18,15 @@ public class Trajectories { */ public static Trajectory startToShoot1_three = generateTrajectory( List.of( - new Pose2d(1.51, 1.36, Rotation2d.fromDegrees(180)), - new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(140.1))), + new Pose2d(1.51, 1.36 + Units.inchesToMeters(3), Rotation2d.fromDegrees(180)), + new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1))), 5.0, 2.5, 0.0, 0.0, true); public static Trajectory shoot1ToMiddle1_three = generateTrajectory( - List.of(new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(140.1)), + List.of(new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1)), new Pose2d(5.18, 1.87, Rotation2d.fromDegrees(180))), 5.0, 2.5, @@ -50,14 +51,14 @@ public class Trajectories { false); public static Trajectory middle2ToShoot2_three = generateTrajectory( List.of(new Pose2d(5.18, 1.87, Rotation2d.fromDegrees(180)), - new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(138.59))), + new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1))), 5.0, 2.5, 3.0, 0.0, false); public static Trajectory shoot2ToNote2_three= generateTrajectory( - List.of(new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(138.59)), + List.of(new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1)), new Pose2d(7.69, 0.74, Rotation2d.fromDegrees(180))), 5.0, 2.5, @@ -66,7 +67,7 @@ public class Trajectories { false); public static Trajectory note2ToShoot3_three = generateTrajectory( List.of(new Pose2d(7.69, 0.74, Rotation2d.fromDegrees(180)), - new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(140.0))), + new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.0))), 5.0, 2.5, 0.0, @@ -78,61 +79,61 @@ public class Trajectories { */ public static Trajectory startToNote1_four = generateTrajectory( List.of(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180)), - new Pose2d(2.57, 4.15, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + new Pose2d(2.45, 4.15, Rotation2d.fromDegrees(180))), + 4.0, + 2.5, 0.0, 0.0, true); public static Trajectory note1ToShoot1_four = generateTrajectory( - List.of(new Pose2d(2.57, 4.15, Rotation2d.fromDegrees(180)), - new Pose2d(1.72, 5.41, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + List.of(new Pose2d(2.45, 4.15, Rotation2d.fromDegrees(180)), + new Pose2d(1.4, 5.3, Rotation2d.fromDegrees(180))), + 5.0, + 4.0, 0, 0, false); public static Trajectory shoot1ToNote2_four = generateTrajectory( - List.of(new Pose2d(1.72, 5.41, Rotation2d.fromDegrees(180)), - new Pose2d(2.73, 5.54, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + List.of(new Pose2d(1.4, 5.3, Rotation2d.fromDegrees(180)), + new Pose2d(2.6, 5.59, Rotation2d.fromDegrees(180))), + 5.0, + 4.0, 0, 0, - false); + true); public static Trajectory note2ToShoot2_four = generateTrajectory( - List.of(new Pose2d(2.73, 5.54, Rotation2d.fromDegrees(180)), - new Pose2d(1.84, 5.96, Rotation2d.fromDegrees(-157.83))), - 2.0, - 1.0, + List.of(new Pose2d(2.6, 5.59, Rotation2d.fromDegrees(180)), + new Pose2d(2.0, 6.0, Rotation2d.fromDegrees(180))), + 5.0, + 4.0, 0.0, 0.0, false); public static Trajectory shoot2ToNote3_four = generateTrajectory( - List.of(new Pose2d(1.84, 5.96, Rotation2d.fromDegrees(-157.83)), - new Pose2d(2.73, 7.06, Rotation2d.fromDegrees(-136.16))), - 2.0, - 1.0, + List.of(new Pose2d(2.0, 6.0, Rotation2d.fromDegrees(180)), + new Pose2d(2.45, 7.06, Rotation2d.fromDegrees(180))), + 5.0, + 4.0, 0.0, 0.0, - false); + true); public static Trajectory note3ToShoot3_four = generateTrajectory( - List.of(new Pose2d(2.73, 7.06, Rotation2d.fromDegrees(-136.16)), - new Pose2d(1.84, 6.43, Rotation2d.fromDegrees(-143.16))), - 2.0, - 1.0, + List.of(new Pose2d(2.45, 7.06, Rotation2d.fromDegrees(180)), + new Pose2d(2.1, 6.43, Rotation2d.fromDegrees(-136.16))), + 5.0, + 4.0, 0.0, 0.0, false); - public static Trajectory shoot3ToEnd_four = generateTrajectory( - List.of(new Pose2d(1.84, 6.43, Rotation2d.fromDegrees(-143.16)), - new Pose2d(1.79, 5.43, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, - 0.0, - 0.0, - false); + // public static Trajectory shoot3ToEnd_four = generateTrajectory( + // List.of(new Pose2d(1.90, 6.43, Rotation2d.fromDegrees(-136.16)), + // new Pose2d(1.79, 5.43, Rotation2d.fromDegrees(180))), + // 5.0, + // 4.0, + // 0.0, + // 0.0, + // false); /** * Six Note Path (Top) @@ -141,31 +142,31 @@ public class Trajectories { List.of(new Pose2d(1.72, 5.56, Rotation2d.fromDegrees(180)), new Pose2d(2.56, 4.16, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 0.0, true); public static Trajectory note1ToShoot1_six = generateTrajectory( List.of(new Pose2d(2.56, 4.16, Rotation2d.fromDegrees(180)), - new Pose2d(2.28, 4.89, Rotation2d.fromDegrees(158.62))), + new Pose2d(2.40, 4.89, Rotation2d.fromDegrees(158.62))), 5.0, - 2.5, + 4.0, 0.0, 3.0, true); public static Trajectory shoot1ToNote2_six = generateTrajectory( - List.of(new Pose2d(2.28, 4.89, Rotation2d.fromDegrees(158.62)), - new Pose2d(2.41, 5.56, Rotation2d.fromDegrees(178.45))), + List.of(new Pose2d(2.40, 4.89, Rotation2d.fromDegrees(158.62)), + new Pose2d(2.47, 5.56, Rotation2d.fromDegrees(178.45))), 5.0, - 2.5, + 4.0, 3.0, 0.0, false); public static Trajectory note2ToShoot2_six = generateTrajectory( - List.of(new Pose2d(2.41, 5.56, Rotation2d.fromDegrees(178.45)), + List.of(new Pose2d(2.47, 5.56, Rotation2d.fromDegrees(178.45)), new Pose2d(2.10, 6.15, Rotation2d.fromDegrees(-160.76))), 5.0, - 2.5, + 4.0, 0.0, 2.0, true); @@ -173,7 +174,7 @@ public class Trajectories { List.of(new Pose2d(2.10, 6.15, Rotation2d.fromDegrees(-160.76)), new Pose2d(2.56, 7.01, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 2.0, 2.0, false); @@ -181,7 +182,7 @@ public class Trajectories { List.of(new Pose2d(2.56, 7.01, Rotation2d.fromDegrees(180)), new Pose2d(5.37, 6.58, Rotation2d.fromDegrees(-156.72))), 5.0, - 2.5, + 4.0, 2.0, 0.0, false); @@ -189,7 +190,7 @@ public class Trajectories { List.of(new Pose2d(5.37, 6.58, Rotation2d.fromDegrees(-156.72)), new Pose2d(5.79, 6.34, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 3.0, false); @@ -197,7 +198,7 @@ public class Trajectories { List.of(new Pose2d(5.79, 6.34, Rotation2d.fromDegrees(180)), new Pose2d(7.97,5.79, Rotation2d.fromDegrees(-164.18))), 5.0, - 2.5, + 4.0, 3.0, 0.0, false); @@ -205,7 +206,7 @@ public class Trajectories { List.of(new Pose2d(7.97, 5.79, Rotation2d.fromDegrees(-164.18)), new Pose2d(5.82,6.41, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 3.0, false); @@ -213,7 +214,7 @@ public class Trajectories { List.of(new Pose2d(5.82, 6.41, Rotation2d.fromDegrees(180)), new Pose2d(7.97,7.47, Rotation2d.fromDegrees(-162.5))), 5.0, - 2.5, + 4.0, 3.0, 0.0, false); @@ -221,10 +222,17 @@ public class Trajectories { List.of(new Pose2d(7.97,7.47, Rotation2d.fromDegrees(-162.5)), new Pose2d(5.37, 6.58, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 0.0, false); + + public static Trajectory note1_one = generateTrajectory( + List.of(new Pose2d(0.90, 6.45, Rotation2d.fromDegrees(-119.49)), + new Pose2d(4.31, 7.36, Rotation2d.fromDegrees(180))), + 2.0, 1.0, 0.0, 0.0, true); + + /** * generates a Trajectory given a list of Pose2d points, max velocity, max * acceleration, start velocity, and end velocity, and if flipped due to diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index d23325d..b0e010d 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -1,27 +1,72 @@ package frc.robot.commands; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.signals.NeutralModeValue; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.RobotMap; +import frc.robot.RobotMap.Pivot; +import frc.robot.commands.elevator.MoveToPosition; import frc.robot.commands.elevator.ZeroElevator; import frc.robot.commands.indexer.IndexToShooter; import frc.robot.commands.intake.IntakeNote; import frc.robot.commands.intake.ZeroIntake; import frc.robot.commands.pivot.PivotToAngle; +import frc.robot.commands.pivot.QuickPivot; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; import frc.robot.commands.shooter.RevShooter; -import frc.robot.commands.shooter.ShootNote; +import frc.robot.commands.shooter.ShootAmpNote; +import frc.robot.commands.shooter.ShootSpeakerNote; +import frc.robot.subsystems.Elevator; public class CommandGroups { - public static final Command FULL_ZERO = new ZeroPivot().alongWith(new ZeroElevator(), new ZeroIntake()); - - public static final Command FULL_INTAKE = new MoveNoteToShooter().raceWith(new IndexToShooter().alongWith(new IntakeNote(), new ZeroPivot())); - - public static final Command FULL_SHOOT_SPEAKER = new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER).alongWith(new RevShooter(RobotMap.Shooter.Goal.SPEAKER)).andThen(new ShootNote()); + + public static Command getFullZeroCommand() { + return new ZeroPivot() + .alongWith(new ZeroElevator(), new ZeroIntake()); + } + + public static Command getFullIntakeCommand() { + return new MoveNoteToShooter() + .raceWith(new IndexToShooter() + .alongWith(new IntakeNote())) + .andThen(new QuickPivot(), new ZeroPivot()); + } - public static final Command FULL_SHOOT_AMP = new PivotToAngle(RobotMap.Pivot.Goal.AMP).alongWith(new RevShooter(RobotMap.Shooter.Goal.AMP)).andThen(new ShootNote()); + public static Command getFullShootSpeaker() { + return new RevShooter(RobotMap.Shooter.Goal.SPEAKER) + .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) + .andThen(new ShootSpeakerNote()) + .andThen(new ZeroPivot()); + } + + public static Command getFullShootAmp() { + return new PivotToAngle(RobotMap.Pivot.Goal.AMP) + .andThen(new RevShooter(RobotMap.Shooter.Goal.AMP)) + .andThen(new ShootAmpNote()).andThen(new ZeroPivot()); + } + + public static Command getFullClimb() { + return new InstantCommand(() -> Elevator.getInstance().setFollowerNeutralMode( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast))) + .andThen(new PivotToAngle(Pivot.Goal.CLIMB) + .alongWith(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT))); + } + + public static Command getFullRetractClimb() { + return new MoveToPosition(0) + .andThen(new InstantCommand(() -> Elevator.getInstance().setFollowerNeutralMode( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)))); + } + + public static Command getFullShootNoAlign() { + return new RevShooter(RobotMap.Shooter.Goal.AMP) + .andThen(new ShootAmpNote()).andThen(new ZeroPivot()); + } + - public static final Command FULL_SHOOT_NO_ALIGN = new RevShooter(RobotMap.Shooter.Goal.AMP).andThen(new ShootNote()); // public static final Command PRE_DRIVEFWD_CLIMB = new PivotToAngle(RobotMap.Pivot.Goal.TRAP1); // wait for drive forward // public static final Command PRE_DRIVEBKWD_CLIMB = new PivotToAngle(RobotMap.Pivot.Goal.TRAP2).andThen(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT).alongWith(new ZeroPivot())); // wait for drive backwards diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index 0917ea4..e566790 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.swerve.Drivetrain; import frc.robot.OI; import frc.robot.util.Flip; @@ -48,9 +49,12 @@ public void execute() { vy *= -1; } // Scaling velocities based on multipliers + + vx = scaleValues(vx, RobotMap.Drivetrain.MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); vy = scaleValues(vy, RobotMap.Drivetrain.MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); omega = scaleValues(omega, RobotMap.Drivetrain.MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); + omega = Drivetrain.getInstance().adjustPigeon(omega); @@ -61,13 +65,23 @@ public void execute() { // aligns to speaker if (OI.getInstance().getDriver().getRightBumperState()) { omega = Drivetrain.getInstance().alignToSpeaker(); - Drivetrain.getInstance().setPreviousHeading(-Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); + Drivetrain.getInstance().setPreviousHeading(Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); } // aligns to amp - if (OI.getInstance().getDriver().getLeftBumperState()) { + if (OI.getInstance().getDriver().getRightTrigger() > 0.5) { vx = -Drivetrain.getInstance().alignToAmp()[0]; - omega = Drivetrain.getInstance().alignToAmp()[1]; + // vy = -Drivetrain.getInstance().alignToAmp()[1]; + // vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + vy *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + omega = Drivetrain.getInstance().alignToAmp()[2]; + Drivetrain.getInstance().setPreviousHeading(Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); + } + + if (OI.getInstance().getDriver().getLeftTrigger() > 0.5) { + vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + vy *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + omega *= RobotMap.Drivetrain.EXTENDED_MAX_ANGLE_VELOCITY; } // if rotational velocity is very small diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index 7e72ada..ce4571d 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -1,25 +1,34 @@ -package frc.robot.commands.elevator; +// package frc.robot.commands.elevator; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotMap; -import frc.robot.subsystems.Elevator; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.RobotMap; +// import frc.robot.subsystems.Elevator; -public class ElevatorManual extends Command{ - public ElevatorManual() { - addRequirements(Elevator.getInstance()); - } +// public class ElevatorManual extends Command{ +// private double power; - @Override - public void execute() { - Elevator.getInstance().setElevatorPower(0.6); - } +// public ElevatorManual(double power) { +// this.power = power; +// addRequirements(Elevator.getInstance()); +// } - public boolean isFinished() { - return false; - } +// @Override +// public void execute() { +// Elevator.getInstance().setElevatorPower(power); +// } - @Override - public void end(boolean interrupted) { - Elevator.getInstance().setElevatorPower(0); - } -} +// public boolean isFinished() { +// if (power > 0) +// return Elevator.getInstance().isAtTop(); +// else +// return Elevator.getInstance().isStalling(); +// } + +// @Override +// public void end(boolean interrupted) { +// if (power > 0) +// Elevator.getInstance().setElevatorPower(RobotMap.Elevator.ELEVATOR_kG); +// else +// Elevator.getInstance().setElevatorPower(0); +// } +// } diff --git a/src/main/java/frc/robot/commands/elevator/MoveToPosition.java b/src/main/java/frc/robot/commands/elevator/MoveToPosition.java index 2e0372c..accbbf1 100644 --- a/src/main/java/frc/robot/commands/elevator/MoveToPosition.java +++ b/src/main/java/frc/robot/commands/elevator/MoveToPosition.java @@ -7,25 +7,28 @@ public class MoveToPosition extends Command { - private double setpoint; + private double height; - public MoveToPosition(double desired) { - this.setpoint = desired; + public MoveToPosition(double height) { + this.height = height; addRequirements(Elevator.getInstance()); } public void execute() { - Elevator.getInstance().moveToPosition(setpoint); + if (Elevator.getInstance().getPosition() > height) + Elevator.getInstance().setElevatorPower(-0.6); + else if (Elevator.getInstance().getPosition() < height) + Elevator.getInstance().setElevatorPower(0.6); } public boolean isFinished() { - return MathUtil.compareSetpoint(Elevator.getInstance().getPosition(), setpoint, RobotMap.Elevator.MAX_ERROR); + return MathUtil.compareSetpoint(Elevator.getInstance().getPosition(), height, RobotMap.Elevator.MAX_ERROR); } - // public void end(boolean interrupted) { - // Elevator.getInstance().moveToPosition(0); - // Elevator.getInstance().setElevatorPower(0); - // } + @Override + public void end(boolean interrupted) { + Elevator.getInstance().setElevatorPower(0); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java index b2fc573..3533705 100644 --- a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -10,16 +10,16 @@ public ZeroElevator() { } public void execute() { - Elevator.getInstance().setElevatorPower(RobotMap.Elevator.ZERO_SPEED); + Elevator.getInstance().setElevatorPower(-RobotMap.Elevator.EXTEND_SPEED); } public boolean isFinished() { - return Elevator.getInstance().isLimitHit(); + return Elevator.getInstance().isLimitHit() || Elevator.getInstance().isStalling(); } - // public void end(boolean interrupted) { - // Elevator.getInstance().setElevatorPower(0); - // Elevator.getInstance().setSensorPosition(0); - // } + public void end(boolean interrupted) { + Elevator.getInstance().setElevatorPower(0); + Elevator.getInstance().setSensorPosition(0); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/OuttakeNote.java b/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java similarity index 76% rename from src/main/java/frc/robot/commands/intake/OuttakeNote.java rename to src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java index 0754fb7..c6a44d7 100644 --- a/src/main/java/frc/robot/commands/intake/OuttakeNote.java +++ b/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java @@ -1,19 +1,20 @@ package frc.robot.commands.intake; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.OI; import frc.robot.RobotMap; import frc.robot.subsystems.Intake; -public class OuttakeNote extends Command { +public class OuttakeStuckNote extends Command { - public OuttakeNote() + public OuttakeStuckNote() { addRequirements(Intake.getInstance()); } @Override public void execute() { - Intake.getInstance().setDeployPos(0); + Intake.getInstance().setDeployPos(RobotMap.Intake.INTAKE_DEPLOY); Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_OUTAKE_SPEED); } @@ -23,6 +24,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { + Intake.getInstance().setDeployPos(0); Intake.getInstance().setRollerPower(0); } diff --git a/src/main/java/frc/robot/commands/intake/ZeroIntake.java b/src/main/java/frc/robot/commands/intake/ZeroIntake.java index 7e01911..7e29a50 100644 --- a/src/main/java/frc/robot/commands/intake/ZeroIntake.java +++ b/src/main/java/frc/robot/commands/intake/ZeroIntake.java @@ -14,7 +14,7 @@ public void execute() { } public boolean isFinished() { - return Intake.getInstance().limitSwitchHit(); + return Intake.getInstance().isStalling(); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 0cadb1b..48d0a04 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -1,5 +1,7 @@ package frc.robot.commands.pivot; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; import frc.robot.subsystems.Pivot; @@ -9,9 +11,11 @@ public class PivotToAngle extends Command { private RobotMap.Pivot.Goal setpoint; private double ref; + private Debouncer debouncer; public PivotToAngle(RobotMap.Pivot.Goal goal) { setpoint = goal; + debouncer = new Debouncer(0.35, DebounceType.kRising); // 0.5 if not working addRequirements(Pivot.getInstance()); } @@ -19,30 +23,38 @@ public void execute() { switch (setpoint) { case SPEAKER: ref = Pivot.getInstance().getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker()); + Pivot.getInstance().moveToPosition(ref); break; case AMP: ref = RobotMap.Pivot.AMP_ANGLE; + Pivot.getInstance().moveToPositionAmp(ref); break; - case TRAP1: - ref = RobotMap.Pivot.TRAP1_ANGLE; - break; - case TRAP2: - ref = RobotMap.Pivot.TRAP2_ANGLE; - break; - case TRAP_SCORE: - ref = RobotMap.Pivot.TRAP_SCORE_ANGLE; + case CLIMB: + ref = RobotMap.Pivot.CLIMB_ANGLE; + Pivot.getInstance().moveToPosition(ref); break; } - Pivot.getInstance().moveToPosition(ref); } + public double getRef() { + return ref; + } + public boolean isFinished() { - return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR); + double error; + switch(setpoint) { + case SPEAKER: + error = RobotMap.Pivot.MAX_ERROR_SPEAKER; + return debouncer.calculate(MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, error)); + default: + error = RobotMap.Pivot.MAX_ERROR_AMP; + return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, error); + } } - // public void end(boolean interrupted) { - // Pivot.getInstance().setPercentOutput(0); - // } + public void end(boolean interrupted) { + Pivot.getInstance().moveToPosition(ref); + } } diff --git a/src/main/java/frc/robot/commands/pivot/QuickPivot.java b/src/main/java/frc/robot/commands/pivot/QuickPivot.java new file mode 100644 index 0000000..c09c2cd --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/QuickPivot.java @@ -0,0 +1,24 @@ +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotMap; +import frc.robot.subsystems.Pivot; + +public class QuickPivot extends Command { + public QuickPivot() { + addRequirements(Pivot.getInstance()); + } + + public void execute() { + Pivot.getInstance().setPercentOutput(0.3); + } + + public boolean isFinished() { + return Pivot.getInstance().getPosition() >= RobotMap.Pivot.QUICK_ANGLE; + } + + public void end(boolean interrupted) { + Pivot.getInstance().setPercentOutput(0); + } + +} diff --git a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java index 8d40857..9790d06 100644 --- a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java +++ b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java @@ -1,5 +1,6 @@ package frc.robot.commands.pivot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; import frc.robot.subsystems.Pivot; @@ -11,15 +12,18 @@ public ZeroPivot() { public void execute() { Pivot.getInstance().setPercentOutput(RobotMap.Pivot.ZERO_SPEED); + // Pivot.getInstance().setPercentOutput(0); } public boolean isFinished() { - return Pivot.getInstance().isLimitHit() || Pivot.getInstance().isStalling(); + return Pivot.getInstance().getPosition() <= 1; + // return Pivot.getInstance().isLimitHit(); + // return Pivot.getInstance().isStalling(); } public void end(boolean interrupted) { Pivot.getInstance().setPercentOutput(0); - Pivot.getInstance().setSensorPosition(0); + // Pivot.getInstance().setSensorPosition(0); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/RevShooter.java b/src/main/java/frc/robot/commands/shooter/RevShooter.java index 96f890c..7260679 100644 --- a/src/main/java/frc/robot/commands/shooter/RevShooter.java +++ b/src/main/java/frc/robot/commands/shooter/RevShooter.java @@ -1,5 +1,6 @@ package frc.robot.commands.shooter; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; import frc.robot.subsystems.Shooter; @@ -29,6 +30,8 @@ public boolean isFinished() { case AMP: return Shooter.getInstance().isShooterAmpRevved(); case SPEAKER: + if (DriverStation.isAutonomous()) + return Shooter.getInstance().isAutonShooterSpeakerRevved(); return Shooter.getInstance().isShooterSpeakerRevved(); default: return false; diff --git a/src/main/java/frc/robot/commands/shooter/ShootNote.java b/src/main/java/frc/robot/commands/shooter/ShootAmpNote.java similarity index 88% rename from src/main/java/frc/robot/commands/shooter/ShootNote.java rename to src/main/java/frc/robot/commands/shooter/ShootAmpNote.java index f4784d5..d7a4868 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootNote.java +++ b/src/main/java/frc/robot/commands/shooter/ShootAmpNote.java @@ -4,8 +4,8 @@ import frc.robot.RobotMap; import frc.robot.subsystems.Shooter; -public class ShootNote extends Command { - public ShootNote() { +public class ShootAmpNote extends Command { + public ShootAmpNote() { addRequirements(Shooter.getInstance()); } diff --git a/src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java b/src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java new file mode 100644 index 0000000..7f7c51e --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java @@ -0,0 +1,26 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotMap; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.swerve.Drivetrain; + +public class ShootSpeakerNote extends Command { + public ShootSpeakerNote() { + addRequirements(Shooter.getInstance()); + } + + public void execute() { + if (Drivetrain.getInstance().alignedToSpeaker()) + Shooter.getInstance().setIndexer(RobotMap.Shooter.SHOOTING_SPEED); + } + + public boolean isFinished() { + return !Shooter.getInstance().shooterIndexerOccupied(); + } + + public void end(boolean interrupted) { + Shooter.getInstance().setShooter(0); + Shooter.getInstance().setIndexer(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index ceab68c..99eff52 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,6 +1,8 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; @@ -8,6 +10,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.RobotMap; public class Elevator extends SubsystemBase { @@ -36,17 +39,12 @@ private void configMotors() { masterConfig.MotorOutput.Inverted = RobotMap.Elevator.MASTER_INVERT; - masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; masterConfig.Voltage.PeakForwardVoltage = RobotMap.MAX_VOLTAGE; masterConfig.Voltage.PeakReverseVoltage = -RobotMap.MAX_VOLTAGE; - masterConfig.CurrentLimits.StatorCurrentLimitEnable = true; - masterConfig.CurrentLimits.StatorCurrentLimit = 90; - masterConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - masterConfig.CurrentLimits.SupplyCurrentLimit = 90; - masterConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = RobotMap.Elevator.ELEVATOR_FORWARD_SOFT_LIMIT; masterConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RobotMap.Elevator.ELEVATOR_REVERSE_SOFT_LIMIT; masterConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; @@ -59,6 +57,13 @@ private void configMotors() { master.getConfigurator().apply(masterConfig); follower.getConfigurator().apply(followerConfig); + + + + } + + public void setFollowerNeutralMode(MotorOutputConfigs motorConfig) { + follower.getConfigurator().apply(motorConfig); } public double getPosition() { @@ -87,6 +92,14 @@ public boolean isLimitHit() { return !limitSwitch.get(); } + public boolean isAtTop() { + return master.getPosition().getValue() >= RobotMap.Elevator.STAGE_HEIGHT; + } + + public boolean isStalling() { + return master.getStatorCurrent().getValue() >= RobotMap.Elevator.ELEVATOR_STALLING_CURRENT; + } + public static Elevator getInstance() { if(instance == null) { instance = new Elevator(); @@ -94,4 +107,10 @@ public static Elevator getInstance() { return instance; } + public TalonFX getMaster() { + return master; + } + public TalonFX getFollower() { + return follower; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index db39f53..30125d4 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -1,6 +1,10 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; @@ -9,23 +13,27 @@ public class Indexer extends SubsystemBase { private static Indexer instance; - private CANSparkMax master; + private TalonFX master; private Indexer() { - master = new CANSparkMax(RobotMap.Indexer.MASTER_ID, MotorType.kBrushless); - - master.setInverted(RobotMap.Indexer.MASTER_INVERT); + master = new TalonFX(RobotMap.Indexer.MASTER_ID); configMotors(); } public void configMotors() { - master.restoreFactoryDefaults(); + master.clearStickyFaults(); + + TalonFXConfiguration masterConfiguration = new TalonFXConfiguration(); + masterConfiguration.MotorOutput.Inverted = RobotMap.Indexer.MASTER_INVERT; + + masterConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast; + masterConfiguration.CurrentLimits.SupplyCurrentLimit = RobotMap.Indexer.CURRENT_LIMIT; + masterConfiguration.CurrentLimits.SupplyCurrentThreshold = RobotMap.Indexer.CURRENT_LIMIT_THRESHOLD; + masterConfiguration.CurrentLimits.SupplyTimeThreshold = RobotMap.Indexer.CURRENT_LIMIT_TIME; + masterConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; - master.setSmartCurrentLimit(RobotMap.Indexer.CURRENT_LIMIT); - master.setIdleMode(IdleMode.kBrake); - - master.burnFlash(); + master.getConfigurator().apply(masterConfiguration); } public void setPower(double power) { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index fb97c91..eadb8e3 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -8,6 +8,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; @@ -56,7 +57,7 @@ public void setSensorPosition(double pos) { } public void setDeployPos(double rots) { - deployPositionPID.setReference(rots, ControlType.kPosition); + deployPositionPID.setReference(rots, ControlType.kPosition, RobotMap.PID.SLOT_INDEX, 2.5); } public boolean limitSwitchHit() { @@ -70,6 +71,10 @@ public void setDeployPower(double power) { public void setRollerPower(double power) { roller.setVoltage(power * RobotMap.MAX_VOLTAGE); } + + public boolean isStalling() { + return deploy.getOutputCurrent() >= RobotMap.Intake.INTAKE_STALLING_CURRENT; + } public static Intake getInstance() { if (instance == null) instance = new Intake(); diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index d9d69b7..1d1a7a6 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -3,21 +3,22 @@ import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Velocity; @@ -26,8 +27,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Robot; import frc.robot.RobotMap; -import frc.robot.util.Telemetry; +import frc.robot.util.MathUtil; +// import frc.robot.util.Telemetry; public class Pivot extends SubsystemBase { private static Pivot instance; @@ -36,6 +39,8 @@ public class Pivot extends SubsystemBase { private DigitalInput limitSwitch; + private CANcoder canCoder; + private InterpolatingDoubleTreeMap speakerAngles; // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. private final MutableMeasure _appliedVoltage = mutable(Volts.of(0)); @@ -51,12 +56,23 @@ private Pivot() { limitSwitch = new DigitalInput(RobotMap.Pivot.LIMIT_SWITCH_ID); - speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 0.0); // TODO + canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); + speakerAngles = new InterpolatingDoubleTreeMap(); + speakerAngles.put(0.0, 10.0); + speakerAngles.put(1.787, 22.1 - 2.0); + speakerAngles.put(2.043, 25.5 - 2.0); + speakerAngles.put(2.361, 30.0 - 2.0); + speakerAngles.put(2.839, 39.643 - 5.0); + speakerAngles.put(3.228, 42.574 - 5.0); + speakerAngles.put(3.713, 45.914 - 5.0); + speakerAngles.put(4.156, 46.5 - 5.0); + speakerAngles.put(4.507, 47.463 - 2.0); + speakerAngles.put(5.051, 48.990 - 2.0); + configCANcoder(); configMotors(); } - + private void configMotors() { master.clearStickyFaults(); @@ -66,42 +82,57 @@ private void configMotors() { masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - masterConfig.Feedback.SensorToMechanismRatio = RobotMap.Pivot.PIVOT_GEAR_RATIO; - - masterConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = RobotMap.Pivot.PIVOT_FORWARD_SOFT_LIMIT; - masterConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RobotMap.Pivot.PIVOT_REVERSE_SOFT_LIMIT; - masterConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - masterConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + // masterConfig.Feedback.SensorToMechanismRatio = RobotMap.Pivot.PIVOT_GEAR_RATIO; masterConfig.Slot0.kP = RobotMap.Pivot.PIVOT_kP; - // masterConfig.Slot0.kS = RobotMap.Pivot.PIVOT_kS; - // masterConfig.Slot0.kV = RobotMap.Pivot.PIVOT_kV; - // masterConfig.Slot0.kA = RobotMap.Pivot.PIVOT_kA; - masterConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - masterConfig.Slot0.kG = RobotMap.Pivot.PIVOT_kG; + masterConfig.Slot0.kI = RobotMap.Pivot.PIVOT_kI; + masterConfig.Slot0.kD = RobotMap.Pivot.PIVOT_kD; + masterConfig.Slot0.kS = RobotMap.Pivot.PIVOT_kS; + + masterConfig.Slot1.kP = RobotMap.Pivot.PIVOT_AMP_kP; + masterConfig.Slot1.kS = RobotMap.Pivot.PIVOT_AMP_kS; + masterConfig.Slot1.kD = RobotMap.Pivot.PIVOT_AMP_kD; masterConfig.MotionMagic.MotionMagicCruiseVelocity = RobotMap.Pivot.MAX_CRUISE_VElOCITY; masterConfig.MotionMagic.MotionMagicAcceleration = RobotMap.Pivot.MAX_CRUISE_ACCLERATION; + masterConfig.Feedback.FeedbackRemoteSensorID = canCoder.getDeviceID(); + masterConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + master.getConfigurator().apply(masterConfig); } + private void configCANcoder() { + canCoder.clearStickyFaults(); + + CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); + canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + canCoderConfig.MagnetSensor.MagnetOffset = -RobotMap.Pivot.CAN_CODER_OFFSET; // offset is ADDED, so -offset + + canCoder.getConfigurator().apply(canCoderConfig); + } + /* * Get pivot angle in degrees */ public double getPosition() { - return master.getPosition().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; + return canCoder.getAbsolutePosition().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; } public boolean isStalling() { return master.getStatorCurrent().getValue() > RobotMap.Pivot.STALLING_CURRENT; } + public double getMasterCurrent() { + return master.getStatorCurrent().getValue(); + } + /* * Get pivot angle in degrees per second */ public double getVelocity() { - return master.getVelocity().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; + return canCoder.getVelocity().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; } public double getVoltage() { @@ -109,12 +140,23 @@ public double getVoltage() { } public void moveToPosition(double desiredAngle) { - MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle); - master.setControl(motionMagicVoltage); + double kG = RobotMap.Pivot.PIVOT_kG * Math.cos(Math.toRadians(getPosition() + RobotMap.Pivot.OFFSET_ANGLE)); + MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle / RobotMap.Pivot.PIVOT_ROT_TO_ANGLE); + master.setControl(motionMagicVoltage.withFeedForward(kG)); + } + + public void moveToPositionAmp(double desiredAngle) { + double kG = RobotMap.Pivot.PIVOT_AMP_kG * Math.cos(Math.toRadians(getPosition() + RobotMap.Pivot.OFFSET_ANGLE)); + MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle / RobotMap.Pivot.PIVOT_ROT_TO_ANGLE); + master.setControl(motionMagicVoltage.withFeedForward(kG).withSlot(RobotMap.PID.PID_AUXILIARY)); } public void setPercentOutput(double power) { - DutyCycleOut percentOutput = new DutyCycleOut(power); + if (MathUtil.compareDouble(power, 0)) + { + master.stopMotor(); + } + VoltageOut percentOutput = new VoltageOut(power * RobotMap.MAX_VOLTAGE); master.setControl(percentOutput); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 72749e2..8ddca37 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; +import frc.robot.subsystems.swerve.Drivetrain; public class Shooter extends SubsystemBase { private static Shooter instance; @@ -70,6 +71,10 @@ public boolean isShooterSpeakerRevved() { return master.getRotorVelocity().getValue() >= RobotMap.Shooter.REVVED_RPS; } + public boolean isAutonShooterSpeakerRevved() { + return master.getRotorVelocity().getValue() >= RobotMap.Shooter.AUTON_REVVED_RPS; + } + public boolean isShooterAmpRevved() { return master.getRotorVelocity().getValue() >= RobotMap.Shooter.REVVED_AMP_RPS; } diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 2cfb9fc..79a9632 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -22,18 +22,24 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotMap; import frc.robot.util.Flip; import frc.robot.util.Limelight; -import frc.robot.util.Telemetry; +// import frc.robot.util.Telemetry; public class Drivetrain extends SubsystemBase { private static Drivetrain instance; @@ -59,12 +65,14 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaSpeakerController = new PIDController(RobotMap.Drivetrain.OMEGA_kP, RobotMap.Drivetrain.OMEGA_kI, RobotMap.Drivetrain.OMEGA_kD); private static PIDController vxAmpController = new PIDController(RobotMap.Drivetrain.VX_AMP_kP, 0, 0); - private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); + private static PIDController vyAmpController = new PIDController(RobotMap.Drivetrain.VY_AMP_kP, 0, 0); + + public static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.3, 0.3, 0.1); // increase to trust encoder (state) - // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust vsion - // measurements less + private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust encoder (state) + // measurements less + private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.5); // increase to trust vsion + // measurements less private boolean robotCentric; @@ -97,6 +105,7 @@ private Drivetrain() { omegaSpeakerController.enableContinuousInput(-Math.PI, Math.PI); vxAmpController.setTolerance(RobotMap.Drivetrain.MAX_ERROR_VX_AMP); + vyAmpController.setTolerance(RobotMap.Drivetrain.MAX_ERROR_VY_AMP); omegaAmpController.setTolerance(RobotMap.Drivetrain.MAX_ERROR_AMP_DEG); omegaAmpController.setSetpoint(Math.PI / 2.0); omegaAmpController.enableContinuousInput(-Math.PI, Math.PI); @@ -129,8 +138,9 @@ private void initPigeon() { pigeonConfigs.MountPose.MountPoseRoll = 0; pigeonConfigs.MountPose.MountPoseYaw = 0; pigeonConfigs.Pigeon2Features.EnableCompass = false; + pigeon.getYaw().setUpdateFrequency(250); + pigeon.getConfigurator().apply(pigeonConfigs); - pigeon.setYaw(0); } /** @@ -141,24 +151,16 @@ private void initPigeon() { * @return adjusted rotational speed */ public double adjustPigeon(double omega) { + double currHeading = poseEstimator.getEstimatedPosition().getRotation().getDegrees(); if (Math.abs(omega) <= RobotMap.Drivetrain.MIN_OUTPUT) { - omega = -RobotMap.Drivetrain.PIGEON_kP * (prevHeading - getHeading()); + omega = -RobotMap.Drivetrain.PIGEON_kP * (prevHeading - currHeading); } else { - prevHeading = getHeading(); + prevHeading = currHeading; } return omega; } - /* - * Returns yaw of pigeon in degrees (heading of robot) - */ - public double getHeading() { - double yaw = pigeon.getYaw().getValue(); - // SmartDashboard.putNumber("pigeon heading", pigeon.getYaw()); - return yaw; - } - /** * @return pitch of pigeon in degrees */ @@ -215,7 +217,7 @@ public void toggleRobotCentric() { * @return heading of pigeon as a Rotation2d */ public Rotation2d getRotation() { - return Rotation2d.fromDegrees(getHeading()); + return pigeon.getRotation2d(); } /** @@ -242,7 +244,7 @@ public void setPose(Pose2d pose) { swerveModules[2].zeroTranslation(); swerveModules[3].zeroTranslation(); setYaw(pose.getRotation().getDegrees()); - poseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); + poseEstimator.resetPosition(getRotation(), getModulePositions(), pose); } /** @@ -265,14 +267,20 @@ public void setPreviousHeading(double prev) { } public double alignToSpeaker() { + double degRefSpeaker = getRefAngleSpeaker(); + + // Telemetry.putNumber("swerve", "Desired Omega", refAngleFieldRel.getRadians()); + // Telemetry.putNumber("swerve", "Current Omega", getPoseEstimatorPose2d().getRotation().getRadians()); + // return 0; + return omegaSpeakerController.calculate(getPoseEstimatorPose2d().getRotation().getRadians(), + Math.toRadians(degRefSpeaker)); + } + + public double getRefAngleSpeaker() { Rotation2d refAngleFieldRel = Flip.apply(RobotMap.Field.SPEAKER) .minus(getPoseEstimatorPose2d().getTranslation()).getAngle(); - - Telemetry.putNumber("swerve", "Desired Omega", refAngleFieldRel.getRadians()); - Telemetry.putNumber("swerve", "Current Omega", getPoseEstimatorPose2d().getRotation().getRadians()); - return omegaSpeakerController.calculate(getPoseEstimatorPose2d().getRotation().getRadians(), - refAngleFieldRel.getRadians()); + return refAngleFieldRel.getDegrees(); } public double getDistanceToSpeaker() { @@ -282,16 +290,19 @@ public double getDistanceToSpeaker() { public double[] alignToAmp() { - double refXFieldRel = Flip.apply(RobotMap.Field.AMP) - .minus(getPoseEstimatorPose2d().getTranslation()).getX(); + Translation2d refFieldRel = Flip.apply(RobotMap.Field.AMP) + .minus(getPoseEstimatorPose2d().getTranslation()); - double vx = MathUtil.clamp(vxAmpController.calculate(refXFieldRel, 0), -1, 1); + double vx = MathUtil.clamp(vxAmpController.calculate(refFieldRel.getX(), 0), -1, 1); double omega = MathUtil.clamp(omegaAmpController.calculate(getPoseEstimatorPose2d().getRotation().getRadians()), -1, 1); - - return new double[]{vx, omega}; + double vy = MathUtil.clamp(vyAmpController.calculate(refFieldRel.getY(), Units.inchesToMeters(14 + 7)), -1, 1); + + return new double[]{vx, vy, omega}; } public boolean alignedToSpeaker() { + if (DriverStation.isAutonomous()) + return true; return omegaSpeakerController.atSetpoint(); } @@ -406,15 +417,29 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return _sysId.dynamic(direction); } + public boolean isPoseValid() { + return Limelight.isPoseValid(Limelight.getBotPose2d()); + } + + public boolean isPoseNear() { + return Limelight.isPoseNear(getPoseEstimatorPose2d(), Limelight.getBotPose2d()); + } + @Override public void periodic() { updatePose(); if (Limelight.hasTargets()) { Pose2d visionBot = Limelight.getBotPose2d(); - if (Limelight.isPoseValid(visionBot, getPoseEstimatorPose2d())) { - poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); + double x = Limelight.getDistanceToTag(); + + double stdX = .085 * x; + double stdTheta = .05 + .085 * x; + if (Limelight.isPoseValid(visionBot)) { + poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdX, stdX, stdTheta)); } } - } + + } } + \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 4e93d44..e3cda45 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.RobotMap; +// import frc.robot.util.Telemetry; import frc.robot.util.Telemetry; public class SwerveModule { @@ -86,6 +87,8 @@ private void configTranslation() { transConfig.Slot0.kI = RobotMap.SwerveModule.TRANSLATION_kI; transConfig.Slot0.kD = RobotMap.SwerveModule.TRANSLATION_kD; + translation.getVelocity().setUpdateFrequency(250); + translation.getPosition().setUpdateFrequency(250); translation.getConfigurator().apply(transConfig); } @@ -110,6 +113,8 @@ private void configRotation() { rotConfig.Slot0.kI = RobotMap.SwerveModule.ROTATION_kI; rotConfig.Slot0.kD = RobotMap.SwerveModule.ROTATION_kD; + rotation.getPosition().setUpdateFrequency(250); + rotation.getConfigurator().apply(rotConfig); } @@ -139,8 +144,8 @@ public void setAngleAndDrive(SwerveModuleState state) { Telemetry.putModule(ID, "Desired Velocity", state.speedMetersPerSecond); Telemetry.putModule(ID, "Current Velocity", getSpeed()); - Telemetry.putModule(ID, "Desired Angle", state.angle.getDegrees()); - Telemetry.putModule(ID, "Current Angle", getAngle()); + // Telemetry.putModule(ID, "Desired Angle", state.angle.getDegrees()); + // Telemetry.putModule(ID, "Current Angle", getAngle()); // SmartDashboard.putNumber("Desired Velocity " + ID, state.speedMetersPerSecond); // SmartDashboard.putNumber("Current Velocity " + ID, getSpeed()); diff --git a/src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json b/src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json new file mode 100644 index 0000000..493161c --- /dev/null +++ b/src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json @@ -0,0 +1,145 @@ +{ + "version": "3.0.2", + "layout": [ + { + "type": 5, + "fields": [], + "listFields": [ + [ + { + "type": "Robot", + "key": "NT:/SmartDashboard/Field/Robot", + "sourceTypeIndex": 0, + "sourceType": 5 + }, + { + "type": "Ghost", + "key": "NT:/TEAM 1072/Odometry/llPose", + "sourceTypeIndex": 0, + "sourceType": 5 + } + ] + ], + "options": { + "game": "2024 Field", + "unitDistance": "meters", + "unitRotation": "degrees", + "origin": "right", + "size": 0.75, + "allianceBumpers": "auto", + "allianceOrigin": "blue", + "orientation": "blue left, red right" + }, + "configHidden": false, + "title": "Odometry" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/TEAM 1072/Pivot/Pivot Angle", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/TEAM 1072/Pivot/Pivot Speaker Setpoint", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "pivot" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Field/Robot/2", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/TEAM 1072/Odometry/Speaker Align Ref in Deg", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "rot" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/TEAM 1072/Drive/SwerveModuleStates/Measured/0/speed", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/TEAM 1072/Drive/SwerveModuleStates/Setpoints Optimized/0/speed", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Line Graph" + } + ] +} diff --git a/src/main/java/frc/robot/util/Flip.java b/src/main/java/frc/robot/util/Flip.java index d14973c..2def527 100644 --- a/src/main/java/frc/robot/util/Flip.java +++ b/src/main/java/frc/robot/util/Flip.java @@ -61,6 +61,8 @@ public static Trajectory.State apply(Trajectory.State state) { * field flipped along y-axis */ public static boolean isFlipped() { - return DriverStation.getAlliance().get() == Alliance.Red; + if (DriverStation.getAlliance().isPresent()) + return DriverStation.getAlliance().get() == Alliance.Red; + return false; } } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 3050ca9..7a11acc 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -1,39 +1,127 @@ package frc.robot.util; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.RobotMap; +import frc.robot.subsystems.swerve.Drivetrain; public final class Limelight { + private static NetworkTableInstance table; public static final String LIMELIGHT_TABLE_KEY = "limelight"; - public static final NetworkTable TABLE = NetworkTableInstance.getDefault().getTable(LIMELIGHT_TABLE_KEY); public static Pose2d getBotPose2d() { return toPose2D(getBotPoseVal()); } - public static boolean isPoseValid(Pose2d botPose, Pose2d visionBot) { - return visionBot.getTranslation().getDistance(botPose.getTranslation()) < RobotMap.Camera.MAX_ERROR_VISION_POSE; + public static double getBestTargetArea() { + return getValue("ta").getDouble(0.0); + } + + public static boolean isPoseValid(Pose2d visionPose) { + return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE && + !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); + } + + // + public static double getDistanceToTag() { + Pose2d targetPose = new Pose2d(); + if (MathUtil.compareDouble(getApriltagId(), 1)) + { + targetPose = new Pose2d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 2)) + { + targetPose = new Pose2d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 3)) + { + targetPose = new Pose2d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 4)) + { + targetPose = new Pose2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 5)) + { + targetPose = new Pose2d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.00), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 6)) + { + targetPose = new Pose2d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.00), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 7)) + { + targetPose = new Pose2d(Units.inchesToMeters(-1.50), Units.inchesToMeters(218.42), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 8)) + { + targetPose = new Pose2d(Units.inchesToMeters(-1.50), Units.inchesToMeters(196.17), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 9)) + { + targetPose = new Pose2d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 10)) + { + targetPose = new Pose2d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 11)) + { + targetPose = new Pose2d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 12)) + { + targetPose = new Pose2d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 13)) + { + targetPose = new Pose2d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 14)) + { + targetPose = new Pose2d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 15)) + { + targetPose = new Pose2d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 16)) + { + targetPose = new Pose2d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), new Rotation2d()); + } + + Pose2d robotPose = getBotPose2d(); + + return robotPose.getTranslation().getDistance(targetPose.getTranslation()); + } + + public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { + return getDistanceBetweenPose(pose, visionPose) <= 1.0; + + } + + public static double getDistanceBetweenPose(Pose2d pose, Pose2d visionPose) { + return pose.getTranslation().getDistance(visionPose.getTranslation()); } public static double getTimestamp() { - return Timer.getFPGATimestamp() - getBotPoseVal()[6] / 1000.0; + return Timer.getFPGATimestamp() - (getValue("tl").getDouble(0.0) + getValue("cl").getDouble(0.0)) / 1000.0; } public static boolean hasTargets() { - return MathUtil.compareDouble(TABLE.getEntry("tv").getDouble(0.0), 1.0); + return getApriltagId() > -1; } public static boolean atAmp() { if (hasTargets()) { - if (DriverStation.getAlliance().get() == Alliance.Red) { + if (Flip.isFlipped()) { return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_AMP_RED); } else { return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_AMP_BLUE); @@ -44,28 +132,48 @@ public static boolean atAmp() { public static boolean atSpeaker() { if (hasTargets()) { - if (DriverStation.getAlliance().get() == Alliance.Red) { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[1]); - } else { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[1]); - } + + if (Flip.isFlipped()) { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[1]); + } else { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[1]); + } } return false; } public static boolean atStage() { if (hasTargets()) { - if (DriverStation.getAlliance().get() == Alliance.Red) { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[1]) || - MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[2]); - } else { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[1]) || - MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[2]); - } + if (Flip.isFlipped()) { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[1]) || + MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[2]); + } else { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[1]) || + MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[2]); + } + } return false; } + + public static int getNumTargets() { + return countStringOccurrences(NetworkTableInstance.getDefault().getTable(LIMELIGHT_TABLE_KEY).getEntry("json").getString(""), "pts"); + } + + public static int countStringOccurrences(String str, String substr) { + int occ = 0; + for (int i = 0; i < str.length() - substr.length() + 1; i++) { + if (str.substring(i, i + substr.length()).equals(substr)) { + occ++; + } + } + + return occ; + } + + + /* entries[0] = forward; * entries[1] = side; * entries[2] = up; @@ -73,28 +181,28 @@ public static boolean atStage() { * entries[4] = pitch; * entries[5] = yaw; */ public static void setCameraPose(double forward, double up, double pitch) { - TABLE.getEntry("camerapose_robotspace_set").setDoubleArray(new double[]{forward, 0, up, 0, pitch, 0}); + getValue("camerapose_robotspace").setDoubleArray(new double[]{forward, 0, up, 0, pitch, 0}); } public static void setPipeline(double idx) { - TABLE.getEntry("pipeline").setDouble(idx); + getValue("pipeline").setDouble(idx); } public static double getApriltagId() { - return TABLE.getEntry("tid").getDouble(0.0); + return getValue("tid").getDouble(0.0); } public static double getTx() { - return TABLE.getEntry("tx").getDouble(0.0); + return getValue("tx").getDouble(0.0); } public static double getTy() { - return TABLE.getEntry("ty").getDouble(0.0); + return getValue("ty").getDouble(0.0); } - private static double[] getBotPoseVal() { - return TABLE.getEntry("botpose_wpiblue").getDoubleArray(new double[6]); + public static double[] getBotPoseVal() { + return getValue("botpose_wpiblue").getDoubleArray(new double[7]); } private static Pose2d toPose2D(double[] inData){ @@ -108,4 +216,13 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + public static NetworkTableEntry getValue(String key) { + if (table == null) { + table = NetworkTableInstance.getDefault(); + table.getTable(LIMELIGHT_TABLE_KEY).getEntry("pipeline").setNumber(0); + } + + return table.getTable(LIMELIGHT_TABLE_KEY).getEntry(key); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 77c29b2..99baf31 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -1,5 +1,6 @@ package frc.robot.util; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -16,7 +17,7 @@ public class Telemetry { // private NetworkTable table; private NetworkTableInstance inst; - private NetworkTable main; + private static NetworkTable main; // private static NetworkTable _realOutputs; private static NetworkTable _swerve; @@ -100,97 +101,115 @@ public void autons(String entry, String list) { } public void odometry() { - NetworkTableEntry rotation = _odometry.getEntry("Rotation"); - rotation.setDouble(drive.getRotation().getRadians()); + // NetworkTableEntry rotation = _odometry.getEntry("Rotation"); + // rotation.setDouble(drive.getRotation().getDegrees()); + Pose2d visionPose = Limelight.getBotPose2d(); + NetworkTableEntry isPoseNear = _odometry.getEntry("Pose Nearness"); + isPoseNear.setBoolean(drive.isPoseNear()); + + NetworkTableEntry isPoseValid = _odometry.getEntry("Pose Validity"); + isPoseValid.setBoolean(drive.isPoseValid()); + + NetworkTableEntry distanceToTag = _odometry.getEntry("Tag Distance"); + distanceToTag.setDouble(Limelight.getDistanceToTag()); + + NetworkTableEntry distanceBetweenPoses = _odometry.getEntry("Pose Distance"); + distanceBetweenPoses.setDouble(Limelight.getDistanceBetweenPose(drive.getPoseEstimatorPose2d(), visionPose)); + + NetworkTableEntry llPose = _odometry.getEntry("llPose"); + llPose.setDoubleArray(new double[]{visionPose.getX(), visionPose.getY(), visionPose.getRotation().getDegrees()}); + + NetworkTableEntry refSpeakerDeg = _odometry.getEntry("Speaker Align Ref in Deg"); + refSpeakerDeg.setDouble(drive.getRefAngleSpeaker()); } public void debug() { - NetworkTableEntry a_D = _driver.getEntry("Button A"); - a_D.setBoolean(oiDriver.getButtonAState()); + // NetworkTableEntry a_D = _driver.getEntry("Button A"); + // a_D.setBoolean(oiDriver.getButtonAState()); - NetworkTableEntry b_D = _driver.getEntry("Button B"); - b_D.setBoolean(oiDriver.getButtonBState()); + // NetworkTableEntry b_D = _driver.getEntry("Button B"); + // b_D.setBoolean(oiDriver.getButtonBState()); - NetworkTableEntry x_D = _driver.getEntry("Button X"); - x_D.setBoolean(oiDriver.getButtonXState()); + // NetworkTableEntry x_D = _driver.getEntry("Button X"); + // x_D.setBoolean(oiDriver.getButtonXState()); - NetworkTableEntry y_D = _driver.getEntry("Button Y"); - y_D.setBoolean(oiDriver.getButtonYState()); + // NetworkTableEntry y_D = _driver.getEntry("Button Y"); + // y_D.setBoolean(oiDriver.getButtonYState()); - NetworkTableEntry leftBumper_D = _driver.getEntry("Left Bumper"); - leftBumper_D.setBoolean(oiDriver.getLeftBumperState()); + // NetworkTableEntry leftBumper_D = _driver.getEntry("Left Bumper"); + // leftBumper_D.setBoolean(oiDriver.getLeftBumperState()); - NetworkTableEntry rightBumper_D = _driver.getEntry("Right Bumper"); - rightBumper_D.setBoolean(oiDriver.getRightBumperState()); + // NetworkTableEntry rightBumper_D = _driver.getEntry("Right Bumper"); + // rightBumper_D.setBoolean(oiDriver.getRightBumperState()); - NetworkTableEntry leftTrigger_D = _driver.getEntry("Left Trigger"); - leftTrigger_D.setDouble(oiDriver.getLeftTrigger()); + // NetworkTableEntry leftTrigger_D = _driver.getEntry("Left Trigger"); + // leftTrigger_D.setDouble(oiDriver.getLeftTrigger()); - NetworkTableEntry rightTrigger_D = _driver.getEntry("Right Trigger"); - rightTrigger_D.setDouble(oiDriver.getRightTrigger()); + // NetworkTableEntry rightTrigger_D = _driver.getEntry("Right Trigger"); + // rightTrigger_D.setDouble(oiDriver.getRightTrigger()); - NetworkTableEntry upD_D = _driver.getEntry("Up DPad"); - upD_D.setBoolean(oiDriver.getUpDPadButtonState()); + // NetworkTableEntry upD_D = _driver.getEntry("Up DPad"); + // upD_D.setBoolean(oiDriver.getUpDPadButtonState()); - NetworkTableEntry leftD_D = _driver.getEntry("Left DPad"); - leftD_D.setBoolean(oiDriver.getLeftDPadButtonState()); + // NetworkTableEntry leftD_D = _driver.getEntry("Left DPad"); + // leftD_D.setBoolean(oiDriver.getLeftDPadButtonState()); - NetworkTableEntry downD_D = _driver.getEntry("Down DPad"); - downD_D.setBoolean(oiDriver.getDownDPadButtonState()); + // NetworkTableEntry downD_D = _driver.getEntry("Down DPad"); + // downD_D.setBoolean(oiDriver.getDownDPadButtonState()); - NetworkTableEntry rightD_D = _driver.getEntry("Right DPad"); - rightD_D.setBoolean(oiDriver.getDownDPadButtonState()); + // NetworkTableEntry rightD_D = _driver.getEntry("Right DPad"); + // rightD_D.setBoolean(oiDriver.getDownDPadButtonState()); - NetworkTableEntry select_D = _driver.getEntry("Select"); - select_D.setBoolean(oiDriver.getButtonSelectState()); + // NetworkTableEntry select_D = _driver.getEntry("Select"); + // select_D.setBoolean(oiDriver.getButtonSelectState()); - NetworkTableEntry start_D = _driver.getEntry("Start"); - start_D.setBoolean(oiDriver.getButtonStartState()); + // NetworkTableEntry start_D = _driver.getEntry("Start"); + // start_D.setBoolean(oiDriver.getButtonStartState()); - NetworkTableEntry a = _operator.getEntry("Button A"); - a.setBoolean(oiOperator.getButtonAState()); + // NetworkTableEntry a = _operator.getEntry("Button A"); + // a.setBoolean(oiOperator.getButtonAState()); - NetworkTableEntry b = _operator.getEntry("Button B"); - b.setBoolean(oiOperator.getButtonBState()); + // NetworkTableEntry b = _operator.getEntry("Button B"); + // b.setBoolean(oiOperator.getButtonBState()); - NetworkTableEntry x = _operator.getEntry("Button X"); - x.setBoolean(oiOperator.getButtonXState()); + // NetworkTableEntry x = _operator.getEntry("Button X"); + // x.setBoolean(oiOperator.getButtonXState()); - NetworkTableEntry y = _operator.getEntry("Button Y"); - y.setBoolean(oiOperator.getButtonYState()); + // NetworkTableEntry y = _operator.getEntry("Button Y"); + // y.setBoolean(oiOperator.getButtonYState()); - NetworkTableEntry leftBumper = _operator.getEntry("Left Bumper"); - leftBumper.setBoolean(oiOperator.getLeftBumperState()); + // NetworkTableEntry leftBumper = _operator.getEntry("Left Bumper"); + // leftBumper.setBoolean(oiOperator.getLeftBumperState()); - NetworkTableEntry rightBumper = _operator.getEntry("Right Bumper"); - rightBumper.setBoolean(oiOperator.getRightBumperState()); + // NetworkTableEntry rightBumper = _operator.getEntry("Right Bumper"); + // rightBumper.setBoolean(oiOperator.getRightBumperState()); - NetworkTableEntry leftTrigger = _operator.getEntry("Left Trigger"); - leftTrigger.setDouble(oiOperator.getLeftTrigger()); + // NetworkTableEntry leftTrigger = _operator.getEntry("Left Trigger"); + // leftTrigger.setDouble(oiOperator.getLeftTrigger()); - NetworkTableEntry rightTrigger = _operator.getEntry("Right Trigger"); - rightTrigger.setDouble(oiOperator.getRightTrigger()); + // NetworkTableEntry rightTrigger = _operator.getEntry("Right Trigger"); + // rightTrigger.setDouble(oiOperator.getRightTrigger()); - NetworkTableEntry upD = _operator.getEntry("Up DPad"); - upD.setBoolean(oiOperator.getUpDPadButtonState()); + // NetworkTableEntry upD = _operator.getEntry("Up DPad"); + // upD.setBoolean(oiOperator.getUpDPadButtonState()); - NetworkTableEntry leftD = _operator.getEntry("Left DPad"); - leftD.setBoolean(oiOperator.getLeftDPadButtonState()); + // NetworkTableEntry leftD = _operator.getEntry("Left DPad"); + // leftD.setBoolean(oiOperator.getLeftDPadButtonState()); - NetworkTableEntry downD = _operator.getEntry("Down DPad"); - downD.setBoolean(oiOperator.getDownDPadButtonState()); + // NetworkTableEntry downD = _operator.getEntry("Down DPad"); + // downD.setBoolean(oiOperator.getDownDPadButtonState()); - NetworkTableEntry rightD = _operator.getEntry("Right DPad"); - rightD.setBoolean(oiOperator.getDownDPadButtonState()); + // NetworkTableEntry rightD = _operator.getEntry("Right DPad"); + // rightD.setBoolean(oiOperator.getDownDPadButtonState()); - NetworkTableEntry select = _operator.getEntry("Select"); - select.setBoolean(oiOperator.getButtonSelectState()); + // NetworkTableEntry select = _operator.getEntry("Select"); + // select.setBoolean(oiOperator.getButtonSelectState()); - NetworkTableEntry start = _operator.getEntry("Start"); - start.setBoolean(oiOperator.getButtonStartState()); + // NetworkTableEntry start = _operator.getEntry("Start"); + // start.setBoolean(oiOperator.getButtonStartState()); - NetworkTableEntry isRobotCentric = _debug.getEntry("Robot Centric"); - isRobotCentric.setBoolean(drive.robotCentric()); + // NetworkTableEntry isRobotCentric = _debug.getEntry("Robot Centric"); + // isRobotCentric.setBoolean(drive.robotCentric()); } public void shooter() { @@ -199,36 +218,51 @@ public void shooter() { } public void elevator() { - NetworkTableEntry elevatorLimitSwitchHit = _elevator.getEntry("Elevator Limit Switch Hit"); - elevatorLimitSwitchHit.setBoolean(elevator.isLimitHit()); + // NetworkTableEntry elevatorLimitSwitchHit = _elevator.getEntry("Elevator Limit Switch Hit"); + // elevatorLimitSwitchHit.setBoolean(elevator.isLimitHit()); + + NetworkTableEntry elevatorStalling = _elevator.getEntry("Elevator Stalling"); + elevatorStalling.setBoolean(elevator.isLimitHit()); + + NetworkTableEntry elevatorMasterCurrent = _elevator.getEntry("Elevator Master Current"); + elevatorMasterCurrent.setDouble(elevator.getMaster().getStatorCurrent().getValue()); - NetworkTableEntry elevatorSensorPosition = _elevator.getEntry("Elevator Sensor Position"); - elevatorSensorPosition.setDouble(elevator.getPosition()); + NetworkTableEntry elevatorFollowerCurrent = _elevator.getEntry("Elevator Follower Current"); + elevatorFollowerCurrent.setDouble(elevator.getFollower().getStatorCurrent().getValue()); - NetworkTableEntry elevatorSensorVelocity = _elevator.getEntry("Sensor Velocity"); - elevatorSensorVelocity.setDouble(elevator.getVelocity()); + // NetworkTableEntry elevatorSensorPosition = _elevator.getEntry("Elevator Sensor Position"); + // elevatorSensorPosition.setDouble(elevator.getPosition()); + + // NetworkTableEntry elevatorSensorVelocity = _elevator.getEntry("Sensor Velocity"); + // elevatorSensorVelocity.setDouble(elevator.getVelocity()); } public void intake() { - NetworkTableEntry intakeLimitSwitchHit = _intake.getEntry("Intake Limit Switch Hit"); - intakeLimitSwitchHit.setBoolean(intake.limitSwitchHit()); + NetworkTableEntry intakeStalling = _intake.getEntry("Intake Stalling"); + intakeStalling.setBoolean(intake.isStalling()); } public void pivot() { - NetworkTableEntry pivotLimitSwitchHit = _pivot.getEntry("Pivot Limit Switch Hit"); - pivotLimitSwitchHit.setBoolean(pivot.isLimitHit()); - NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); pivotSensorPosition.setDouble(pivot.getPosition()); + NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); + pivotRef.setDouble(pivot.getPivotSetpoint(drive.getDistanceToSpeaker())); + NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); pivotSensorVelocity.setDouble(pivot.getVelocity()); - NetworkTableEntry pivotVelocity = _pivot.getEntry("Pivot Velocity"); - pivotVelocity.setDouble(pivot.getVelocity()); + NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); + pivotStalling.setBoolean(pivot.isStalling()); - // NetworkTableEntry setPivotAngle = _pivot.getEntry("Pivot Angle"); - // setPivotAngle.setDouble(pivot.getPivotSetpoint(0)) + NetworkTableEntry pivotLimitSwitch = _pivot.getEntry("Pivot Limit Switch"); + pivotLimitSwitch.setBoolean(pivot.isLimitHit()); + + NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); + pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); + + NetworkTableEntry setPivotAngle = _pivot.getEntry("Pivot Angle"); + setPivotAngle.setDouble(pivot.getPivotSetpoint(0)); } public void vision() { @@ -252,6 +286,7 @@ public void vision() { NetworkTableEntry distance = _limelight.getEntry("Distance To Speaker"); distance.setDouble(drive.getDistanceToSpeaker()); + } public static void putModule(int id, String entry, double number) { diff --git a/src/main/java/frc/robot/util/apriltag.vpr b/src/main/java/frc/robot/util/limelight.vpr similarity index 84% rename from src/main/java/frc/robot/util/apriltag.vpr rename to src/main/java/frc/robot/util/limelight.vpr index d651941..38a5837 100644 --- a/src/main/java/frc/robot/util/apriltag.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -1,95 +1,95 @@ -area_max:100 -area_min:0.0017850625000000004 -area_similarity:0 -aspect_max:20.000000 -aspect_min:0.000000 -black_level:0 -blue_balance:1975 -botfloorsnap:1 -botlength:0.7112 -bottype:swerve -botwidth:0.7112 -calibration_type:0 -classifier_conf:0.100000 -contour_grouping:0 -contour_sort_final:6 -convexity_max:100 -convexity_min:10 -corner_approx:5.000000 -crop_x_max:1.000000 -crop_x_min:-1.000000 -crop_y_max:1.000000 -crop_y_min:-1.000000 -cross_a_a:1 -cross_a_x:0 -cross_a_y:0 -cross_b_a:1 -cross_b_x:0 -cross_b_y:0 -desc:apriltag -desired_contour_region:0 -detector_conf:0.800000 -detector_idfilters: -dilation_steps:0 -direction_filter:0 -dual_close_sort_origin:0 -erosion_steps:0 -exposure:692 -fiducial_decoder_strictness:strict -fiducial_denoise:0.000000 -fiducial_idfilters: -fiducial_locfilters: -fiducial_method:sqpnp -fiducial_qualitythreshold:2.0 -fiducial_refine:1 -fiducial_resdiv:2 -fiducial_size:152.4 -fiducial_skip3d:0 -fiducial_type:aprilClassic36h11 -fiducial_vis_mode:3dbotposefieldspace -flicker_correction:0 -force_convex:1 -hue_max:85 -hue_min:60 -image_flip:0 -image_source:0 -img_to_show:0 -intersection_filter:0 -invert_hue:0 -lcgain:6.000000 -multigroup_max:7 -multigroup_min:1 -multigroup_rejector:0 -pipeline_led_enabled:0 -pipeline_led_power:40 -pipeline_res:0 -pipeline_type:3 -red_balance:1200 -roi_x:0.000000 -roi_y:0.000000 -rsf:0 -rspitch:0 -rsroll:0 -rss:0 -rsu:0 -rsyaw:0 -sat_max:255 -sat_min:70 -send_corners:0 -send_raw_contours:0 -solve3d:0 -solve3d_algo:0 -solve3d_bindtarget:1 -solve3d_conf:0.990000 -solve3d_error:8 -solve3d_guess:0 -solve3d_iterations:50 -solve3d_precise:0 -solve3d_zoffset:0.000000 -tsf:0 -tss:0 -tsu:0 -val_max:255 -val_min:70 -x_outlier_miqr:1.5 -y_outlier_miqr:1.5 +area_max:100 +area_min:0.0577200625 +area_similarity:0 +aspect_max:20.000000 +aspect_min:0.000000 +black_level:5 +blue_balance:1558 +botfloorsnap:1 +botlength:0.7112 +bottype:swerve +botwidth:0.7112 +calibration_type:0 +classifier_conf:0.100000 +contour_grouping:0 +contour_sort_final:6 +convexity_max:100 +convexity_min:10 +corner_approx:5.000000 +crop_x_max:1.000000 +crop_x_min:-1.000000 +crop_y_max:1.000000 +crop_y_min:-1.000000 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:limelight +desired_contour_region:0 +detector_conf:0.800000 +detector_idfilters: +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:899 +fiducial_decoder_strictness:strict +fiducial_denoise:0.000000 +fiducial_idfilters: +fiducial_locfilters: +fiducial_method:sqpnp +fiducial_qualitythreshold:2.0 +fiducial_refine:1 +fiducial_resdiv:2 +fiducial_size:152.4 +fiducial_skip3d:0 +fiducial_type:aprilClassic36h11 +fiducial_vis_mode:3dbotposefieldspace +flicker_correction:0 +force_convex:1 +hue_max:85 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +invert_hue:0 +lcgain:7.3 +multigroup_max:7 +multigroup_min:1 +multigroup_rejector:0 +pipeline_led_enabled:0 +pipeline_led_power:0 +pipeline_res:3 +pipeline_type:3 +red_balance:1303 +roi_x:0.000000 +roi_y:0.000000 +rsf:1.5 +rspitch:-30.494 +rsroll:0 +rss:0 +rsu:0.545 +rsyaw:0 +sat_max:255 +sat_min:70 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precise:0 +solve3d_zoffset:0.000000 +tsf:0 +tss:0 +tsu:0 +val_max:255 +val_min:70 +x_outlier_miqr:1.5 +y_outlier_miqr:1.5