diff --git a/.Glass/glass.json b/.Glass/glass.json index 20ee899..b42754f 100644 --- a/.Glass/glass.json +++ b/.Glass/glass.json @@ -1,29 +1,23 @@ { "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + }, "retained": { "SmartDashboard": { - "Rotation PID": { + "Auton Chooser": { "open": true }, "open": true } }, "transitory": { - "LiveWindow": { - "AngledElevator": { - "open": true - }, - "Drivetrain": { - "open": true - }, - "Ungrouped": { - "open": true - }, - "open": true - }, - "Shuffleboard": { - "open": true - }, "SmartDashboard": { "Auton Chooser": { "open": true @@ -33,89 +27,14 @@ }, "types": { "/FMSInfo": "FMSInfo", - "/LiveWindow/AngledElevator": "Subsystem", - "/LiveWindow/Claw": "Subsystem", - "/LiveWindow/Drivetrain": "Subsystem", "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/SmartDashboard/AngledElevator": "Subsystem", - "/SmartDashboard/Auton Chooser": "String Chooser", - "/SmartDashboard/Rotation PID": "String Chooser", - "/SmartDashboard/test controller": "PIDController" - }, - "windows": { - "/LiveWindow/AngledElevator": { - "window": { - "visible": true - } - }, - "/LiveWindow/Claw": { - "window": { - "visible": true - } - }, - "/LiveWindow/Drivetrain": { - "window": { - "visible": true - } - }, - "/SmartDashboard/AngledElevator": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Auton Chooser": { - "window": { - "visible": true - } - } + "/SmartDashboard/Auton Chooser": "String Chooser" } }, "NetworkTables Settings": { "mode": "Client (NT4)", - "serverTeam": "10.10.72.2" - }, - "Plots": { - "Plot <0>": { - "plots": [ - { - "height": 786, - "series": [ - { - "color": [ - 0.2980392277240753, - 0.44705885648727417, - 0.6901960968971252, - 1.0 - ], - "id": "NT:/SmartDashboard/test controller/p" - }, - { - "color": [ - 0.8666667342185974, - 0.5176470875740051, - 0.32156863808631897, - 1.0 - ], - "id": "NT:/SmartDashboard/kP" - } - ] - } - ] - }, - "Plot <1>": { - "plots": [ - { - "height": 772 - } - ] - } - }, - "enterKey": 345 + "serverTeam": "1072" + } } diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 69572af..f740bae 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -1,5 +1,6 @@ package frc.robot; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.commands.claw.ToggleClaw; import frc.robot.commands.drivetrain.AlignPitch; @@ -39,30 +40,33 @@ public XboxGamepad getOperator() { private void initBindings() { // set Right DPad Button to toggle claw - driver.getRightDPadButton().onTrue(new ToggleClaw()); + // driver.getRightDPadButton().onTrue(new ToggleClaw()); // X = HP, Y = High, B = Middle, A = Lo - driver.getButtonX().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[3])); - driver.getButtonY().whileTrue(new MoveToPosition(RobotMap.AngledElevator + // driver.getButtonX().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[3])); + // driver.getButtonY().whileTrue(new MoveToPosition(RobotMap.AngledElevator - .POSITIONS[2])); - driver.getButtonB().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1])); - driver.getButtonA().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[0])); + // .POSITIONS[2])); + // driver.getButtonB().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1])); + // driver.getButtonA().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[0])); // set LeftBumper Button to Align Pitch driver.getLeftBumper().whileTrue(new AlignPitch()); // set Start Button to Align Yaw (Reset Yaw) - driver.getButtonStart().onTrue(new InstantCommand(() -> { + driver.getRightBumper().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().setYaw(0); })); // set Select Button to Zero Elevator - driver.getButtonSelect().onTrue(new ZeroElevator()); - - // set X button on OPERATOR controller to Zero Elevator - operator.getButtonX().onTrue(new ZeroElevator()); + // driver.getButtonSelect().onTrue(new ZeroElevator()); + //operator.getRightDPadButton().debounce(0.13, DebounceType.kRising).onTrue(new ToggleClaw()); + operator.getRightDPadButton().onTrue(new ToggleClaw()); + operator.getRightBumper().onTrue(new ZeroElevator()); // set A button on OPERATOR controller to set Elevator to POSITION LOW + operator.getButtonX().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[3])); + operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2])); + operator.getButtonB().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1])); operator.getButtonA().whileTrue(new MoveToPosition(RobotMap.AngledElevator.POSITIONS[0])); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f721035..5ce8fe0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,161 +4,197 @@ package frc.robot; -import java.util.concurrent.atomic.AtomicIntegerFieldUpdater; +import org.photonvision.PhotonCamera; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.auton.Autons; +import frc.robot.auton.SwervePositionController; import frc.robot.auton.Trajectories; +import frc.robot.commands.claw.ToggleClaw; import frc.robot.commands.drivetrain.SwerveManual; import frc.robot.commands.elevator.ElevatorManual; import frc.robot.subsystems.AngledElevator; import frc.robot.subsystems.Claw; import frc.robot.subsystems.Drivetrain; +import frc.robot.util.CameraPoseEstimation; +import frc.robot.util.Telemetry; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the + * The VM is configured to automatically run this class, and to call the + * functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the + * name of this class or + * the package after creating this project, you must also update the + * build.gradle file in the * project. */ public class Robot extends TimedRobot { - - private SendableChooser autonChooser; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - LiveWindow.setEnabled(true); - LiveWindow.enableAllTelemetry(); - - // SmartDashboard.putData(RobotMap.Field.FIELD); - // SmartDashboard.putBoolean("isRed", Trajectories.isFlipped()); - - CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); - CommandScheduler.getInstance() - .setDefaultCommand(AngledElevator.getInstance(), new ElevatorManual()); - - - autonChooser = new SendableChooser(); - autonChooser.setDefaultOption("Middle And Cross Path", "Middle And Cross Path"); - autonChooser.addOption("Middle Path", "Middle Path"); - autonChooser.addOption("Bottom Path", "Bottom Path"); - autonChooser.addOption("Top Path", "Top Path"); - autonChooser.addOption("No auton", "No auton"); - SmartDashboard.putData("Auton Chooser", autonChooser); - // SmartDashboard.putData("Rotation PID Controller", Drivetrain.getInstance().thetaController); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - RobotMap.Field.FIELD.setRobotPose(Drivetrain.getInstance().getPoseEstimatorPose2d()); - - SmartDashboard.putString("Current Auton:", autonChooser.getSelected()); - SmartDashboard.putData("Rotation PID COntroller thing", Drivetrain.getInstance().thetaController); - - // SmartDashboard.putNumber("kP",); - // SmartDashboard.putNumber("kI", Drivetrain.getInstance().thetaController.getI()); - // SmartDashboard.putNumber("kD", Drivetrain.getInstance().thetaController.getD()); - - // SmartDashboard.putData(Drivetrain.getInstance()); - // SmartDashboard.putData(Claw.getInstance()); - - NetworkTableInstance.getDefault().flushLocal(); - NetworkTableInstance.getDefault().flush(); - } - - @Override - public void autonomousInit() { - // SmartDashboard.putNumber("X kP", RobotMap.SwervePositionController.X_kP); - // SmartDashboard.putNumber("Y kP", RobotMap.SwervePositionController.Y_kP); - // SmartDashboard.putNumber("Theta kP", RobotMap.SwervePositionController.THETA_kP); - switch (autonChooser.getSelected()) { - case "Top Path": - Drivetrain.getInstance() - .setPose(Trajectories.apply(new Pose2d(1.91, 4.44, Rotation2d.fromDegrees(180)))); - Autons.topPath.schedule(); - break; - case "Bottom Path": - Drivetrain.getInstance() - .setPose(Trajectories.apply(new Pose2d(1.91, 1.09, Rotation2d.fromDegrees(180)))); - Autons.bottomPath.schedule(); - break; - // case "Top Path And Push": - // Drivetrain.getInstance() - // .setPose(Trajectories.apply(new Pose2d(1.91, 4.44, Rotation2d.fromDegrees(180)))); - // Autons.topPathAndPush.schedule(); - // break; - // case "Bottom Path And Push": - // Drivetrain.getInstance() - // .setPose(Trajectories.apply(new Pose2d(1.91, 1.09, Rotation2d.fromDegrees(180)))); - // Autons.bottomPathAndPush.schedule(); - // break; - case "Middle Path": - Drivetrain.getInstance() - .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); - Autons.middlePath.schedule(); - break; - case "No auton": - Drivetrain.getInstance().setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); - Autons.noAuton.schedule(); - break; - default: - Drivetrain.getInstance() - .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); - Autons.middleAndCross.schedule(); - break; + + private SendableChooser autonChooser; + private PowerDistribution powerDistribution; + private Telemetry telemetry; + + /** + * This function is run when the robot is first started up and should be used + * for any + * initialization code. + */ + @Override + public void robotInit() { + LiveWindow.setEnabled(true); + LiveWindow.enableAllTelemetry(); + + telemetry = new Telemetry(); + + // SmartDashboard.putData(RobotMap.Field.FIELD); + // SmartDashboard.putBoolean("isRed", Trajectories.isFlipped()); + + CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); + CommandScheduler.getInstance() + .setDefaultCommand(AngledElevator.getInstance(), new ElevatorManual()); + + autonChooser = new SendableChooser(); + autonChooser.setDefaultOption("Middle And Cross Path", "Middle And Cross Path"); + autonChooser.addOption("Middle Path", "Middle Path"); + autonChooser.addOption("Bottom Path", "Bottom Path"); + autonChooser.addOption("Top Path", "Top Path"); + autonChooser.addOption("No auton", "No auton"); + SmartDashboard.putData("Auton Chooser", autonChooser); + // SmartDashboard.putData("Rotation PID Controller", + // Drivetrain.getInstance().thetaController); + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + RobotMap.Field.FIELD.setRobotPose(Drivetrain.getInstance().getPoseEstimatorPose2d()); + + SmartDashboard.putString("Current Auton:", autonChooser.getSelected()); + SmartDashboard.putData("Rotation PID COntroller thing", Drivetrain.getInstance().thetaController); + + // SmartDashboard.putNumber("kP",); + // SmartDashboard.putNumber("kI", + // Drivetrain.getInstance().thetaController.getI()); + // SmartDashboard.putNumber("kD", + // Drivetrain.getInstance().thetaController.getD()); + + // SmartDashboard.putData(Drivetrain.getInstance()); + // SmartDashboard.putData(Claw.getInstance()); + + telemetry.putAlignPitch(); + telemetry.putDrivetrain(); + telemetry.putElevator(); + telemetry.putSwerveModule(); + telemetry.putSwervePositionController(); + + NetworkTableInstance.getDefault().flushLocal(); + NetworkTableInstance.getDefault().flush(); + } + + @Override + public void autonomousInit() { + // SmartDashboard.putNumber("X kP", RobotMap.SwervePositionController.X_kP); + // SmartDashboard.putNumber("Y kP", RobotMap.SwervePositionController.Y_kP); + // SmartDashboard.putNumber("Theta kP", + // RobotMap.SwervePositionController.THETA_kP); + switch (autonChooser.getSelected()) { + case "Top Path": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 4.44, Rotation2d.fromDegrees(180)))); + Autons.topPath.schedule(); + break; + case "Bottom Path": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 1.09, Rotation2d.fromDegrees(180)))); + Autons.bottomPath.schedule(); + break; + // case "Top Path And Push": + // Drivetrain.getInstance() + // .setPose(Trajectories.apply(new Pose2d(1.91, 4.44, + // Rotation2d.fromDegrees(180)))); + // Autons.topPathAndPush.schedule(); + // break; + // case "Bottom Path And Push": + // Drivetrain.getInstance() + // .setPose(Trajectories.apply(new Pose2d(1.91, 1.09, + // Rotation2d.fromDegrees(180)))); + // Autons.bottomPathAndPush.schedule(); + // break; + case "Middle Path": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); + Autons.middlePath.schedule(); + break; + case "Middle And Cross Path with Middle Cube": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); + Autons.middleAndCrossMidCube.schedule(); + break; + case "No auton": + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); + Autons.noAuton.schedule(); + break; + default: + Drivetrain.getInstance() + .setPose(Trajectories.apply(new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)))); + Autons.middleAndCross.schedule(); + break; + } + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + Autons.bottomPath.cancel(); + Autons.topPath.cancel(); + Autons.middlePath.cancel(); + Autons.middleAndCross.cancel(); + Autons.noAuton.cancel(); + Drivetrain.getInstance().setYaw(180); + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void disabledInit() { + // AngledElevator.getInstance().setDesiredPosition(0); + AngledElevator.getInstance().moveToPosition(0); + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void testInit() { + } + + @Override + public void testPeriodic() { + } + + @Override + public void simulationInit() { + } + + @Override + public void simulationPeriodic() { } - } - - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - Autons.bottomPath.cancel(); - Autons.topPath.cancel(); - Autons.middlePath.cancel(); - Autons.middleAndCross.cancel(); - Autons.noAuton.cancel(); - Drivetrain.getInstance().setYaw(180); - } - - @Override - public void teleopPeriodic() { - AngledElevator.getInstance().setkP(SmartDashboard.getNumber("kP", RobotMap.AngledElevator.kP)); - AngledElevator.getInstance().setkG(SmartDashboard.getNumber("kG", RobotMap.AngledElevator.kG)); - Drivetrain.getInstance().setTranslationkP(SmartDashboard.getNumber("TranslationkP", RobotMap.SwerveModule.TRANSLATION_KP)); - } - - @Override - public void disabledInit() { - // AngledElevator.getInstance().setDesiredPosition(0); - AngledElevator.getInstance().moveToPosition(0); - } - - @Override - public void disabledPeriodic() {} - - @Override - public void testInit() {} - - @Override - public void testPeriodic() {} - - @Override - public void simulationInit() {} - - @Override - public void simulationPeriodic() {} } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 051c3c1..9638378 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -32,9 +32,9 @@ public static final class Drivetrain { // Pigeon ID public static final int PIGEON_ID = 1; - public static final double PIGEON_kP = 1.1; + public static final double PIGEON_kP = 0.067; - public static final double MIN_OUTPUT = 0.01; + public static final double MIN_OUTPUT = 0.05; public static final double MAX_ERROR_YAW = 0.5; public static final double OFFSET = 9.5; @@ -66,7 +66,7 @@ public static final class SwerveModule { // current limit constants for translation motors public static final double TRANS_PEAK = 30; - public static final double TRANS_CONTINUOUS = 60; + public static final double TRANS_CONTINUOUS = 55; public static final double TRANS_PEAK_DUR = 0.1; // current limit constants for rotation motors @@ -74,6 +74,9 @@ public static final class SwerveModule { public static final double ROT_CONTINUOUS = 40; public static final double ROT_PEAK_DUR = 0.1; + // velocity measurement window for translation motors + public static final int VELOCITY_WINDOW = 32; + // gear ratios public static final double TRANSLATION_GEAR_RATIO = 6.75; public static final double ROTATION_GEAR_RATIO = 12.8; @@ -97,15 +100,15 @@ public static final class SwerveModule { public static final double TRANSLATION_KA = 0.21522; // pid - public static double TRANSLATION_KP = 0.076; - public static double TRANSLATION_KI = 0.00; - public static double TRANSLATION_KD = 0.00; //0.9; - ; //0.7; + public static double TRANSLATION_KP = 0.076; + public static double TRANSLATION_KI = 0.00; + public static double TRANSLATION_KD = 0.00; //0.9; + // ; //0.7; } public static final class AlignPitch { // PID values for the pitch controller - public static final double kP = 0.014; // no bumpers - 0.0187, with - 0.017 + public static final double kP = 0.016; // no bumpers - 0.0187, with - 0.017 public static final double kI = 0.0; public static final double kD = 0; @@ -119,16 +122,17 @@ public static final class AlignPitch { public static final class SwerveManual { // Speed multipliers public static final double SPEED_MULTIPLIER = 1.0; - public static final double ROT_MULITPLIER = 0.25; + public static final double ROT_MULITPLIER = 0.9; public static final double CLAMP_MULTIPLIER = 0.7; - public static final double MAX_ACCELERATION = 35; - public static final double MAX_ACCELERATION_EXTENDED = 6.5; + public static final double MAX_ACCELERATION = 15; + public static final double MAX_ACCELERATION_EXTENDED = 6; } // CLAW public static final class Claw { - public static final int CLAW_FORWARD_ID = 0; - public static final int CLAW_REVERSE_ID = 1; + public static final int PH_ID = 1; + public static final int CLAW_FORWARD_ID = 7; + public static final int CLAW_REVERSE_ID = 6; } // ELEVATOR @@ -150,7 +154,7 @@ public static final class AngledElevator { public static final boolean FOLLOWER_INVERTED = false; // TODO // Thresholds for soft-limits - public static final double FORWARD_LIMIT = 40000; + public static final double FORWARD_LIMIT = 42500; public static final double REVERSE_LIMIT = 0; // Time for the motor to go from neutral to full @@ -158,7 +162,7 @@ public static final class AngledElevator { // POSITIONS: Low, Middle, High, Human Player (HP) public static double[] POSITIONS = { - 10018, 27000, 39500, 28500 + 10018, 27000, 40000, 29000 }; // HORIZONTAL Offsets: Middle, High, Human Player (HP) @@ -168,7 +172,7 @@ public static final class AngledElevator { } public static final class ZeroElevator { - public static double ZERO_SPEED = -0.25; + public static final double ZERO_SPEED = -0.37; } // AUTON CONSTANTS diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 94c9715..beeede1 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -44,6 +44,22 @@ public class Autons { () -> Rotation2d.fromDegrees(180)), new AlignPitch()); + public static final SequentialCommandGroup middleAndCrossMidCube = new SequentialCommandGroup( + new ZeroElevator(), + new CloseClaw(), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[1]), + new OpenClaw(), + new MoveToPosition(0) + .alongWith( + new SwervePositionController( + Trajectories.middleAndCross1, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180))), + new SwervePositionController( + Trajectories.middleAndCross2, + () -> Rotation2d.fromDegrees(180), + () -> Rotation2d.fromDegrees(180)), + new AlignPitch()); /** * bottomPath : Grab game piece from bot, drop off at node, and move back out of * community zone @@ -90,7 +106,7 @@ public class Autons { public static final SequentialCommandGroup noAuton = new SequentialCommandGroup( new ZeroElevator(), new CloseClaw(), - new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), + new MoveToPosition(RobotMap.AngledElevator.POSITIONS[2]), new OpenClaw(), new MoveToPosition(0)); } diff --git a/src/main/java/frc/robot/auton/Trajectories.java b/src/main/java/frc/robot/auton/Trajectories.java index 6c7ce1e..c61a4bb 100644 --- a/src/main/java/frc/robot/auton/Trajectories.java +++ b/src/main/java/frc/robot/auton/Trajectories.java @@ -69,7 +69,7 @@ public class Trajectories { new Pose2d(1.91, 2.75, Rotation2d.fromDegrees(180)), new Pose2d(6.5, 2.75, Rotation2d.fromDegrees(180))), 1.5, - 0.7, + 1.0, 0.0, 0.0, true); diff --git a/src/main/java/frc/robot/commands/claw/CloseClaw.java b/src/main/java/frc/robot/commands/claw/CloseClaw.java index e6fe93f..4b0cd3c 100644 --- a/src/main/java/frc/robot/commands/claw/CloseClaw.java +++ b/src/main/java/frc/robot/commands/claw/CloseClaw.java @@ -1,5 +1,6 @@ package frc.robot.commands.claw; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.Claw; @@ -10,7 +11,7 @@ public CloseClaw() { } public void initialize() { - Claw.getInstance().pinchClaw(); + Claw.getInstance().getClaw().set(DoubleSolenoid.Value.kForward); } } diff --git a/src/main/java/frc/robot/commands/claw/OpenClaw.java b/src/main/java/frc/robot/commands/claw/OpenClaw.java index 4d76e21..8a2497c 100644 --- a/src/main/java/frc/robot/commands/claw/OpenClaw.java +++ b/src/main/java/frc/robot/commands/claw/OpenClaw.java @@ -1,6 +1,7 @@ package frc.robot.commands.claw; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj.DoubleSolenoid; import frc.robot.subsystems.Claw; public class OpenClaw extends InstantCommand { @@ -10,7 +11,7 @@ public OpenClaw() { } public void initialize() { - Claw.getInstance().releaseClaw(); + Claw.getInstance().getClaw().set(DoubleSolenoid.Value.kReverse); } } diff --git a/src/main/java/frc/robot/commands/claw/ToggleClaw.java b/src/main/java/frc/robot/commands/claw/ToggleClaw.java index a73d52d..093e061 100644 --- a/src/main/java/frc/robot/commands/claw/ToggleClaw.java +++ b/src/main/java/frc/robot/commands/claw/ToggleClaw.java @@ -1,16 +1,20 @@ package frc.robot.commands.claw; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import frc.robot.OI; import frc.robot.subsystems.Claw; public class ToggleClaw extends InstantCommand{ - public ToggleClaw() { addRequirements(Claw.getInstance()); } - public void initialize() { - Claw.getInstance().toggleClaw(); + public void initialize() { + if (Claw.getInstance().getClaw().get() == DoubleSolenoid.Value.kForward) + Claw.getInstance().getClaw().set(DoubleSolenoid.Value.kReverse); + else + Claw.getInstance().getClaw().set(DoubleSolenoid.Value.kForward); } - } + diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index a5f68e0..6454977 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -34,6 +34,9 @@ public void execute() { MathUtil.mapJoystickOutput( OI.getInstance().getDriver().getRightX(), Constants.JOYSTICK_DEADBAND); + + omega = Drivetrain.getInstance().adjustPigeon(omega); + // Scaling velocities based on multipliers vx = scaleValues(vx, RobotMap.MAX_DRIVING_SPEED) * ((AngledElevator.getInstance().isFarExtended()) ? RobotMap.SwerveManual.CLAMP_MULTIPLIER : RobotMap.SwerveManual.SPEED_MULTIPLIER); vy = scaleValues(vy, RobotMap.MAX_DRIVING_SPEED) * ((AngledElevator.getInstance().isFarExtended()) ? RobotMap.SwerveManual.CLAMP_MULTIPLIER : RobotMap.SwerveManual.SPEED_MULTIPLIER); @@ -43,22 +46,21 @@ public void execute() { vy = limitAcceleration(vy, prevvy); vx = limitAcceleration(vx, prevvx); - // sets velocities to zero if robot is not visibly moving - if (isRobotStill()) { - vx = 0; - vy = 0; - } - - Drivetrain.getInstance().adjustPigeon(omega); - + // aligns to nearest target // if (OI.getInstance().getDriver().getRightBumperState()) { // omega = Drivetrain.getInstance().alignToTarget(omega); // } - // if rotational velocity is very small - if (Math.abs(omega) < RobotMap.Drivetrain.MIN_OUTPUT) { - omega = 0.0001; + // sets velocities to zero if robot is not visibly moving + if (isRobotStill()) { + vx = 0; + vy = 0; + + // if rotational velocity is very small + if (Math.abs(omega) < RobotMap.Drivetrain.MIN_OUTPUT) { + omega = 0.0001; + } } Drivetrain.getInstance() @@ -80,6 +82,7 @@ private double limitAcceleration(double value, double prevValue) { * Constants.ROBOT_LOOP; // previous velocity + direction of movement (+/-) * acceleration * time (a=v/t) } + return value; } diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index 639f1d6..c4acbd8 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -16,10 +16,10 @@ public void execute() { * else if DRIVER presses the Down DPad Button, Move the elevator down by 500 steps */ - if (OI.getInstance().getDriver().getUpDPadButtonState()) { + if (OI.getInstance().getOperator().getUpDPadButtonState()) { AngledElevator.getInstance().setDesiredPosition(AngledElevator.getInstance().getPosition() + 500); } - else if (OI.getInstance().getDriver().getDownDPadButtonState()) { + else if (OI.getInstance().getOperator().getDownDPadButtonState()) { AngledElevator.getInstance().setDesiredPosition(AngledElevator.getInstance().getPosition() - 500); } diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 32e1698..a7c18e5 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -13,33 +13,12 @@ public class Claw extends SubsystemBase { private DoubleSolenoid claw; private Claw() { - claw = new DoubleSolenoid(PneumaticsModuleType.REVPH, RobotMap.Claw.CLAW_FORWARD_ID, RobotMap.Claw.CLAW_REVERSE_ID); - // addChild("Claw", claw); + claw = new DoubleSolenoid(RobotMap.Claw.PH_ID, PneumaticsModuleType.REVPH, RobotMap.Claw.CLAW_FORWARD_ID, RobotMap.Claw.CLAW_REVERSE_ID); + addChild("Claw", claw); } - // Release Claw (open) - public void releaseClaw() { - claw.set(Value.kReverse); - } - - // Pinch claw (close) - public void pinchClaw() { - claw.set(Value.kForward); - } - - // Toggle Claw based on state - public void toggleClaw() { - if (claw.get() == DoubleSolenoid.Value.kForward) - releaseClaw(); - else - pinchClaw(); - } - - public boolean getState() { - if (claw.get() == DoubleSolenoid.Value.kForward) - return true; - - return false; + public DoubleSolenoid getClaw() { + return claw; } public static Claw getInstance() { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index d786527..dff16ad 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -6,6 +6,8 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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; @@ -21,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.RobotMap; -import frc.robot.util.CameraPoseEstimation; import frc.robot.util.SwerveModule; public class Drivetrain extends SubsystemBase { @@ -32,6 +33,8 @@ public class Drivetrain extends SubsystemBase { private Pigeon2 pigeon; private double prevHeading; + private Debouncer debouncer; + private SwerveDriveKinematics kinematics; // converts chassis speeds (x, y, theta) to module states (speed, angle) public ProfiledPIDController thetaController = new ProfiledPIDController(RobotMap.Drivetrain.THETA_P, RobotMap.Drivetrain.THETA_I, RobotMap.Drivetrain.THETA_D, new Constraints(4, 3.5)); @@ -83,6 +86,8 @@ private Drivetrain() { initalPoseMeters, stateStdDevs, visionStdDevs); + + debouncer = new Debouncer(0.4, DebounceType.kRising); } /* @@ -104,7 +109,7 @@ private void initPigeon() { * @return adjusted rotational speed */ public double adjustPigeon(double omega) { - if (Math.abs(omega) <= RobotMap.Drivetrain.MIN_OUTPUT) { + if (debouncer.calculate(Math.abs(omega) <= RobotMap.Drivetrain.MIN_OUTPUT)) { omega = -RobotMap.Drivetrain.PIGEON_kP * (prevHeading - getHeading()); } else { @@ -112,7 +117,7 @@ public double adjustPigeon(double omega) { } return omega; - } + } /* * Returns yaw of pigeon in degrees (heading of robot) @@ -279,23 +284,23 @@ public void periodic() { updatePose(); } -// @Override -// public void initSendable(SendableBuilder builder) { -// builder.setSmartDashboardType("Drivetrain"); -// builder.setActuator(true); -// builder.setSafeState(() -> setAngleAndDrive(new ChassisSpeeds())); -// builder.addDoubleProperty("Pitch Value", () -> getPitch(), null); -// builder.addDoubleProperty("Roll Value", () -> getRoll(), null); - -// for (int i = 0; i < 4; i++) { -// builder.addDoubleProperty( -// SwerveModule.swerveIDToName(i) + " Translation Speed", swerveModules[i]::getSpeed, null); -// builder.addDoubleProperty( -// SwerveModule.swerveIDToName(i) + " Translation Position", -// swerveModules[i]::getWheelPosition, -// null); -// builder.addDoubleProperty( -// SwerveModule.swerveIDToName(i) + " Rotation Angle", swerveModules[i]::getAngle, null); -// } -// } + // @Override + // public void initSendable(SendableBuilder builder) { + // builder.setSmartDashboardType("Drivetrain"); + // builder.setActuator(true); + // builder.setSafeState(() -> setAngleAndDrive(new ChassisSpeeds())); + // builder.addDoubleProperty("Pitch Value", () -> getPitch(), null); + // builder.addDoubleProperty("Roll Value", () -> getRoll(), null); + + // for (int i = 0; i < 4; i++) { + // builder.addDoubleProperty( + // SwerveModule.swerveIDToName(i) + " Translation Speed", swerveModules[i]::getSpeed, null); + // builder.addDoubleProperty( + // SwerveModule.swerveIDToName(i) + " Translation Position", + // swerveModules[i]::getWheelPosition, + // null); + // builder.addDoubleProperty( + // SwerveModule.swerveIDToName(i) + " Rotation Angle", swerveModules[i]::getAngle, null); + // } + // } } diff --git a/src/main/java/frc/robot/util/CameraPoseEstimation.java b/src/main/java/frc/robot/util/CameraPoseEstimation.java index 5b1f582..2fd729f 100644 --- a/src/main/java/frc/robot/util/CameraPoseEstimation.java +++ b/src/main/java/frc/robot/util/CameraPoseEstimation.java @@ -91,6 +91,7 @@ public CameraPoseEstimation() { aprilTagFieldLayout = new AprilTagFieldLayout(CameraPoseEstimation.APRIL_TAGS, 16.4846, 8.1026); // define the camera name as "limelight" camera = new PhotonCamera("limelight"); + // define loocation of the camera based on the robot robotToCamera = new Transform3d( new Translation3d(0, Units.inchesToMeters(10.81259), 0), new Rotation3d(0, 0, 0)); diff --git a/src/main/java/frc/robot/util/SwerveModule.java b/src/main/java/frc/robot/util/SwerveModule.java index 71f4a1b..76076e0 100644 --- a/src/main/java/frc/robot/util/SwerveModule.java +++ b/src/main/java/frc/robot/util/SwerveModule.java @@ -1,7 +1,5 @@ package frc.robot.util; -import java.net.NetworkInterface; - import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -18,6 +16,7 @@ import harkerrobolib.util.Constants; import harkerrobolib.util.HSFalconBuilder; + public class SwerveModule { //motors on the swerve modules private TalonFX translation; @@ -69,7 +68,7 @@ private void init() { translation.config_kI(Constants.SLOT_INDEX, RobotMap.SwerveModule.TRANSLATION_KI); translation.config_kD(Constants.SLOT_INDEX, RobotMap.SwerveModule.TRANSLATION_KD); translation.enableVoltageCompensation(true); //disables voltage compensation - translation.configVelocityMeasurementWindow(32); //number of samples measured + translation.configVelocityMeasurementWindow(RobotMap.SwerveModule.VELOCITY_WINDOW); //number of samples measured translation.configVelocityMeasurementPeriod(SensorVelocityMeasPeriod.Period_10Ms); canCoder.configFactoryDefault(); canCoder.clearStickyFaults(); diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 47659de..8c761ae 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -13,16 +13,27 @@ import frc.robot.subsystems.Drivetrain; public class Telemetry { - NetworkTable table; + NetworkTable table; NetworkTableInstance inst; - public Telemetry(){ + + NetworkTable swervemodule; + NetworkTable drivetrain; + NetworkTable elevator; + NetworkTable alignpitch; + NetworkTable swervepositioncontroller; + + public Telemetry() { inst = NetworkTableInstance.getDefault(); - table = inst.getTable("datatable"); - } + table = inst.getTable("Main use This!!"); - public void putSwerveModule(){ - NetworkTable swervemodule = table.getSubTable("Swerve Module"); + swervemodule = table.getSubTable("Swerve Module"); + drivetrain = table.getSubTable("Drivetrain"); + elevator = table.getSubTable("Elevator"); + alignpitch = table.getSubTable("Align Pitch"); + swervepositioncontroller = table.getSubTable("SwervePositionController"); + } + public void putSwerveModule() { NetworkTableEntry rotationkP = swervemodule.getEntry("Rotation kP"); rotationkP.setDouble(RobotMap.SwerveModule.ROTATION_KP); @@ -44,14 +55,13 @@ public void putSwerveModule(){ NetworkTableEntry translationkA = swervemodule.getEntry("Translation kA"); translationkA.setDouble(RobotMap.SwerveModule.TRANSLATION_KA); } - public void putDrivetrain(){ - NetworkTable drivetrain = table.getSubTable("Drivetrain"); + + public void putDrivetrain() { NetworkTableEntry kP = drivetrain.getEntry("Pigeon kP"); kP.setDouble(RobotMap.Drivetrain.PIGEON_kP); } - public void putElevator(){ - NetworkTable elevator = table.getSubTable("Elevator"); + public void putElevator() { NetworkTableEntry kP = elevator.getEntry("kP"); kP.setDouble(RobotMap.AngledElevator.kP); @@ -59,9 +69,7 @@ public void putElevator(){ kG.setDouble(RobotMap.AngledElevator.kG); } - public void putAlignPitch(){ - NetworkTable alignpitch = table.getSubTable("Align Pitch"); - + public void putAlignPitch() { NetworkTableEntry kP = alignpitch.getEntry("kP"); kP.setDouble(RobotMap.AlignPitch.kP); @@ -72,9 +80,7 @@ public void putAlignPitch(){ kD.setDouble(RobotMap.AlignPitch.kD); } - public void putSwervePositionController(){ - NetworkTable swervepositioncontroller = table.getSubTable("SwervePositionController"); - + public void putSwervePositionController() { NetworkTableEntry X_kP = swervepositioncontroller.getEntry("X_kP"); X_kP.setDouble(RobotMap.SwervePositionController.X_kP);