diff --git a/src/main/deploy/pathplanner/autos/Bottom Blue 2 Algae.auto b/src/main/deploy/pathplanner/autos/Bottom Blue 2 Algae.auto deleted file mode 100644 index 13a9934..0000000 --- a/src/main/deploy/pathplanner/autos/Bottom Blue 2 Algae.auto +++ /dev/null @@ -1,113 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BottomBlueIntake(1)" - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BottomBlueShoot(2)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BottomBlueIntake(4)" - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BottomBlueShoot(5)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - } - ] - } - }, - "resetOdom": true, - "folder": "BottomBlueShoot", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleBlue.auto b/src/main/deploy/pathplanner/autos/MiddleBlue.auto deleted file mode 100644 index 88f5151..0000000 --- a/src/main/deploy/pathplanner/autos/MiddleBlue.auto +++ /dev/null @@ -1,113 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "PickupMiddleBlue(1)" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleBlueShoot(2)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleBluePickup(3)" - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleBlueShoot(4)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - } - ] - } - }, - "resetOdom": true, - "folder": "MiddleBlue", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Spinny Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Spinny Auto.auto diff --git a/src/main/deploy/pathplanner/autos/TopBlueThreeAlgae.auto b/src/main/deploy/pathplanner/autos/TopBlueThreeAlgae.auto deleted file mode 100644 index 8cfef3e..0000000 --- a/src/main/deploy/pathplanner/autos/TopBlueThreeAlgae.auto +++ /dev/null @@ -1,163 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TopBlue-GrabBall1(1)" - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TopBlueShoot(2)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TopBlueGrabBall2(3)" - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TopBlueShoot1(4)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TopBlueIntake(5)" - } - }, - { - "type": "named", - "data": { - "name": "ReefIntake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TopBlueSHoot(6)" - } - }, - { - "type": "named", - "data": { - "name": "Rev" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - } - ] - } - }, - "resetOdom": true, - "folder": "TopBlue", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/lollipop bottom.auto b/src/main/deploy/pathplanner/autos/lollipop bottom.auto index db762fa..72961d5 100644 --- a/src/main/deploy/pathplanner/autos/lollipop bottom.auto +++ b/src/main/deploy/pathplanner/autos/lollipop bottom.auto @@ -96,6 +96,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Lolipop", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/lollipop.auto b/src/main/deploy/pathplanner/autos/lollipop.auto index e6b2ff1..c33103d 100644 --- a/src/main/deploy/pathplanner/autos/lollipop.auto +++ b/src/main/deploy/pathplanner/autos/lollipop.auto @@ -140,6 +140,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Lolipop", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/lollipop2.auto b/src/main/deploy/pathplanner/autos/lollipop2.auto index ec30427..5d06e88 100644 --- a/src/main/deploy/pathplanner/autos/lollipop2.auto +++ b/src/main/deploy/pathplanner/autos/lollipop2.auto @@ -96,6 +96,6 @@ } }, "resetOdom": true, - "folder": null, + "folder": "Lolipop", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BottomBlueIntake(4).path b/src/main/deploy/pathplanner/paths/BottomBlueIntake(4).path index dae7841..11a4db9 100644 --- a/src/main/deploy/pathplanner/paths/BottomBlueIntake(4).path +++ b/src/main/deploy/pathplanner/paths/BottomBlueIntake(4).path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 7.065474845328297, + "x": 7.065474845328298, "y": 2.7842725409836064 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/MiddleBluePickup(3).path b/src/main/deploy/pathplanner/paths/MiddleBluePickup(3).path index 8f68c79..a406088 100644 --- a/src/main/deploy/pathplanner/paths/MiddleBluePickup(3).path +++ b/src/main/deploy/pathplanner/paths/MiddleBluePickup(3).path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 7.0617585145555415, + "x": 7.061758514555541, "y": 2.806505668164609 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Spinny Path.path b/src/main/deploy/pathplanner/paths/Spinny Path.path new file mode 100644 index 0000000..853589a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Spinny Path.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 4.25, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.75, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/lolipop bottom end.path b/src/main/deploy/pathplanner/paths/lolipop bottom end.path new file mode 100644 index 0000000..fc8052c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/lolipop bottom end.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.62920081967213, + "y": 2.7842725409836064 + }, + "prevControl": null, + "nextControl": { + "x": 6.697687318356997, + "y": 3.0247088100818598 + }, + "isLocked": false, + "linkedName": "MiddleShoot" + }, + { + "anchor": { + "x": 6.62920081967213, + "y": 1.0902499999999997 + }, + "prevControl": { + "x": 6.440208753712055, + "y": 0.9265979330893049 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "lollipop bottom", + "idealStartingState": { + "velocity": 0, + "rotation": 61.26020470831185 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/lollipop bottom.path b/src/main/deploy/pathplanner/paths/lollipop bottom.path index 25957d6..7102667 100644 --- a/src/main/deploy/pathplanner/paths/lollipop bottom.path +++ b/src/main/deploy/pathplanner/paths/lollipop bottom.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.167, - "y": 2.1848872950819667 + "x": 7.12725, + "y": 0.46625 }, "prevControl": null, "nextControl": { - "x": 3.9700416238315688, - "y": 2.1695739287523126 + "x": 4.120125349973398, + "y": 1.3448517809128675 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.1848872950819667 }, "prevControl": { - "x": 3.5602715891046266, - "y": 2.197389996695135 + "x": 3.699169192096951, + "y": 1.4610852449617846 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index d99b160..56757b9 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -12,11 +12,9 @@ "lollipop top" ], "autoFolders": [ - "BottomBlueShoot", - "ExamplesForLearning", "JustDrive", - "MiddleBlue", - "TopBlue" + "Lolipop", + "New Folder" ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7204ff8..614fccf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,19 +59,14 @@ public static final class Ports { public static final int backRightAbsolute = 20; //done //Shooter CAN ids - + public static final int shooterMotor = 14; //done //Intake CAN id - public static final int shooterMotor = 14; //done public static final int intakeMotor = 7; //done public static final int pivotMotor = 5; //done public static final int indexerMotor = 6; //done - public static final int reefIntakeMotor = 22; - public static final int reefPivotMotor = 25; - public static final int climberMotor = 23; - public static final int intakeHexPort = 1; - public static final int reefHexPort = 2; + public static final int intakeHexPort = 3; //Done public static final Port arduino = SerialPort.Port.kUSB1; @@ -89,11 +84,11 @@ public static final class DriveConstants { public static class IntakeConstants{ public static final double encoderOffset = 0; - public static final double rest = 0.1776698044417451; - public static final double grab = 0.06343960158599005; - public static final double restball = 0.14493320312333007; + public static final double rest = 0.6859650171491255; //0.1776698044417451; + public static final double grab = 0.5717348142933704; //0.06343960158599005; + public static final double restball = 0.6672284158307104; //0.14493320312333007; //TODO: Tune values - public static double kP = 20.0; + public static double kP = 15.0; public static double kI = 0; public static double kD = 0; @@ -101,50 +96,13 @@ public static class IntakeConstants{ public static double kV = 3.5; public static double kA = 0; - public static double kPdown = 20.0; + public static double kPdown = 15.0; public static double kIdown = 0; public static double kDdown = 0; public double shootBallPivotPos = 0; } - public static class ReefIntakeConstants{ -// tune all of these this is just temporary - public static final double encoderOffset = 0.6457992161449804 - 0.25; - public static final double reefRest = 0.09504339605108491; - public static final double reefGrab = 0.38473420336835507; - - public static double kP = 0.1; - public static double kI = 0; - public static double kD = 0; - - public static double kS = 0.01; - public static double kV = 1.42;//adjust this if arm to high increase - public static double kG = 0.03; - - public static double kPdown = 20.0; - public static double kIdown = 0; - public static double kDdown = 0; - } - - public static class ClimberConstants{ - public static final double encoderOffset = 0; - public static final double rest = 0.1776698044417451; - public static final double takeOff = 0.06343960158599005; - - public static double kP = 20.0; - public static double kI = 0; - public static double kD = 0; - - public static double kS = 0; - public static double kV = 3.5; - public static double kA = 0; - - public static double kPdown =20.0; - public static double kIdown = 0; - public static double kDdown = 0; - } - public static class ShooterConstants{ public static double kP = 0.09; //1.01; @@ -162,6 +120,12 @@ public static class Vision{ public static double dif1 = 0; } + + public static class DrivetrainOverrides{ + public static double turnDif = 0; + public static double moveDif = 0; + public static boolean overrideMode = false; + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 586b66c..17aed59 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,6 +41,7 @@ */ public class Robot extends TimedRobot { private Command m_autonomousCommand; + private Command m_testCommand; private final RobotContainer m_robotContainer; private final PowerDistribution pdh; @@ -112,7 +113,9 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + LED.intialize(); + } @Override public void disabledPeriodic() { @@ -188,7 +191,7 @@ public void teleopInit() { public void teleopPeriodic() { if (m_robotContainer.topLeftButton.getAsBoolean()) m_robotContainer.setFastMode(); - else if (m_robotContainer.bottomLeftButton.getAsBoolean()) m_robotContainer.setOverrideMode(); + //else if (m_robotContainer.bottomLeftButton.getAsBo olean()) m_robotContainer.setOverrideMode(); else m_robotContainer.setMediumMode(); // if (m_robotContainer.bottomRightButton.getAsBoolean()) m_robotContainer.setOverrideMode(); // @@ -198,6 +201,12 @@ public void teleopPeriodic() { public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + m_testCommand = m_robotContainer.getSelfTestCommand(); + + // schedule the autonomous command (example) + if (m_testCommand != null) { + m_testCommand.schedule(); + } } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07e08a4..607b9ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,11 +6,11 @@ import frc.robot.Constants.Ports; import frc.robot.commands.IntakeBall; +import frc.robot.commands.LimelightShoot; import frc.robot.commands.LollipopIntakeBall; import frc.robot.commands.ProcessorScoring; -import frc.robot.commands.ReefIntakeBall; -import frc.robot.commands.ReefKnockOff; import frc.robot.commands.Rev; +import frc.robot.commands.SelfTest; import frc.robot.commands.ShootBall; import frc.robot.commands.Unshoot; import frc.robot.generated.TunerConstants; @@ -18,7 +18,6 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Limelight; -import frc.robot.subsystems.ReefIntake; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -51,19 +50,19 @@ public class RobotContainer { //Subsystems public final Shooter shooter = new Shooter(); public final Intake intake = new Intake(); - public final ReefIntake reefIntake = new ReefIntake(); public final CommandSwerveDrivetrain drivetrain; //Commands add commnets public SendableChooser autoChooser; private double speed = 0.5; private double dif = 0; + private double turndif = 0; public double shooterSpeedTest = 0; //Controller configurations // Replace with CommandPS4Controller or CommandJoystick if needed //private final CommandXboxController ps4 = new CommandXboxController(0); - private final CommandXboxController m_driverController = new CommandXboxController(Ports.controller); + public final CommandXboxController m_driverController = new CommandXboxController(Ports.controller); private final Joystick leftStick = new Joystick(Constants.Ports.leftStick); public final JoystickButton topLeftButton = new JoystickButton(leftStick, 1); public final JoystickButton bottomLeftButton = new JoystickButton(leftStick, 2); @@ -98,7 +97,6 @@ public RobotContainer() { NamedCommands.registerCommand("Intake", new IntakeBall(intake)); NamedCommands.registerCommand("Processor", new ProcessorScoring(intake)); NamedCommands.registerCommand("UpIntake", new LollipopIntakeBall(intake)); - NamedCommands.registerCommand("ReefIntake", new ReefIntakeBall(reefIntake, shooter, intake)); drivetrain = TunerConstants.createDrivetrain(); autoChooser = AutoBuilder.buildAutoChooser(); @@ -114,7 +112,7 @@ public RobotContainer() { drivetrain.applyRequest(() -> drive.withVelocityX(-leftStick.getY() * MaxSpeed * speed + (dif * MaxSpeed)) // Drive forward with negative Y (forward) .withVelocityY(-leftStick.getX() * MaxSpeed * speed) // Drive left with negative X (left) - .withRotationalRate(rightStick.getTwist() * MaxAngularRate * speed * 2) // Drive counterclockwise with negative X (left) + .withRotationalRate(rightStick.getTwist() * MaxAngularRate * speed * 2 + (turndif * MaxAngularRate)) // Drive counterclockwise with negative X (left) ) // drive.withVelocityX(-m_driverController.getLeftY() * MaxSpeed * 0.5) // Drive forward with negative Y (forward) @@ -125,9 +123,11 @@ public RobotContainer() { //Reset orientation topRightButton.onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + // m_driverController.x().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); //Defense mode bottomRightButton.whileTrue(drivetrain.applyRequest(() -> brake)); + // m_driverController.y().onTrue(drivetrain.applyRequest(() -> brake)); drivetrain.registerTelemetry(logger::telemeterize); @@ -142,6 +142,7 @@ public void setFastMode(){ public void setMediumMode(){ if(Constants.DriveConstants.breakMode) drivetrain.applyRequest(() -> brake); + else if(Constants.DrivetrainOverrides.overrideMode) setOverrideMode(); else speed = SmartDashboard.getNumber("Swerve Speed", 0.5); } @@ -153,6 +154,8 @@ public void setSlowMode(){ public void setOverrideMode(){ speed = 0; dif = Limelight.setPower(); + turndif = Limelight.turnSetPower(); + //System.out.println(dif); //if(Limelight.shootNow()) new ShootBall(shooter, intake,4500); } @@ -169,29 +172,19 @@ public void setOverrideMode(){ private void configureBindings() { //Controller - m_driverController.rightBumper().onTrue((new ShootBall(shooter, intake,4500))); - m_driverController.leftBumper().onTrue(new Rev(shooter, 4500, intake)); + m_driverController.rightBumper().onTrue((new ShootBall(shooter, intake,3200))); + m_driverController.leftBumper().onTrue(new Rev(shooter, 3200, intake)); m_driverController.a().onTrue(new IntakeBall(intake)); m_driverController.b().onTrue(new ProcessorScoring(intake)); m_driverController.start().onTrue(new Unshoot(shooter, intake)); - //Reef intake manual controls: - m_driverController.leftTrigger(0.1).whileTrue(Commands.run(() -> reefIntake.setVoltagePivot(m_driverController.getLeftTriggerAxis()))); //left reef up - m_driverController.rightTrigger(0.1).whileTrue(Commands.run(() -> reefIntake.setVoltagePivot(-m_driverController.getRightTriggerAxis()))); //right reef down - m_driverController.back().whileTrue(Commands.run(() -> reefIntake.setVoltagePivot(0))); - m_driverController.x().whileTrue(Commands.run(() -> reefIntake.setIntakePos(Constants.ReefIntakeConstants.reefGrab))); - m_driverController.y().whileTrue(Commands.run(() -> reefIntake.setIntakePos(Constants.ReefIntakeConstants.reefRest))); - - //Reef intake wheel manual control: - buttonBoard.button(1).whileTrue(Commands.run(() -> reefIntake.setVoltageIntake(2))); - buttonBoard.button(2).whileTrue(Commands.run(() -> reefIntake.setVoltageIntake(-2))); - buttonBoard.button(3).whileTrue(Commands.run(() -> reefIntake.setVoltageIntake(0))); - - //Intake manual controls: m_driverController.pov(0).whileTrue(Commands.run(() -> intake.setIntakePos(Constants.IntakeConstants.rest))); m_driverController.pov(180).whileTrue(Commands.run(() -> intake.setIntakePos(Constants.IntakeConstants.restball))); m_driverController.pov(270).whileTrue(Commands.run(() -> intake.setIntakePos(Constants.IntakeConstants.grab))); + + + // bottomRightButton.onTrue(new LimelightShoot(shooter, intake, 4500)); //Uncomment and use this after reef intake is tuned, remember to comment out the other use of x and y when you do this. @@ -239,4 +232,13 @@ public Command getAutonomousCommand() { //An example command will be run in autonomous return autoChooser.getSelected(); } + + /** + * Use this to pass the self test command to the main {@link Robot} class. + * + * @return the self test command + */ + public Command getSelfTestCommand() { + return new SelfTest(drivetrain, shooter, intake); + } } diff --git a/src/main/java/frc/robot/commands/Climb.java b/src/main/java/frc/robot/commands/Climb.java deleted file mode 100644 index 55fbd1f..0000000 --- a/src/main/java/frc/robot/commands/Climb.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Climber; -import frc.robot.Constants.ClimberConstants; -import edu.wpi.first.wpilibj.Timer; - -public class Climb extends Command{ - private Climber climber; - private double volts; - - public Climb (Climber climber, double rpm) { - this.climber = climber; - this.volts = volts; - - addRequirements(climber); - } - - // TODO: Unfinished - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if ((volts < 0) || volts < 0.1){ //fix after la crosse - climber.setVoltageClimb(volts); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - - } - - - @Override - public boolean isFinished() { - return false; //TODO: Change this to the correct condition - } - -} diff --git a/src/main/java/frc/robot/commands/IntakeBall.java b/src/main/java/frc/robot/commands/IntakeBall.java index 0ee6c86..ff61070 100644 --- a/src/main/java/frc/robot/commands/IntakeBall.java +++ b/src/main/java/frc/robot/commands/IntakeBall.java @@ -21,20 +21,20 @@ public void initialize(){ @Override public void execute(){ - intake.setVoltageIntake(0.4*12); //runs intake to suck in the ball - intake.setVoltageIndex(0.05*12); //runs indexer slightly backward to stop the ball from going into the shooter + intake.setVoltageIntake(0.65*12); //runs intake to suck in the ball + intake.setVoltageIndex(0.02*12); //runs indexer slightly backward to stop the ball from going into the shooter } @Override public void end(boolean interrupted){ - LED.setBlue(); //turns on the LED to show that the intake has a ball + LED.setYellow(); //turns on the LED to show that the intake has a ball intake.setIntakePos(IntakeConstants.rest); //Puts intake in rest position - intake.setVoltageIntake(0.05*12); //Keeps the intake motor running to keep the ball in the intake + intake.setVoltageIntake(0.07*12); //Keeps the intake motor running to keep the ball in the intake intake.setVoltageIndex(0); //Stops indexer } @Override public boolean isFinished(){ - return intake.distanceSensorCheckRange(0,4); + return intake.distanceSensorCheckRange(0, 1.8); } } diff --git a/src/main/java/frc/robot/commands/LimelightShoot.java b/src/main/java/frc/robot/commands/LimelightShoot.java new file mode 100644 index 0000000..94479e6 --- /dev/null +++ b/src/main/java/frc/robot/commands/LimelightShoot.java @@ -0,0 +1,94 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Shooter; +import frc.robot.Constants; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LED; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.Limelight; + +// just remove all the useless stuff +public class LimelightShoot extends Command { + + private Shooter shooter; + private Intake intake; + private double rpm; + private int counter; + private Timer timer; + private int step; + + private boolean shooterReady = false; + + // Creates a new ShootBall command to run the shooter and intake to shoot the ball + public LimelightShoot(Shooter shooter, Intake intake, double rpm) { + this.shooter = shooter; + this.intake = intake; + + this.rpm = rpm; + timer = new Timer(); + addRequirements(shooter); + addRequirements(intake); + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + LED.setYellow(); + + + intake.setIntakePos(IntakeConstants.restball); // set the intake to restball position + timer = new Timer(); + shooter.setTargetRPM(rpm); + counter = 0; + step = 0; + + + + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + // if(!Limelight.getTv()) step == 0; + // if(Limelight.getTx() > Math.abs() + // else dif = 0; + if(step == 3){ + if(shooter.isReady())counter++; //counter creates a delay to make sure the shooter is ready to shoot and + if(counter == 10)shooterReady = true; + if(shooterReady){ //if the shooter is ready to shoot, set the intake to run at a certain voltage to shoot the ball + Constants.DriveConstants.breakMode = true; // set the drive to break mode to stop the robot from moving while shooting the ball + LED.setGreen(); //turns on the LED to show that the shooter is shooting + intake.setVoltageIntake(0.7*12); + intake.setVoltageIndex(-0.7*12); + shooterReady = false; + if (!shooterReady){ + timer.start(); + } + } + } + shooter.setTargetRPM(rpm); // set the target RPM to the desired RPM to shoot the ball + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Constants.DriveConstants.breakMode = false; // set the drive to coast mode to allow the robot to move after shooting the ball + intake.setVoltageIntake(0); // set the intake and shooter to stop shooting + intake.setVoltageIndex(0); + shooter.setTargetRPM(0); + intake.setIntakePos(IntakeConstants.rest); // set the intake to rest position + LED.setRed(); //turns off the LED to show that the shooter is not running + } + + @Override + public boolean isFinished() { + return !Limelight.getTv() || timer.get() > 1; // check if the timer is greater than 1 second to end the command, this will give a short burst to shoot the ball + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ProcessorScoring.java b/src/main/java/frc/robot/commands/ProcessorScoring.java index 419b392..7b63127 100644 --- a/src/main/java/frc/robot/commands/ProcessorScoring.java +++ b/src/main/java/frc/robot/commands/ProcessorScoring.java @@ -1,22 +1,23 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LED; import frc.robot.Constants.IntakeConstants; public class ProcessorScoring extends Command{ private Intake intake; public ProcessorScoring(Intake intake){ - this.intake = intake; + this.intake = intake; addRequirements(intake); } @Override public void initialize(){ intake.setIntakePos(IntakeConstants.rest); // set the intake to rest position - intake.setVoltageIntake(-2.4); // runs the indexer out of the intake - intake.setVoltageIndex(2.4); // runs the intake out of the intake + intake.setVoltageIntake(-1.5); // runs the indexer out of the intake 2.4 + intake.setVoltageIndex(1.5); // runs the intake out of the intake /2.4 } @Override @@ -29,6 +30,7 @@ public void execute() { public void end(boolean interupt) { intake.setVoltageIntake(0);// stops the indexer intake.setVoltageIndex(0);// stops the intake + LED.setRed(); } @Override diff --git a/src/main/java/frc/robot/commands/ReefIntakeBall.java b/src/main/java/frc/robot/commands/ReefIntakeBall.java deleted file mode 100644 index 7624413..0000000 --- a/src/main/java/frc/robot/commands/ReefIntakeBall.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.ReefIntakeConstants; -import frc.robot.subsystems.ReefIntake; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.Intake; - - - -public class ReefIntakeBall extends Command{ - private ReefIntake reefIntake; - private Shooter shooter; - private Intake intake; - - public ReefIntakeBall(ReefIntake reefIntake, Shooter shooter, Intake intake){ - this.reefIntake = reefIntake; - addRequirements(reefIntake); - this.shooter = shooter; - addRequirements(shooter); - this.intake = intake; - addRequirements(intake); - } - - @Override - public void initialize(){ - reefIntake.setIntakePos(ReefIntakeConstants.reefGrab); //Sets reefIntake to grab position - intake.setIntakePos(IntakeConstants.restball); //Puts intake in rest position - - } - - @Override - public void execute(){ - reefIntake.setVoltageIntake(0.4*12); //runs reefIntake to suck in the ball - shooter.setTargetRPM(-1000); //Sets shooter to run at -1000 RPM to suck in the ball and put it in the intake - intake.setVoltageIntake(0.05*12); //Keeps the intake motor running to keep the ball in the intake - intake.setVoltageIndex(3.6); //Stops indexer - - } - @Override - public void end(boolean interrupted){ - reefIntake.setIntakePos(IntakeConstants.rest); //Puts reefIntake in rest position - intake.setIntakePos(IntakeConstants.restball); //Pusts intake in rest position - reefIntake.setVoltageIntake(0.05*12); //Keeps the reefIntake motor running to keep the ball in thereefIntake - shooter.setTargetRPM(0); //Stops shooter - intake.setVoltageIndex(0); //Stops indexer - } - - @Override - public boolean isFinished(){ - return intake.distanceSensorCheckRange(0,4); - } -} diff --git a/src/main/java/frc/robot/commands/ReefKnockOff.java b/src/main/java/frc/robot/commands/ReefKnockOff.java deleted file mode 100644 index 909eb33..0000000 --- a/src/main/java/frc/robot/commands/ReefKnockOff.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.ReefIntakeConstants; -import frc.robot.subsystems.ReefIntake; - - - -public class ReefKnockOff extends Command{ - private ReefIntake reefIntake; - - public ReefKnockOff(ReefIntake reefIntake){ - this.reefIntake = reefIntake; - addRequirements(reefIntake); - } - - @Override - public void initialize(){ - reefIntake.setIntakePos(ReefIntakeConstants.reefGrab); //Sets reefIntake to grab position - } - - @Override - public void execute(){ - reefIntake.setVoltageIntake(2); //runs reefIntake to suck in the ball - } - @Override - public void end(boolean interrupted){ - reefIntake.setIntakePos(IntakeConstants.rest); //Puts reefIntake in rest position - reefIntake.setVoltageIntake(0); //Keeps the reefIntake motor running to keep the ball in thereefIntake - } - - @Override - public boolean isFinished(){ - return true; - } -} diff --git a/src/main/java/frc/robot/commands/Rev.java b/src/main/java/frc/robot/commands/Rev.java index ac62707..475709a 100644 --- a/src/main/java/frc/robot/commands/Rev.java +++ b/src/main/java/frc/robot/commands/Rev.java @@ -24,7 +24,7 @@ public Rev(Shooter shoot, double rpm, Intake intake){ @Override public void initialize() { intake.setIntakePos(IntakeConstants.restball); // set the intake to restball position - LED.setBlue(); //turns on the LED to show that the shooter is running + LED.setYellow(); //turns on the LED to show that the shooter is running shooter.setTargetRPM(rpm); // set the target RPM to the desired RPM to run the shooter at } @@ -37,6 +37,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + LED.setGreen(); } diff --git a/src/main/java/frc/robot/commands/SelfTest.java b/src/main/java/frc/robot/commands/SelfTest.java new file mode 100644 index 0000000..2c2816e --- /dev/null +++ b/src/main/java/frc/robot/commands/SelfTest.java @@ -0,0 +1,75 @@ +package frc.robot.commands; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; + +public class SelfTest extends Command { + private CommandSwerveDrivetrain swerveSubsystem; + private Shooter shooter; + private Intake intake; + + private SwerveRequest.RobotCentric driveForward = new SwerveRequest.RobotCentric().withVelocityX(1); + private SwerveRequest.RobotCentric driveSideways = new SwerveRequest.RobotCentric().withVelocityY(1); + private SwerveRequest.RobotCentric stop = new SwerveRequest.RobotCentric(); + + + public SelfTest(CommandSwerveDrivetrain swerveSubsystem, Shooter shooter, Intake intake){ + this.swerveSubsystem = swerveSubsystem; + this.shooter = shooter; + this.intake = intake; + addRequirements(swerveSubsystem, shooter, intake); + } + + @Override + public void execute(){ + System.out.println("--------------------------------------------------------------------------------"); + System.out.println("-----------------------Starting Morgan Le Fay's Self Test-----------------------"); + System.out.println("--------------------------------------------------------------------------------\n\n\n"); + + System.out.println("Testing Swerve Subsystem..."); + swerveSubsystem.applyRequest(() -> driveForward); + Timer.delay(2); + swerveSubsystem.applyRequest(() -> stop); + swerveSubsystem.applyRequest(() -> driveSideways); + swerveSubsystem.applyRequest(() -> stop); + + System.out.println("Testing Intake Subsystem..."); + intake.setIntakePos(IntakeConstants.grab); + Timer.delay(0.5); + intake.setIntakePos(IntakeConstants.rest); + Timer.delay(0.5); + intake.setIntakePos(IntakeConstants.restball); + Timer.delay(0.5); + intake.setVoltageIntake(0.3); + Timer.delay(0.5); + intake.setVoltageIntake(0); + intake.setVoltageIndex(0.5); + Timer.delay(0.5); + intake.setVoltageIndex(0); + System.out.println("ALL INTAKE COMMANDS EXECUTED\n"); + + System.out.println("Testing Shooter Subsystem..."); + shooter.setTargetRPM(3000); + Timer.delay(2); + shooter.setTargetRPM(0); + System.out.println("SHOOTER SUBSYSTEM TEST COMPLETE\n"); + } + + @Override + public void end(boolean interrupted){ + System.out.println("-------------------------------------------------------------------------"); + System.out.println("----------------------------Self Test Complete---------------------------"); + System.out.println("-------------------------------------------------------------------------\n\n\n"); + } + + @Override + public boolean isFinished(){ + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootBall.java b/src/main/java/frc/robot/commands/ShootBall.java index 634be77..80e41f2 100644 --- a/src/main/java/frc/robot/commands/ShootBall.java +++ b/src/main/java/frc/robot/commands/ShootBall.java @@ -34,7 +34,6 @@ public ShootBall(Shooter shooter, Intake intake, double rpm) { // Called when the command is initially scheduled. @Override public void initialize() { - LED.setBlue(); // shooter.vroom(20); shooter.setTargetRPM(rpm); intake.setIntakePos(IntakeConstants.restball); // set the intake to restball position diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java deleted file mode 100644 index 6a4a9e2..0000000 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ /dev/null @@ -1,87 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.Rev2mDistanceSensor; -import com.revrobotics.Rev2mDistanceSensor.Port; -import com.revrobotics.Rev2mDistanceSensor.RangeProfile; - -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.math.MathUtil; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Constants.ClimberConstants; -import frc.robot.Constants.IntakeConstants; - - -public class Climber extends SubsystemBase{ - private TalonFX climberMotor; - private DutyCycleEncoder climberEncoder; - private PIDController climberPID; - - double difference = 0; - double setPosition = 0; - public Climber() { - - var talonFXConfigs = new TalonFXConfiguration(); - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kP = 0.01; - - climberEncoder = new DutyCycleEncoder(2,1,Constants.ClimberConstants.encoderOffset); - - climberMotor = new TalonFX(Constants.Ports.climberMotor); - - climberPID = new PIDController(ClimberConstants.kP, ClimberConstants.kI, ClimberConstants.kD); - - climberMotor.getConfigurator().apply(talonFXConfigs); - climberMotor.setNeutralMode(NeutralModeValue.Brake); - } - - public double getPosition(){ - return climberEncoder.get(); - } - - public double getClimberVelocity(){ - return climberMotor.getVelocity().getValueAsDouble(); - } - - public double getClimberVolts(){ - return climberMotor.getSupplyVoltage().getValueAsDouble(); - } - - public double getClimberCurent(){ - return climberMotor.getSupplyCurrent().getValueAsDouble(); - } - public void setVoltageClimb(double volts){ - climberMotor.setVoltage(volts); - } - public void setMaxHeight(){ - //this.set(0.5); - } - @Override - public void periodic() { - difference = Math.abs(setPosition - getPosition()); - } - - - - - - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 06bbfb0..1316e68 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,17 +3,11 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ResetMode; + import com.revrobotics.Rev2mDistanceSensor; import com.revrobotics.Rev2mDistanceSensor.Port; import com.revrobotics.Rev2mDistanceSensor.RangeProfile; import com.revrobotics.Rev2mDistanceSensor.Unit; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.MathUtil; @@ -31,11 +25,11 @@ public class Intake extends SubsystemBase { /** Creates a new Intake. */ private TalonFX intakeMotor; private TalonFX indexerMotor; - private SparkMax pivotMotor; + private TalonFX pivotMotor; private Rev2mDistanceSensor distanceSensor; - private SparkMaxConfig pivotConfig; + // private SparkMaxConfig pivotConfig; private DutyCycleEncoder encoder; private PIDController pivotPID; @@ -68,23 +62,23 @@ public Intake(){ var slot0Configs = talonFXConfigs.Slot0; slot0Configs.kP = 0.01; - pivotConfig = new SparkMaxConfig(); + //pivotConfig = new SparkMaxConfig(); - pivotConfig.idleMode(IdleMode.kBrake); + //pivotConfig.idleMode(IdleMode.kBrake); - pivotConfig.encoder.positionConversionFactor(1000); - pivotConfig.encoder.velocityConversionFactor(1000); + //pivotConfig.encoder.positionConversionFactor(1000); + //pivotConfig.encoder.velocityConversionFactor(1000); - pivotConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(1.0,0.0,0.0); + //pivotConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(1.0,0.0,0.0); encoder = new DutyCycleEncoder(Constants.Ports.intakeHexPort,1,Constants.IntakeConstants.encoderOffset); intakeMotor = new TalonFX(Constants.Ports.intakeMotor); indexerMotor = new TalonFX(Constants.Ports.indexerMotor); - - pivotMotor = new SparkMax(Constants.Ports.pivotMotor, MotorType.kBrushless); + pivotMotor = new TalonFX(Constants.Ports.pivotMotor); + //pivotMotor = new SparkMax(Constants.Ports.pivotMotor, MotorType.kBrushless); - pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + //pivotMotor.configure(pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); pivotPID = new PIDController(IntakeConstants.kP, IntakeConstants.kI,IntakeConstants.kD); pivotForward = new SimpleMotorFeedforward(IntakeConstants.kS, IntakeConstants.kV, IntakeConstants.kA); @@ -95,8 +89,10 @@ public Intake(){ intakeMotor.setNeutralMode(NeutralModeValue.Brake); indexerMotor.getConfigurator().apply(talonFXConfigs); indexerMotor.setNeutralMode(NeutralModeValue.Brake); + pivotMotor.getConfigurator().apply(talonFXConfigs); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); - distanceSensorDerivative = 0.0; //initialize to 0 + setIntakePos(IntakeConstants.restball); } @@ -104,13 +100,10 @@ public boolean distanceSensorCheckRange(double min, double max) { return (RobotBase.isReal() && (latestDistance < max && latestDistance > min)); } - - public void setVoltageIntake(double volts){ intakeMotor.setVoltage(volts); } - public void setVoltageIndex(double volts){ indexerMotor.setVoltage(volts); } @@ -166,12 +159,12 @@ public double getPivotSpeed(){ //getVoltagePivot public double getPivotVoltage(){ - return pivotMotor.getBusVoltage(); + return pivotMotor.getSupplyVoltage().getValueAsDouble(); } //getCurrentPivot public double getPivotCurrent(){ - return pivotMotor.getOutputCurrent(); + return pivotMotor.getSupplyCurrent().getValueAsDouble(); } /**Returns true of the intake is on. */ @@ -212,6 +205,7 @@ public void periodic() { difference = Math.abs(setPosition - getPosition()); SmartDashboard.putNumber("Intake Motor Velocity", getIntakeVelocity()); + SmartDashboard.putBoolean("Ball Detected: ", distanceSensorCheckRange(0, 1.8)); SmartDashboard.putNumber("IntakeM Voltage", getIntakeVolts()); SmartDashboard.putNumber("IntakeM Current", getIntakeCurrent()); SmartDashboard.putBoolean("Detected Color", isOn()); diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java index f6fb8fd..371bcdc 100644 --- a/src/main/java/frc/robot/subsystems/LED.java +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -4,33 +4,33 @@ import frc.robot.Constants; public class LED { - // private static SerialPort arduino = new SerialPort(9600, Constants.Ports.arduino, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne); + private static SerialPort arduino = new SerialPort(9600, Constants.Ports.arduino, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne); public static void intialize(){ - //arduino.writeString("F"); + arduino.writeString("E"); } public static void setRed(){ - //arduino.writeString("R"); //RED + arduino.writeString("R"); //RED } - - public static void setBlue(){ //intakeHasBall - //arduino.writeString("B"); //BLUE + arduino.writeString("B"); //BLUE } - - public static void setGreen(){ //shootingShot - //arduino.writeString("G"); //GREEN + arduino.writeString("G"); //GREEN } - public static void setBlack(){ //off - //arduino.writeString("A"); //BLACK + public static void setYellow(){ + arduino.writeString("Y"); //Yellow } + public static void setBlinkingRed(){ + arduino.writeString("E"); //RED + } - - + public static void setBlack(){ //off + arduino.writeString("A"); //BLACK + } } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 6e25ff8..7d447a8 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -19,6 +19,7 @@ public Limelight(){ SmartDashboard.putNumber("LL Ta", getTa()); SmartDashboard.putBoolean("LL ready", Limelight.shootNow()); SmartDashboard.putNumber("LL power" , Limelight.setPower()); + SmartDashboard.putNumber("LL turn power" , Limelight.turnSetPower()); SmartDashboard.putNumber("LL distance" , Limelight.distanceFromTag()); } @@ -73,4 +74,12 @@ public static boolean shootNow(){ public static Pose2d getLimelightPose(){ return LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight").pose; } + + public static double turnSetPower(){ + if(!getTv()) return 0; + if((getTy())> 10) return 0.3; + if((getTy()) < -10) return -0.3; + return (getTy() / (3/25)); //tune + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ReefIntake.java b/src/main/java/frc/robot/subsystems/ReefIntake.java deleted file mode 100644 index 8dc91b2..0000000 --- a/src/main/java/frc/robot/subsystems/ReefIntake.java +++ /dev/null @@ -1,223 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ResetMode; - -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc.robot.Constants; -import frc.robot.Constants.ReefIntakeConstants; - -/* How to tune the reef intake: - * YOU SHOULD NOT HAVE TO ADD OR DELETE ANY LINES OF CODE, just uncomment and change values when you get to them. - * First setup the hex encoder - * 1. Manually hold the pivot straight up so it balances on itself and take that postion. - * 2. Then Hold the pivot straight out in front of the robot and take that position. - * 3. If the 2nd position is less then the first, then the encoder is backwards, change getPosition() to -encoder.get() - * then repeat steps 1 and 2 - * 4. Now your encoder offset is: 1st position - 0.25. If this value is less then 0 add 1. - * 5. Verify that the encoder offset is correct by manually moving the arm straight up and seeing it reads 0.25. - * HEX ENCODER IS TUNED!!!!!!! - * - * Tune PID - * 1. You can manually move the reef intake with either trigger. - * 2. In elastic use the reef pivot PID to tune the PID. - * 3. You can change the set position of the reef intake with x and y. - * If the Arm is moving the opposite direction of the set point put a negative in front of the pivotPID line 135 - * - * Once you are done PID tuning you can test the flywheel, (button board top left 3 buttons) - * Then test the reef knock off command. Comment out the manual set position in robot container and uncomment the Reef knock off command. - * When you test the reef knock off command you may need to chang ethe flywheel volts which can be changed in ReefKnockOff.java Line 25k - */ - -public class ReefIntake extends SubsystemBase { - private TalonFX reefPivotMotor; - private SparkMax reefIntakeMotor; - - private SparkMaxConfig intakeConfig; - - private DutyCycleEncoder encoder; - - private PIDController pivotPID; - private ArmFeedforward armFeedforward; - - private final double lowerLimit = ReefIntakeConstants.reefRest; // needs to be fine tuned - private final double upperLimit = ReefIntakeConstants.reefGrab; //limits for intake positions - - private double setPosition; - private double difference; - - //Reef Intake is 2.8 lbs - //Bar is 20 inches long - - public ReefIntake(){ - var talonFXConfigs = new TalonFXConfiguration(); - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kP = 0.01; - - intakeConfig = new SparkMaxConfig(); - - intakeConfig.idleMode(IdleMode.kBrake); - - intakeConfig.encoder.positionConversionFactor(1000); - intakeConfig.encoder.velocityConversionFactor(1000); - - intakeConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(1.0,0.0,0.0); - - encoder = new DutyCycleEncoder(Constants.Ports.reefHexPort,1,Constants.ReefIntakeConstants.encoderOffset); - - reefPivotMotor = new TalonFX(Constants.Ports.reefPivotMotor); - reefIntakeMotor = new SparkMax(Constants.Ports.reefIntakeMotor, MotorType.kBrushless); - - reefIntakeMotor.configure(intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - pivotPID = new PIDController(ReefIntakeConstants.kP, ReefIntakeConstants.kI,ReefIntakeConstants.kD); - armFeedforward = new ArmFeedforward(ReefIntakeConstants.kS, ReefIntakeConstants.kG, ReefIntakeConstants.kV); - - reefPivotMotor.getConfigurator().apply(talonFXConfigs); - reefPivotMotor.setNeutralMode(NeutralModeValue.Brake); - //reefPivotMotor.configure(new SparkMaxConfig().idleMode(IdleMode.kBrake), null, null); - //reefPivotMotor.getConfigurator().apply(talonFXConfigs); - //reefPivotMotor.setNeutralMode(NeutralModeValue.Brake); - - setIntakePos(ReefIntakeConstants.reefRest); //uncomment tis - } - - /**Sets intake to suck in */ - public void sucker() { - reefIntakeMotor.setVoltage(0.2*12);//0.3 - - } - - /**Runs intake backwards at 0.12 speed*/ - public void outTake() { - reefIntakeMotor.setVoltage(-2.4); - - } - - /**Stops the intake. */ - public void stopTake(){ - reefIntakeMotor.set(0); - - } - - public void setVoltageIntake(double volts){ - reefIntakeMotor.setVoltage(volts); - - } - - public void setVoltagePivot(double volts){ - reefPivotMotor.setVoltage(volts); - System.out.println("Volts " + volts); - } - - public void setIntakePos(double position) { - position = MathUtil.clamp(position, lowerLimit, upperLimit); - - - setPosition = position; - //Remember to put negative infront of pivot if the arm goes the opposite direction of the set point - reefPivotMotor.setVoltage(-pivotPID.calculate(getPosition(), position)); //reefPivotMotor.setVoltage(-pivotPID.calculate(getPosition(), position)); - } - - /**Stops all motors */ - public void stopAll(){ - reefIntakeMotor.set(0); - - reefPivotMotor.set(0); - - } - - /**Returns the velocity of the intake motor. */ - public double getreefGrabSpeed(){ - return reefIntakeMotor.getEncoder().getVelocity(); - } - - /**Returns the voltage of the intake motor. */ - public double getreefGrabVoltage(){ - return reefIntakeMotor.getBusVoltage(); - } - - /**Returns the current of the reef intake motor. */ - public double getreefGrabCurrent(){ - return reefIntakeMotor.getOutputCurrent(); - } - - public double getPivotSpeed(){ - return reefPivotMotor.getVelocity().getValueAsDouble(); - } - - public double getPivotVoltage(){ - return reefPivotMotor.getMotorVoltage().getValueAsDouble(); - } - - public double getPivotCurrent(){ - return reefPivotMotor.getSupplyCurrent().getValueAsDouble(); - } - - /**Returns true of the intake is on. */ - public boolean isOn(){ - return Math.abs(getreefGrabSpeed()) > 0; - } - /**Returns the intake motor's rotor velocity in rotations per minute */ - public double getRPM(){ - return -(60 * reefIntakeMotor.getEncoder().getVelocity()); - } - public double getSetPosition(){ - return setPosition; - } - - /**Gets the position of the arm from the hex encoder */ - /* We are zeroing when reef intake is straight up (Math.PI/2) - * so just subtract 0.25 (cuz this is rotations) to get the position that the ArmFeedForward expects. - * We mutliply by 6.28 later to get the radians for the ArmFeedForward. - */ - public double getPosition(){ - return encoder.get(); //-0.25; sign depends on which way is forward - } - // checks if the reef intake is ready to intake based on the position its in. - public boolean isReady(){ - return difference < 0.0025 || (getSetPosition() == ReefIntakeConstants.reefGrab && getPosition() < ReefIntakeConstants.reefGrab); - } - - @Override - public void periodic() { - - difference = Math.abs(setPosition - getPosition()); - - // SmartDashboard.putBoolean("Ball Ate detected:", ateBall()); - // SmartDashboard.putBoolean("Ball detected:", hasBall()); - SmartDashboard.putNumber("reef Intake Motor Speed", getreefGrabSpeed()); - SmartDashboard.putNumber("reef IntakeM Voltage", getreefGrabVoltage()); - SmartDashboard.putBoolean("reef Detected Color", isOn()); - SmartDashboard.putNumber("reef RPM", getRPM()); - SmartDashboard.putNumber("reef SetPosition", getSetPosition()); - SmartDashboard.putNumber("reef IntakePosition", getPosition()); - SmartDashboard.putNumber("reef Pivot Speed", getPivotSpeed()); - SmartDashboard.putNumber("reef Pivot Voltage", getPivotVoltage()); - SmartDashboard.putNumber("reef Pivot Current", getPivotCurrent()); - SmartDashboard.putBoolean("reef Pivot Ready", isReady()); - SmartDashboard.putData("reef Pivot PID", pivotPID); - //SmartDashboard.putNumber("reef FeedForward", armFeedforward.calculate(getPosition() * 6.28,0)); - SmartDashboard.putNumber("reef PID Value", pivotPID.calculate(getPosition(), getSetPosition())); - SmartDashboard.putNumber("reef Both", pivotPID.calculate(getPosition(), getSetPosition()) + armFeedforward.calculate(getPosition() * 6.28,0)); - - setIntakePos(getSetPosition()); - -} -} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index c39cc3a..9f63c1d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -54,9 +54,9 @@ public void setTargetRPM(double rpm){ targetRPM = rpm; } - //Returns true if the shooting motor is within 1% of the targetRPM. This is used to check if the motor is ready to shoot. + //Returns true if the shooting motor is within 10% of the targetRPM. This is used to check if the motor is ready to shoot. public boolean isReady(){ - return Math.abs(targetRPM) * 0.01 > Math.abs(targetRPM - getRPM()); + return Math.abs(targetRPM) * 0.05 > Math.abs(targetRPM - getRPM()); } //This is a PRIVATE method that sets the motor to the targetRPM. It is called in the periodic method.