From 68fb231d470c21ea4c9b1939c2d6c703b8c25d3b Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 23 Sep 2023 14:08:06 -0700 Subject: [PATCH 1/7] Fixed Pigeon kP, set driver mode for limelight --- src/main/java/frc/robot/Robot.java | 6 +++++- src/main/java/frc/robot/RobotMap.java | 4 ++-- src/main/java/frc/robot/util/CameraPoseEstimation.java | 1 + 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3713951..6c6247f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +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; @@ -21,6 +23,7 @@ import frc.robot.subsystems.AngledElevator; import frc.robot.subsystems.Claw; import frc.robot.subsystems.Drivetrain; +import frc.robot.util.CameraPoseEstimation; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -41,7 +44,7 @@ public void robotInit() { LiveWindow.setEnabled(true); LiveWindow.enableAllTelemetry(); SmartDashboard.putData(RobotMap.Field.FIELD); - SmartDashboard.putBoolean("red", Trajectories.isFlipped()); + // SmartDashboard.putBoolean("red", Trajectories.isFlipped()); CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); CommandScheduler.getInstance() .setDefaultCommand(AngledElevator.getInstance(), new ElevatorManual()); @@ -52,6 +55,7 @@ public void robotInit() { autonChooser.addOption("Top Path", "Top Path"); autonChooser.addOption("No auton", "No auton"); SmartDashboard.putData("Auton Chooser", autonChooser); + CameraPoseEstimation.getInstance().getCamera().setDriverMode(true); } @Override diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index fb6a690..69c1005 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -36,7 +36,7 @@ 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.62; public static final double MIN_OUTPUT = 0.01; @@ -125,7 +125,7 @@ public static final class SwerveManual { public static final double SPEED_MULTIPLIER = 1.0; public static final double ROT_MULITPLIER = 0.25; public static final double CLAMP_MULTIPLIER = 0.7; - public static final double MAX_ACCELERATION = 35; + public static final double MAX_ACCELERATION = 15; public static final double MAX_ACCELERATION_EXTENDED = 6.5; } 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)); From 918573ed2d28bc48ba5481af9d57c154d852afc3 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 23 Sep 2023 17:27:58 -0700 Subject: [PATCH 2/7] operator controls --- src/main/java/frc/robot/OI.java | 25 +++++++++------ src/main/java/frc/robot/RobotMap.java | 6 ++-- src/main/java/frc/robot/util/Telemetry.java | 34 +++++++++++++++++++++ 3 files changed, 52 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/util/Telemetry.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 69572af..1b61026 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -39,30 +39,35 @@ 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()); + // driver.getButtonSelect().onTrue(new ZeroElevator()); - // set X button on OPERATOR controller to Zero Elevator - operator.getButtonX().onTrue(new ZeroElevator()); + 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/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 69c1005..f1f421f 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -122,11 +122,11 @@ public static final class AlignPitch { public static final class SwerveManual { // Speed multipliers - public static final double SPEED_MULTIPLIER = 1.0; + public static final double SPEED_MULTIPLIER = 0.5; public static final double ROT_MULITPLIER = 0.25; public static final double CLAMP_MULTIPLIER = 0.7; - public static final double MAX_ACCELERATION = 15; - public static final double MAX_ACCELERATION_EXTENDED = 6.5; + public static final double MAX_ACCELERATION = 13; + public static final double MAX_ACCELERATION_EXTENDED = 6; } // CLAW diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java new file mode 100644 index 0000000..d327364 --- /dev/null +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -0,0 +1,34 @@ +// package frc.robot.util; + +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableEntry; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.util.sendable.Sendable; +// import edu.wpi.first.util.sendable.SendableBuilder; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import frc.robot.RobotMap; +// import frc.robot.subsystems.Drivetrain; + +// public final class Telemetry { + +// NetworkTableInstance inst = NetworkTableInstance.getDefault(); +// NetworkTable datatable = inst.getTable("SmartDashboard"); + +// public Telemetry() { +// inst.startDSClient(); +// } + +// private static Telemetry instance; + + +// public void DrivetrainSendableBuilder(SendableBuilder builder){ +// builder.addDoubleProperty("Translation kP", D, null); +// } + +// // public static Telemetry getInstance() { +// // if (instance == null) { +// // instance = new Telemetry(); +// // } +// // return instance; +// // } +// } \ No newline at end of file From 821681f92c8e85ae9022fb65606649c788b973fb Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 30 Sep 2023 17:58:25 -0700 Subject: [PATCH 3/7] 9/30 driver training --- .Glass/glass.json | 40 +++ src/main/java/frc/robot/OI.java | 1 - src/main/java/frc/robot/Robot.java | 257 ++++++++++-------- src/main/java/frc/robot/RobotMap.java | 17 +- src/main/java/frc/robot/auton/Autons.java | 2 +- .../frc/robot/commands/claw/ToggleClaw.java | 4 +- .../commands/drivetrain/SwerveManual.java | 25 +- .../commands/elevator/ElevatorManual.java | 4 +- .../java/frc/robot/subsystems/Drivetrain.java | 49 ++-- .../java/frc/robot/util/SwerveModule.java | 8 +- 10 files changed, 244 insertions(+), 163 deletions(-) create mode 100644 .Glass/glass.json diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..b42754f --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,40 @@ +{ + "NetworkTables": { + "Persistent Values": { + "open": false + }, + "Retained Values": { + "open": false + }, + "Transitory Values": { + "open": false + }, + "retained": { + "SmartDashboard": { + "Auton Chooser": { + "open": true + }, + "open": true + } + }, + "transitory": { + "SmartDashboard": { + "Auton Chooser": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", + "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/SmartDashboard/Auton Chooser": "String Chooser" + } + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "1072" + } +} diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 1b61026..9c176ae 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -59,7 +59,6 @@ private void initBindings() { // set Select Button to Zero Elevator // driver.getButtonSelect().onTrue(new ZeroElevator()); - operator.getRightDPadButton().onTrue(new ToggleClaw()); operator.getRightBumper().onTrue(new ZeroElevator()); // set A button on OPERATOR controller to set Elevator to POSITION LOW diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6c6247f..d600c5f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; 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; @@ -18,6 +19,7 @@ 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; @@ -26,129 +28,162 @@ import frc.robot.util.CameraPoseEstimation; /** - * 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("red", 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); - CameraPoseEstimation.getInstance().getCamera().setDriverMode(true); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - RobotMap.Field.FIELD.setRobotPose(Drivetrain.getInstance().getPoseEstimatorPose2d()); - SmartDashboard.putString("Auton running:", autonChooser.getSelected()); - SmartDashboard.putData(Drivetrain.getInstance()); - SmartDashboard.putData(Claw.getInstance()); - SmartDashboard.putData(AngledElevator.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; + + /** + * 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("red", Trajectories.isFlipped()); + CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); + CommandScheduler.getInstance() + .setDefaultCommand(AngledElevator.getInstance(), new ElevatorManual()); + CommandScheduler.getInstance().setDefaultCommand(Claw.getInstance(), new ToggleClaw()); + 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); + CameraPoseEstimation.getInstance().getCamera().setDriverMode(true); + + // rio voltage and current + + powerDistribution = new PowerDistribution(); } - } - @Override - public void autonomousPeriodic() {} + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + RobotMap.Field.FIELD.setRobotPose(Drivetrain.getInstance().getPoseEstimatorPose2d()); + + SmartDashboard.putString("Current Auton:", autonChooser.getSelected()); + + // double pdhVoltage = powerDistribution.getVoltage(); + // double pdhCurrent = powerDistribution.getCurrent(20); + // SmartDashboard.putNumber("pdh voltage", pdhVoltage); + // SmartDashboard.putNumber("rio current", pdhCurrent); - @Override - public void teleopInit() { - Autons.bottomPath.cancel(); - Autons.topPath.cancel(); - Autons.middlePath.cancel(); - Autons.middleAndCross.cancel(); - Autons.noAuton.cancel(); - Drivetrain.getInstance().setYaw(180); - } + // SmartDashboard.putNumber("kP",); + // SmartDashboard.putNumber("kI", + // Drivetrain.getInstance().thetaController.getI()); + // SmartDashboard.putNumber("kD", + // Drivetrain.getInstance().thetaController.getD()); - @Override - public void teleopPeriodic() { + // 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; + } + } - @Override - public void disabledInit() { - // AngledElevator.getInstance().setDesiredPosition(0); - AngledElevator.getInstance().moveToPosition(0); - } + @Override + public void autonomousPeriodic() { + } - @Override - public void disabledPeriodic() {} + @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 testInit() {} + @Override + public void teleopPeriodic() { - @Override - public void testPeriodic() {} + } - @Override - public void simulationInit() {} + @Override + public void disabledInit() { + // AngledElevator.getInstance().setDesiredPosition(0); + AngledElevator.getInstance().moveToPosition(0); + } - @Override - public void simulationPeriodic() {} + @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 f1f421f..95e23a2 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -36,9 +36,9 @@ public static final class Drivetrain { // Pigeon ID public static final int PIGEON_ID = 1; - public static final double PIGEON_kP = 0.62; + 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; @@ -70,7 +70,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 @@ -78,6 +78,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; @@ -104,7 +107,7 @@ public static final class SwerveModule { public static final double TRANSLATION_KP = 0.076; public static final double TRANSLATION_KI = 0.00; public static final double TRANSLATION_KD = 0.00; //0.9; - ; //0.7; + // ; //0.7; } public static final class AlignPitch { @@ -122,10 +125,10 @@ public static final class AlignPitch { public static final class SwerveManual { // Speed multipliers - public static final double SPEED_MULTIPLIER = 0.5; - public static final double ROT_MULITPLIER = 0.25; + public static final double SPEED_MULTIPLIER = 0.9; + public static final double ROT_MULITPLIER = 0.9; public static final double CLAMP_MULTIPLIER = 0.7; - public static final double MAX_ACCELERATION = 13; + public static final double MAX_ACCELERATION = 15; public static final double MAX_ACCELERATION_EXTENDED = 6; } diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 94c9715..0cdd913 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -90,7 +90,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/commands/claw/ToggleClaw.java b/src/main/java/frc/robot/commands/claw/ToggleClaw.java index a73d52d..2a544ae 100644 --- a/src/main/java/frc/robot/commands/claw/ToggleClaw.java +++ b/src/main/java/frc/robot/commands/claw/ToggleClaw.java @@ -1,15 +1,15 @@ package frc.robot.commands.claw; import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.OI; import frc.robot.subsystems.Claw; public class ToggleClaw extends InstantCommand{ - public ToggleClaw() { addRequirements(Claw.getInstance()); } - public void initialize() { + public void initialize() { Claw.getInstance().toggleClaw(); } 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/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f4281bf..3d41314 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; @@ -20,7 +22,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; -import frc.robot.util.CameraPoseEstimation; import frc.robot.util.SwerveModule; public class Drivetrain extends SubsystemBase { @@ -31,6 +32,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) private static ProfiledPIDController thetaController = new ProfiledPIDController(RobotMap.Drivetrain.THETA_P, RobotMap.Drivetrain.THETA_I, RobotMap.Drivetrain.THETA_D, new Constraints(4, 3.5)); @@ -74,6 +77,8 @@ private Drivetrain() { initalPoseMeters, stateStdDevs, visionStdDevs); + + debouncer = new Debouncer(0.4, DebounceType.kRising); } /* @@ -95,7 +100,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 { @@ -103,7 +108,7 @@ public double adjustPigeon(double omega) { } return omega; - } + } /* * Returns yaw of pigeon in degrees (heading of robot) @@ -242,23 +247,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/SwerveModule.java b/src/main/java/frc/robot/util/SwerveModule.java index 102af78..c032a46 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.beans.FeatureDescriptor; - import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -13,13 +11,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Robot; import frc.robot.RobotMap; import harkerrobolib.util.Constants; import harkerrobolib.util.HSFalconBuilder; -import harkerrobolib.wrappers.HSFalcon; + public class SwerveModule { //motors on the swerve modules @@ -71,7 +67,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(); From 7e750535c6a86341b63e1b397ede55573731c19f Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Wed, 4 Oct 2023 15:37:48 -0700 Subject: [PATCH 4/7] fixed claw --- src/main/java/frc/robot/OI.java | 3 ++- src/main/java/frc/robot/Robot.java | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 9c176ae..9a42c4d 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; @@ -59,7 +60,7 @@ private void initBindings() { // set Select Button to Zero Elevator // driver.getButtonSelect().onTrue(new ZeroElevator()); - operator.getRightDPadButton().onTrue(new ToggleClaw()); + operator.getRightDPadButton().debounce(0.1, DebounceType.kRising).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])); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d600c5f..d0db24b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -55,7 +55,6 @@ public void robotInit() { CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); CommandScheduler.getInstance() .setDefaultCommand(AngledElevator.getInstance(), new ElevatorManual()); - CommandScheduler.getInstance().setDefaultCommand(Claw.getInstance(), new ToggleClaw()); autonChooser = new SendableChooser(); autonChooser.setDefaultOption("Middle And Cross Path", "Middle And Cross Path"); autonChooser.addOption("Middle Path", "Middle Path"); From 8f2c7cd7c1e406015ca39654f9f148cad3a26f28 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Wed, 4 Oct 2023 17:24:37 -0700 Subject: [PATCH 5/7] Driver pratice tuning --- src/main/java/frc/robot/RobotMap.java | 2 +- .../frc/robot/commands/claw/CloseClaw.java | 3 ++- .../java/frc/robot/commands/claw/OpenClaw.java | 3 ++- .../frc/robot/commands/claw/ToggleClaw.java | 8 +++++++- src/main/java/frc/robot/subsystems/Claw.java | 18 ++---------------- 5 files changed, 14 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 95e23a2..4988d0c 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -175,7 +175,7 @@ public static final class AngledElevator { } public static final class ZeroElevator { - public static final double ZERO_SPEED = -0.45; + public static final double ZERO_SPEED = -0.37; } // AUTON CONSTANTS 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 2a544ae..e66a000 100644 --- a/src/main/java/frc/robot/commands/claw/ToggleClaw.java +++ b/src/main/java/frc/robot/commands/claw/ToggleClaw.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.OI; import frc.robot.subsystems.Claw; @@ -10,7 +11,12 @@ public ToggleClaw() { } public void initialize() { - Claw.getInstance().toggleClaw(); + 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/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 2cec45f..08bacba 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -16,22 +16,8 @@ private Claw() { 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 DoubleSolenoid getClaw() { + return claw; } public static Claw getInstance() { From c38ce1a516eb0ce1196e77454f22f5226b90e345 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Wed, 4 Oct 2023 17:48:50 -0700 Subject: [PATCH 6/7] Increase debouncer time to 0.13 --- src/main/java/frc/robot/OI.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 9a42c4d..cd9da4a 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -60,7 +60,7 @@ private void initBindings() { // set Select Button to Zero Elevator // driver.getButtonSelect().onTrue(new ZeroElevator()); - operator.getRightDPadButton().debounce(0.1, DebounceType.kRising).onTrue(new ToggleClaw()); + operator.getRightDPadButton().debounce(0.13, DebounceType.kRising).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])); From a65422f39c5084e23bb0a0ed83b707d771e40ee9 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 14 Oct 2023 13:22:17 -0700 Subject: [PATCH 7/7] CalGames Improvements [Added new Auton Path, tuned values; added channel for REVPH for pneumatics] --- src/main/java/frc/robot/OI.java | 7 +++---- src/main/java/frc/robot/Robot.java | 6 ++++++ src/main/java/frc/robot/RobotMap.java | 13 +++++++------ src/main/java/frc/robot/auton/Autons.java | 16 ++++++++++++++++ src/main/java/frc/robot/auton/Trajectories.java | 2 +- .../java/frc/robot/commands/claw/ToggleClaw.java | 4 +--- src/main/java/frc/robot/subsystems/Claw.java | 2 +- 7 files changed, 35 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index cd9da4a..f740bae 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -60,13 +60,12 @@ private void initBindings() { // set Select Button to Zero Elevator // driver.getButtonSelect().onTrue(new ZeroElevator()); - operator.getRightDPadButton().debounce(0.13, DebounceType.kRising).onTrue(new ToggleClaw()); + //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.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 d0db24b..4d04bb9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,6 +58,7 @@ public void robotInit() { autonChooser = new SendableChooser(); autonChooser.setDefaultOption("Middle And Cross Path", "Middle And Cross Path"); autonChooser.addOption("Middle Path", "Middle Path"); + autonChooser.addOption("Middle and Cross (Mid Cube)", "Middle And Cross Path with Middle Cube"); autonChooser.addOption("Bottom Path", "Bottom Path"); autonChooser.addOption("Top Path", "Top Path"); autonChooser.addOption("No auton", "No auton"); @@ -128,6 +129,11 @@ public void autonomousInit() { .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)))); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 4988d0c..0ee2683 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -112,7 +112,7 @@ public static final class SwerveModule { 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; @@ -125,7 +125,7 @@ public static final class AlignPitch { public static final class SwerveManual { // Speed multipliers - public static final double SPEED_MULTIPLIER = 0.9; + public static final double SPEED_MULTIPLIER = 1.0; public static final double ROT_MULITPLIER = 0.9; public static final double CLAMP_MULTIPLIER = 0.7; public static final double MAX_ACCELERATION = 15; @@ -134,8 +134,9 @@ public static final class SwerveManual { // 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 @@ -157,7 +158,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 @@ -165,7 +166,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) diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 0cdd913..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 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/ToggleClaw.java b/src/main/java/frc/robot/commands/claw/ToggleClaw.java index e66a000..093e061 100644 --- a/src/main/java/frc/robot/commands/claw/ToggleClaw.java +++ b/src/main/java/frc/robot/commands/claw/ToggleClaw.java @@ -16,7 +16,5 @@ public void initialize() { else Claw.getInstance().getClaw().set(DoubleSolenoid.Value.kForward); } - - - } + diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 08bacba..90ac1e1 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -12,7 +12,7 @@ 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); + claw = new DoubleSolenoid(RobotMap.Claw.PH_ID, PneumaticsModuleType.REVPH, RobotMap.Claw.CLAW_FORWARD_ID, RobotMap.Claw.CLAW_REVERSE_ID); addChild("Claw", claw); }