diff --git a/build.gradle b/build.gradle index 8862516..b7f2e02 100644 --- a/build.gradle +++ b/build.gradle @@ -124,4 +124,4 @@ gversion { dateFormat = "yyyy-MM-dd HH:mm:ss z" timeZone = "America/Denver" indent = " " -} +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..4a63cc1 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "System Joysticks": { + "window": { + "enabled": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/deploy/pathplanner/autos/Auto Test Slow.auto b/src/main/deploy/pathplanner/autos/Auto Test Slow.auto new file mode 100644 index 0000000..bb1c386 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Auto Test Slow.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TurnNinety" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto new file mode 100644 index 0000000..db06ab4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Taxi" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Taxi.path similarity index 84% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Taxi.path index 3f475e5..4777d33 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 2.9612559241706156, + "y": 7.0 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 1.9612559241706156, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 0.2, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TurnNinety.path b/src/main/deploy/pathplanner/paths/TurnNinety.path new file mode 100644 index 0000000..e75bda6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TurnNinety.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 1.0, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "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": -90.48219994972354 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6850b74..d551676 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,19 +9,15 @@ package frc4388.robot; import java.io.File; +import com.pathplanner.lib.commands.PathPlannerAuto; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc4388.utility.DeferredBlock; -import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.controller.ButtonBox; -import frc4388.utility.controller.DeadbandedXboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; - +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -32,25 +28,28 @@ import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.XboxController; import frc4388.utility.structs.LEDPatterns; import frc4388.robot.commands.MoveForTimeCommand; -import frc4388.robot.commands.alignment.RotTo45; -import frc4388.robot.commands.MoveUntilSuply; -// import frc4388.robot.commands.alignment.DriveToReef; -// import frc4388.robot.commands.wait.waitElevatorRefrence; -// import frc4388.robot.commands.wait.waitEndefectorRefrence; -// import frc4388.robot.commands.wait.waitFeedCoral; -import frc4388.robot.commands.wait.waitSupplier; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.LEDConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; - -import com.pathplanner.lib.commands.PathPlannerAuto; - +import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.shooter.Shooter; +import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.controller.DeadbandedXboxController; +// Autos +import frc4388.utility.controller.VirtualController; +import frc4388.utility.controller.XboxController; +import frc4388.utility.structs.LEDPatterns; /** * This class is where the bulk of the robot should be declared. Since @@ -93,6 +92,22 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + // private Command RobotShoot = new SequentialCommandGroup( + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + // new WaitCommand(5), + // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) + // ); + + // private Command RobotShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) + // ); + + // private Command RobotIntake = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) + // ); public RobotContainer() { @@ -103,6 +118,7 @@ public class RobotContainer { // }, false); DeferredBlock.addBlock(() -> { // Called on every robot enable TimesNegativeOne.update(); + FieldPositions.update(); }, true); @@ -160,6 +176,13 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotLED.setTo5V())); + // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) + // .onTrue(RobotShoot) + // .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); + + // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) + // .onTrue(RobotIntake) + // .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) // .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); @@ -212,7 +235,7 @@ public class RobotContainer { dir = new File("/home/lvuser/deploy/pathplanner/autos/"); } else { // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); - dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos"); + dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\src\\main\\deploy\\pathplanner\\autos\\"); } String[] autos = dir.list(); @@ -238,7 +261,7 @@ public class RobotContainer { // } // System.out.println("Robot Auto Changed " + filename); }); - // SmartDashboard.putData(autoChooser); + SmartDashboard.putData(autoChooser); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4f7889e..e462071 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,26 +9,24 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; -import org.photonvision.PhotonCamera; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; - import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DigitalInput; + +import frc4388.robot.constants.Constants; //import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.SimConstants; -import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.robot.subsystems.intake.IntakeIO; +import frc4388.robot.subsystems.intake.IntakeReal; +import frc4388.robot.subsystems.shooter.ShooterConstants; +import frc4388.robot.subsystems.shooter.ShooterIO; +import frc4388.robot.subsystems.shooter.ShooterReal; // import frc4388.robot.subsystems.elevator.ElevatorIO; // import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; import frc4388.robot.subsystems.swerve.SwerveIO; import frc4388.robot.subsystems.swerve.SwerveReal; -import frc4388.robot.subsystems.vision.VisionIO; -import frc4388.robot.subsystems.vision.VisionReal; import frc4388.utility.status.FaultCANCoder; -import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -55,18 +53,19 @@ public class RobotMap { /* Swreve Drive Subsystem */ // public final SwerveIO swerveDrivetrain; - // /* Elevator Subsystem */ - // public final ElevatorIO elevatorIO; + // /* Shooter and Intake Subsystem */ + public final ShooterIO shooterIO; + public final IntakeIO intakeIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { case REAL: - // // Configure cameras + // Configure cameras // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - // leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; - // rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + // leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; + // rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS); // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); @@ -84,25 +83,34 @@ public class RobotMap { // swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); - // Configure elevator - - // TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); - // TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + // Configure Shooter 22,23,24 + TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS); + TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.CANIVORE_CANBUS); + TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS); + //Configure Intake 20,21 + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.CANIVORE_CANBUS); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - // elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); + // shooterIO = new ShooterIO() {}; + shooterIO = new ShooterReal(shooter1, shooter2, indexer); + intakeIO = new IntakeReal(arm, roller); // Fault // FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - // // FaultTalonFX.addDevice(elevator, "Elevator"); - // // FaultTalonFX.addDevice(endeffector, "Endeffector"); + + FaultTalonFX.addDevice(shooter1, "Shooter1"); + FaultTalonFX.addDevice(shooter2, "Shooter2"); + FaultTalonFX.addDevice(indexer, "Indexer"); + FaultTalonFX.addDevice(arm, "Arm"); + FaultTalonFX.addDevice(roller, "Roller"); // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -123,10 +131,9 @@ public class RobotMap { default: // leftCamera = new VisionIO() {}; // rightCamera = new VisionIO() {}; - // reefLidar = new LidarIO() {}; - // reverseLidar = new LidarIO() {}; - // swerveDrivetrain = new SwerveIO() {}; - // elevatorIO = new ElevatorIO() {}; + swerveDrivetrain = new SwerveIO() {}; + intakeIO = new IntakeIO() {}; + shooterIO = new ShooterIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt deleted file mode 100644 index a65aea9..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt +++ /dev/null @@ -1,20 +0,0 @@ -AUTO file format - -HEADER static size 0x5 -0x00 BYTE NUM AXES: defualts to 6 -0x01 BYTE NUM POV: defualts to 1 -0x02 BYTE NUM CONTROLLERS: defualts to 2 -0x03 SHORT FRAMES: any value greator or equal than one. - -FRAME PER CONTROLLER: defualt size 0x34 -0x00 DOUBLE AXES[NUM AXES] -0x30 SHORT BUTTONS -0x32 SHORT POVs[NUM POV] - -FRAME: size varrys -FRAME PER CONTROLLER[NUM CONTROLLERS] -INT UNIXTIMESTAMP - -FILE: -HEADER -FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java deleted file mode 100644 index f3d2987..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ /dev/null @@ -1,108 +0,0 @@ -package frc4388.robot.commands.Autos; -// package frc4388.robot.commands.Autos; - -// import java.io.File; -// import java.util.ArrayList; -// import java.util.HashMap; - -// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; -// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -// import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.InstantCommand; -// import frc4388.robot.commands.Swerve.JoystickPlayback; -// import frc4388.robot.commands.Swerve.neoJoystickPlayback; -// import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.utility.controller.VirtualController; - -// public class neoPlaybackChooser { -// private final SendableChooser m_teamChosser = new SendableChooser(); -// private final SendableChooser m_possitionChosser = new SendableChooser(); -// private final SendableChooser m_autoNameChosser = new SendableChooser(); - -// private final SwerveDrive m_swerve; -// private final VirtualController[] m_controllers; -// // private final ArrayList> m_choosers = new ArrayList<>(); -// // private SendableChooser m_playback = null; -// private final ArrayList m_widgets = new ArrayList<>(); -// // private final HashMap m_commandPool = new HashMap<>(); - -// // private final File m_dir = new File("/home/lvuser/autos/"); -// // private int m_cmdNum = 0; - -// // commands -// private Command m_noAuto = new InstantCommand(); - -// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { -// m_teamChosser.addOption("Red", "red"); -// m_teamChosser.setDefaultOption("Blue", "blue"); -// m_teamChosser.addOption("Nuetral", "nuetral"); -// m_possitionChosser.addOption("AMP", "amp"); -// m_possitionChosser.setDefaultOption("Center", "center"); -// m_possitionChosser.addOption("Source", "source"); -// m_swerve = swerve; -// m_controllers = controllers; -// } - -// public neoPlaybackChooser addOption(String name, String option) { -// m_autoNameChosser.addOption(name, option); -// return this; -// } - -// // public PlaybackChooser buildDisplay() { -// // for (int i = 0; i < 10; i++) { -// // appendCommand(); -// // } -// // m_playback = m_choosers.get(0); -// // nextChooser(); - -// // // ! This does not work, why? -// // Shuffleboard.getTab("Auto Chooser") -// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) -// // .withPosition(4, 0); -// // return this; -// // } - -// // This will be bound to a button for the time being -// public void render() { -// // var chooser = new SendableChooser(); -// // // chooser.setDefaultOption("No Auto", m_noAuto); - -// // m_choosers.add(chooser); -// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") -// .add("Command: " + m_choosers.size(), chooser) -// .withSize(4, 1) -// .withPosition(0, m_choosers.size() - 1) -// .withWidget(BuiltInWidgets.kSplitButtonChooser) -// .withWidget(BuiltInWidgets.kComboBoxChooser); - - -// m_widgets.add(widget); -// } - -// // public void nextChooser() { -// // SendableChooser chooser = m_choosers.get(m_cmdNum++); - -// // String[] dirs = m_dir.list(); - -// // if(dirs != null){ // Fix funny error -// // for (String auto : dirs) { -// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); -// // } -// // } - - -// // for (var cmd_name : m_commandPool.keySet()) { -// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); -// // } -// // } - -// public String autoName() { -// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; -// } - -// public Command getCommand() { -// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); -// } -// } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 53c1af0..abf86f9 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,15 +5,15 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 9; - public static final String GIT_SHA = "fe412d09810b860f3ac2a65296613feac250a4e9"; - public static final String GIT_DATE = "2026-01-19 13:57:43 MST"; - public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2026-01-27 19:38:56 MST"; - public static final long BUILD_UNIX_TIME = 1769567936908L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 40; + public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190"; + public static final String GIT_DATE = "2026-02-10 19:42:47 MST"; + public static final String GIT_BRANCH = "shoot-button"; + public static final String BUILD_DATE = "2026-02-11 12:41:33 MST"; + public static final long BUILD_UNIX_TIME = 1770838893524L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index fe4a13c..6ee9e5e 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -7,7 +7,7 @@ package frc4388.robot.constants; -import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.CANBus; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -21,7 +21,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc4388.utility.compute.Trim; -import frc4388.utility.status.CanDevice; import frc4388.utility.structs.Gains; import frc4388.utility.structs.LEDPatterns; @@ -34,7 +33,8 @@ import frc4388.utility.structs.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final String CANBUS_NAME = "rio"; + public static final CANBus RIO_CANBUS = CANBus.roboRIO(); + public static final CANBus CANIVORE_CANBUS = new CANBus("drivetrain"); // public static final class LiDARConstants { // public static final int REEF_LIDAR_DIO_CHANNEL = 7; @@ -91,32 +91,27 @@ public final class Constants { public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } - - public static final class FieldConstants { - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - // Test april tag field layout - // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - // Arrays.asList(new AprilTag[] { - // new AprilTag(1, new Pose3d( - // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - // )), - // }), 100, 100); - - } + public static final class LEDConstants { public static final int LED_SPARK_ID = 9; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED_ORANGE; - public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; - public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; - public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; - public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; + // LED color for when the intake is out + public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED; + // LED color for when the intake is out, but the driver conditions are bad + public static final LEDPatterns INTAKE_OUT_BADPHYS = LEDPatterns.RED_STROBE; - public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; - public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; + // LED color for when the flywheel speed isn't in tolarance + public static final LEDPatterns BAD_FLYWEEL = LEDPatterns.SOLID_GOLD; + // LED color for when the flywheel speed isn't in tolarance, but the driver conditions are bad + public static final LEDPatterns BAD_FLYWEEL_BADPHYS = LEDPatterns.GOLD_STROBE; + + // Operator ready to shoot + public static final LEDPatterns OPREADY = LEDPatterns.SOLID_WHITE; + // Operator ready to shoot, but the driver conditions are bad + public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java new file mode 100644 index 0000000..9fa9146 --- /dev/null +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -0,0 +1,27 @@ +package frc4388.robot.constants; + +import java.util.Arrays; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; + +public final class FieldConstants { + // public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + public static final Translation2d BLUE_HUB_POS = new Translation2d(); + public static final Translation2d RED_HUB_POS = new Translation2d(); + + // Test april tag field layout + public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + Arrays.asList(new AprilTag[] { + new AprilTag(0, new Pose3d( + new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + )), + }), 100, 100); + +} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 6efd00b..6bb0ee9 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -44,6 +44,10 @@ public class LED extends SubsystemBase implements Queryable { this.mode = pattern; } + public String getMode(){ + return mode.name(); + } + @Override public void periodic() { update(); diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java new file mode 100644 index 0000000..c435c43 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/TestRobot.java @@ -0,0 +1,245 @@ +package frc4388.robot.subsystems; + +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; + +public class TestRobot extends SubsystemBase { + + // TalonFX m_intakeMotor; + // TalonFX m_armMotor; + // TalonFX m_storageMotor; + TalonFX m_outerShooter; + TalonFX m_innerShooter; + + ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0); + + + ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0); + ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0); + + ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0); + ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0); + ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0); + + VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0); + VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0); + + public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second + + public enum RobotStare { + IntakeDown, + Idle, + Shoot + } + + public RobotStare robotStare = RobotStare.Idle; + + + public TestRobot( + // TalonFX intakeMotor, + // TalonFX armMotor, + // TalonFX storageMotor, + TalonFX outerShooter, + TalonFX innerShooter + ) { + // m_intakeMotor = intakeMotor; + // m_armMotor = armMotor; + // m_storageMotor = storageMotor; + m_outerShooter = outerShooter; + m_innerShooter = innerShooter; + + // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); + // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG); + m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); + m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); + + m_outerShooter.getConfigurator().apply(SHOOTER_PID); + m_innerShooter.getConfigurator().apply(SHOOTER_PID); + + outerVelocity.Slot = 0; + innerVelocity.Slot = 0; + } + + // public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + + // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + + ); + + public static Slot0Configs SHOOTER_PID; + + static { + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = 0.12; + // SHOOTER_PID.kP = 0.11; + // SHOOTER_PID.kI = 0.48; + // SHOOTER_PID.kD = 0.01; + SHOOTER_PID.kP = 0.3; + SHOOTER_PID.kI = 0.0; + SHOOTER_PID.kD = 0.0; + } + + + ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12); + ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11); + ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48); + ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01); + + @Override + public void periodic() { + + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = kV.get(); + SHOOTER_PID.kP = kP.get(); + SHOOTER_PID.kI = kI.get(); + SHOOTER_PID.kD = kD.get(); + + m_outerShooter.getConfigurator().apply(SHOOTER_PID); + m_innerShooter.getConfigurator().apply(SHOOTER_PID); + + + SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond)); + SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond)); + + + switch (robotStare) { + case Idle: + + // Move the arm to the up position + PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get()); + // m_armMotor.setControl(armPosition); + + + // // Stop the intake motor + // m_intakeMotor.set(0); + + // // Stop the storage motor + // m_storageMotor.set(0); + + + // Stop the outer shooter motor + m_outerShooter.set(0); + // Stop the inner shooter motor + m_innerShooter.set(0); + + break; + case IntakeDown: + // Move the arm to the down sposition + PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get()); + // m_armMotor.setControl(armPosition1); + + // Move balls into the robot because the arm is down + VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get()); + // m_intakeMotor.setControl(intakeVelocity); + + // Move balls into the main box + VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get()); + // m_storageMotor.setControl(storageVelocity); + + + // Stop the outer shooter motor + m_outerShooter.set(0); + // Stop the inner shooter motor + m_innerShooter.set(0); + + break; + case Shoot: + + // Move the arm to the up position + PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get()); + // m_armMotor.setControl(armPosition2); + + // // Stop the intake motor + // m_intakeMotor.set(0); + + // // Move the balls into the + VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get()); + // m_storageMotor.setControl(storageVelocity1); + + // outerVelocity. + // m_outerShooter.setControl(outerVelocity); + double outerRPM = outerRate.get(); + m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); + + + + // m_innerShooter.setControl(innerVelocity); + // m_innerShooter.set(innerRate.get()); + + double innerRPM = innerRate.get(); + m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); + + + // SmartDashboard.putNumber("Test", outerRate.get()); + + break; + default: + break; + } + + + + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..78c06eb --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -0,0 +1,60 @@ +package frc4388.robot.subsystems.climber; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + ClimberIO io; + ClimberStateAutoLogged state = new ClimberStateAutoLogged(); + + // Supplier m_swervePoseSupplier; + + public Climber( + ClimberIO io + // Supplier swervePoseSupplier + ) { + this.io = io; + // this.m_swervePoseSupplier = swervePoseSupplier; + } + + // public enum ClimberMode { + // Up, + // Down, + // } + + // public void setMode(IntakeMode mode) { + // switch (mode) { + // case Up: + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + // io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); + // break; + // case Down: + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + // io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); + // break; + // } + // } + + + + @Override + public void periodic() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Climber", state); + + io.updateInputs(state); + + } +} + + diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..c0b5329 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,74 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import frc4388.utility.status.CanDevice; + +public class ClimberConstants { + // Motor conversions + + public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.; + + //IDs + + + // public static final CanDevice CLIMBER_ID = new CanDevice("SHOOTER 1", 20); + + // Limits + + // 0 is the forward angle on the robot. + // negative is left, positive is right + // public static final Angle L1 = Degrees.of(-180); + // public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); + public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0); + + public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0); + + + public static final Slot0Configs CLIMBER_PID = new Slot0Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.0); + + // 0 is paralell to the ground, 90 is directly up + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + + // Motor configs + public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} + + // public static final class IDs { + // public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); + // } + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..bddb5fc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,43 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Inches; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface ClimberIO { + @AutoLog + public class ClimberState { + Distance climberDistance = Inches.of(0); + Distance climberTargetDistance = Inches.of(0); + Current climberMotorCurrent = Amps.of(0); + + // Distance shooterPitch = Rotations.of(0); + // Angle shooterTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = Amps.of(0); + + // AngularVelocity rollerVelocity = RotationsPerSecond.of(0); + // AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); + // Current rollerMotorCurrent = Amps.of(0); + + // LinearVelocity feederVelocity = InchesPerSecond.of(0); + // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + // Current feederMotorCurrent = Amps.of(0); + } + + // public default void setShooterAngle(ShooterState state, Angle angle) {} + // public default void setShooterPitch(ShooterState state, Angle angle) {} + public default void setClimberDistance(ClimberState state, Distance distance) {} + + public default void updateInputs(ClimberState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java new file mode 100644 index 0000000..2345a08 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java @@ -0,0 +1,78 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.utility.configurable.ConfigurableDouble; + +public class ClimberReal implements ClimberIO { + + + TalonFX m_climberMotor; + + public ClimberReal( + + TalonFX climberMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_climberMotor = climberMotor; + // Apply the configs + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID); + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG); + + } + + private Distance clampDistance(Distance distance, Distance climberLimitLower, Distance climberLimitUpper){ + if(distance.gt(climberLimitUpper)) { + return climberLimitUpper; + }else if(distance.lt(climberLimitLower)) { + return climberLimitLower; + }else{ + return distance; + } + } + + + + + @Override + public void setClimberDistance(ClimberState state, Distance distance) { + state.climberTargetDistance = distance; + // Assume that the angle is always accurate, because I think we will use a shaft encoder + // Assume that 0 degrees = forwards. Might need an offset here + + Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER); + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; + PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance); + m_climberMotor.setControl(posRequest); + } + + @Override + public void updateInputs(ClimberState state) { + double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations); + double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; + state.climberDistance = Inches.of(linearInches); + state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue(); + + // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); + // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + + // state.armAngle = m_armMotor.getPosition().getValue(); + // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); + + + } +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..be507b3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -0,0 +1,84 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Intake extends SubsystemBase { + public IntakeIO io; + IntakeStateAutoLogged state = new IntakeStateAutoLogged(); + + Supplier m_swervePoseSupplier; + + public Intake( + IntakeIO io + // Supplier swervePoseSupplier + ) { + this.io = io; + // this.m_swervePoseSupplier = swervePoseSupplier; + } + + public enum IntakeMode { + Extended, + Retracted, + } + + private IntakeMode mode = IntakeMode.Extended; + + public void setMode(IntakeMode mode) { + this.mode = mode; + } + + public IntakeMode getMode() { + return mode; + } + + + // public enum FieldZone { + // // The robot should aim at the hub + // InShootZone, + // // The robot should aim towards the wall + // AimAtWall, + + // } + + // // Calculate what should be done based off of the position of the robot + // // TODO: Implement field zones + // public FieldZone getTarget(Pose2d position) { + // return FieldZone.InShootZone; + // } + + @Override + public void periodic() { + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Intake", state); + + io.updateInputs(state); + + switch (mode) { + case Extended: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); + break; + case Retracted: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(0)); + break; + } + + } +} + + diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..f2c3490 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,95 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.configurable.ConfigurableDouble; +import frc4388.utility.status.CanDevice; + +public class IntakeConstants { + // Motor conversions + + public static final double ARM_MOTOR_GEAR_RATIO = 100; + public static final double ROLLER_MOTOR_GEAR_RATIO = 3; + + + //IDs + + public static final CanDevice ARM_ID = new CanDevice("ARM", 20); + public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21); + + // Limits + + // 0 is the forward angle on the robot. + // negative is left, positive is right + + //when testing the negative output of 10% made the robot put the intake up + + // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); + // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); + + public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0); + + public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10); + // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); + + // public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); + // public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); + + + public static final Slot0Configs ARM_PID = new Slot0Configs() + .withKP(0.2) + .withKI(0.0) + .withKD(0.0); + + public static final Slot0Configs ROLLER_PID = new Slot0Configs() + .withKP(0.2) + .withKI(0.0) + .withKD(0.0); + + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); + public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); + public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0); + + public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); + public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); + public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); + + + // 0 is paralell to the ground, 90 is directly up + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + + // Motor configs + public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + + public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..b97ad76 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,43 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface IntakeIO { + @AutoLog + public class IntakeState { + Angle armAngle = Rotations.of(0); + Angle armTargetAngle = Rotations.of(0); + Current armMotorCurrent = Amps.of(0); + + // Angle shooterPitch = Rotations.of(0); + // Angle shooterTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = Amps.of(0); + + AngularVelocity rollerVelocity = RotationsPerSecond.of(0); + AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); + Current rollerMotorCurrent = Amps.of(0); + + // LinearVelocity feederVelocity = InchesPerSecond.of(0); + // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + // Current feederMotorCurrent = Amps.of(0); + } + + // public default void setShooterAngle(ShooterState state, Angle angle) {} + // public default void setShooterPitch(ShooterState state, Angle angle) {} + public default void setArmAngle(IntakeState state, Angle angle) {} + public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} + + public default void updateInputs(IntakeState state) {} + public default void updateGains() {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java new file mode 100644 index 0000000..547cc90 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -0,0 +1,112 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; + +public class IntakeReal implements IntakeIO { + + + TalonFX m_armMotor; + TalonFX m_rollerMotor; + + PositionDutyCycle armPosition = new PositionDutyCycle(0); + VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0); + + public IntakeReal( + + TalonFX armMotor, + TalonFX rollerMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_armMotor = armMotor; + m_rollerMotor = rollerMotor; + + // Apply the configs + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + + armPosition.Slot = 0; + rollerVelocity.Slot = 0; + } + + private Angle clampAng(Angle x, Angle min, Angle max){ + if(x.gt(max)) { + return max; + }else if(x.lt(min)) { + return min; + }else{ + return x; + } + } + + + + @Override + public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { + state.rollerTargetVelocity = angularVelocity; + + if(angularVelocity.baseUnitMagnitude() == 0) { + m_rollerMotor.set(0); + return; + } + + // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) + AngularVelocity motorSpeed = angularVelocity.times(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + + // m_rollerMotor.set(motorSpeed); + // VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed)); + } + + @Override + public void setArmAngle(IntakeState state, Angle angle) { + state.armTargetAngle = angle; + // Assume that the angle is always accurate, because I think we will use a shaft encoder + // Assume that 0 degrees = forwards. Might need an offset here + + // angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED); + + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + m_armMotor.setControl(armPosition.withPosition(motorAngle)); + } + + @Override + public void updateInputs(IntakeState state) { + state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); + + state.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + } + + @Override + public void updateGains() { + + IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); + IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); + IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get(); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); + + IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get(); + IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get(); + IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get(); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 60ddaec..44c9b2c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,28 +1,46 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; -import java.util.function.Supplier; +import java.text.FieldPosition; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.status.FaultReporter; +import frc4388.robot.constants.Constants; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.utility.compute.FieldPositions; public class Shooter extends SubsystemBase { - ShooterIO io; + public ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); - Supplier m_swervePoseSupplier; + SwerveDrive m_SwerveDrive; + Intake m_Intake; + LED m_robotLED; + + + // Supplier m_swervePoseSupplier; + public Shooter( ShooterIO io, - Supplier swervePoseSupplier + SwerveDrive swerveDrive, + Intake intake, + LED robotLED ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + this.m_SwerveDrive = swerveDrive; + this.m_Intake = intake; + this.m_robotLED = robotLED; + // this.m_swervePoseSupplier = swervePoseSupplier; } public enum FieldZone { @@ -33,26 +51,118 @@ public class Shooter extends SubsystemBase { } - // Calculate what should be done based off of the position of the robot - // TODO: Implement field zones - public FieldZone getTarget(Pose2d position) { - return FieldZone.InShootZone; + + public enum ShooterMode { + // Shooter is actively shooting + Shooting, + // Shooter is going to fire soon + Ready, + // Not ready to shoot + NotReady, } + private ShooterMode mode = ShooterMode.NotReady; + + public void setShooterReady() { + if(this.mode == ShooterMode.NotReady) { + this.mode = ShooterMode.Ready; + } + } + + public void setShooterNotReady() { + this.mode = ShooterMode.NotReady; + } + + @AutoLogOutput + public ShooterMode getMode() { + return mode; + } + + + @Override public void periodic() { - - - // FaultReporter.register(this); // TODO Implement fault reporter - Logger.processInputs("Shooter", state); - - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); + if(this.mode != ShooterMode.NotReady) { + + ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + + Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + + double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); + + + // TODO: get if the robot is within the angle of the hub + + boolean driverError = + XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | + AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | + distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + + double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + + boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); + boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + + + int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 4 : 0); + switch (bitmask) { + case 0b000: // No Errors + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + case 0b001: // No op err, yes driver err + m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); + break; + case 0b010: // Bad flywheel, no driver err + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + case 0b011: // Bad flywheel, yes driver err + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + break; + case 0b100: // Bad intake, no driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + break; + case 0b101: // Bad intake, yes driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + break; + case 0b110: // Bad intake and shooter (intake is more important), no driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + break; + case 0b111: // Bad intake and shooter (intake is more important), yes driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + break; + } + + // We set the shooter mode to ready if there are no errors + mode = ( + bitmask == 0 ? + ShooterMode.Ready : + ShooterMode.NotReady + ); + + } + + switch (mode) { + case Shooting: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get())); + break; + case Ready: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); + break; + case NotReady: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); + break; + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 59ce39b..108ef6e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,21 +1,74 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; -public class ShooterConstants { +public class ShooterConstants { // Motor conversions - // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.; - public static final double PITCH_MOTOR_GEAR_RATIO = 1.; - public static final double FLYWHEEL_GEAR_RATIO = 1.; + public static final double FEEDER_INCHES_PER_ROT = 1.; + public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; + public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; + public static final double INDEXER_GEAR_RATIO = 1.; + + // public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30); + // public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15); + // public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + + // public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10); + // public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + + public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); + public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); + + public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10); + public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); + + + // Tolerances + public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); + public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); + + public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); + + public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); + public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3); + + public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1); + + + + public static Slot0Configs SHOOTER_PID = new Slot0Configs() + .withKV(0.0) + .withKP(0.0) + .withKI(0.0) + .withKD(0.2); + + public static Slot0Configs INDEXER_PID = new Slot0Configs() + .withKV(0.0) + .withKP(0.0) + .withKI(0.0) + .withKD(0.2); + + + public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); + public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0); + public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0); + + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); + public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); + public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); // Limits @@ -25,8 +78,8 @@ public class ShooterConstants { // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); // 0 is paralell to the ground, 90 is directly up - public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); // Motor configs // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() @@ -40,21 +93,23 @@ public class ShooterConstants { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); - public static final class IDs { - public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); - } - public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); + public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); + public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); + + + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? .withStatorCurrentLimitEnable(true) ).withMotorOutput( new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -64,7 +119,8 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + + public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 999abc3..2e6402b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -24,19 +24,29 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); - AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); - Current flywheelMotorCurrent = Amps.of(0); + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0); - LinearVelocity feederVelocity = InchesPerSecond.of(0); - LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); - Current feederMotorCurrent = Amps.of(0); + AngularVelocity motor1Velocity = RotationsPerSecond.of(0); + AngularVelocity motor2Velocity = RotationsPerSecond.of(0); + AngularVelocity indexerVelocity = RotationsPerSecond.of(0); + + Current motor1Current = Amps.of(0); + Current motor2Current = Amps.of(0); + Current indexerCurrent = Amps.of(0); + + // LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); } // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} - public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {} + public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} + // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} public default void updateInputs(ShooterState state) {} + + + public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 9db0e8b..34dc2df 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,111 +1,108 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { - // TalonFX m_angleMotor; - // TalonFX m_pitchMotor; - TalonFX m_flywheelMotor; - TalonFX m_feederMotor; + TalonFX m_shooter1Motor; + TalonFX m_shooter2Motor; + TalonFX m_indexerMotor; + + VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0); + VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); + VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); + public ShooterReal( - // TalonFX angleMotor, - // TalonFX pitchMotor, - TalonFX flywheelMotor, - TalonFX feederMotor + TalonFX shooter1Motor, + TalonFX shooter2Motor, + TalonFX indexerMotor ) { - // m_angleMotor = angleMotor; - // m_pitchMotor = pitchMotor; - m_flywheelMotor = flywheelMotor; - m_feederMotor = feederMotor; + m_shooter1Motor = shooter1Motor; + m_shooter2Motor = shooter2Motor; + m_indexerMotor = indexerMotor; - // Apply the configs - // m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG); - // m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG); - m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG); - m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG); + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG); + + shooter1Velocity.Slot = 0; + shooter2Velocity.Slot = 0; + m_indexerVelocity.Slot = 0; } - - private Angle clampAng(Angle x, Angle min, Angle max){ - if(x.gt(max)) { - return max; - }else if(x.lt(min)) { - return min; - }else{ - return x; - } - } - - // // TODO: Test - // @Override - // public void setShooterAngle(ShooterState state, Angle angle) { - // state.shooterTargetAngle = angle; - // // Assume that the angle is always accurate, because I think we will use a shaft encoder - // // Assume that 0 degrees = forwards. Might need an offset here - - // Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_angleMotor.setControl(posRequest); - // } - - - // TODO: Test - // @Override - // public void setShooterPitch(ShooterState state, Angle angle) { - // state.shooterTargetPitch = angle; - // // TODO: Test - // // This assumes that the 0 is paralell to the ground. Might need an offset here - - - // Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_pitchMotor.setControl(posRequest); - // } @Override - public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) { - state.flywheelTargetVelocity = angularVelocity; - // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO; - VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velocity); + public void setShooterVelocity(ShooterState state, AngularVelocity target) { + state.motor1TargetVelocity = target; + state.motor2TargetVelocity = target; + + if(target.baseUnitMagnitude() == 0) { + m_shooter1Motor.set(0); + m_shooter2Motor.set(0); + return; + } + + AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); + + m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); + m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); } @Override - public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) { - state.feederTargetVelocity = linearVelocity; - // (IN / SEC) * (ROT / IN) = (ROT / SEC) - double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT; - VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velRequest); + public void setIndexerVelocity(ShooterState state, AngularVelocity target) { + state.indexerTargetVelocity = target; + + if(target.baseUnitMagnitude() == 0) { + m_indexerMotor.set(0); + return; + } + + AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); + m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } + @Override public void updateInputs(ShooterState state) { - // state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO); - // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); - state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); - state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); + // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); + + state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue(); + state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue(); + state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue(); - state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); - state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + } + + @Override + public void updateGains() { + // TEMPORARY PIDs + ShooterConstants.SHOOTER_PID.kP = ShooterConstants.shooter_kP.get(); + ShooterConstants.SHOOTER_PID.kI = ShooterConstants.shooter_kI.get(); + ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get(); + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + + ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get(); + ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get(); + ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get(); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID); } } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index a927fe6..511fb4e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -10,6 +10,12 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -19,15 +25,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; - -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; +import frc4388.utility.status.Status; public class SwerveDrive extends SubsystemBase implements Queryable { // private SwerveDrivetrain swerveDriveTrain; @@ -138,6 +138,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { initalPose2d = pose; io.resetPose(pose); } + // MIRA public void setOdoPose(Pose2d pose) { + // if (pose == null) return; + // io.tareEverything(); + // initalPose2d = pose; + // io.resetPose(pose); + // robotKnowsWhereItIs = true; + // rotTarget = pose.getRotation().getDegrees(); + // } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, // Translation2d rightStick){ @@ -251,13 +260,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { io.setControl(ctrl); } + // Drive with a specific velocity and heading public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) { if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve - - // if (leftStick.getNorm() < 0.05) // if no imput - // return; // don't bother doing swerve drive math and return early. - + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -268,13 +275,65 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD ); io.setControl(ctrl); // SmartDashboard.putBoolean("drift correction", true); - + } + + + + public void driveIntake(Translation2d leftStick, boolean invertRotation){ + // if (invert){ + // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + // driveFieldAngle(stick, heading); + + // } else{ + // driveFieldAngle(leftStick, heading); + // } + double speed = leftStick.getNorm(); + + if(speed < 0.3) { + driveWithInput(leftStick, new Translation2d(), true); + } else { + + + + Rotation2d heading = new Rotation2d(leftStick.getX(), -leftStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); + + // if (invertRotation){ + heading = heading.rotateBy(Rotation2d.fromDegrees(270)); + // } + + // Only drive forward in robot direction (no strafe) + // Translation2d forwardOnly = new Translation2d(speed, 0.0); + driveFieldAngle(leftStick, heading); + } + } + + + // Drive with the robot facing towards a specific position + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + + // Get the current speed of the robot + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + // Calculate a point to aim ahead of the actual position. + Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos); + + // Calculate the angle between the current position and the lead position + Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); + + + driveFieldAngle(leftStick, ang); } public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index f3bbe9f..4e1139f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -22,6 +22,8 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Angle; +import frc4388.robot.constants.Constants; +import frc4388.utility.configurable.ConfigurableDouble; //import edu.wpi.first.units.measure.measure.Distance; import frc4388.utility.status.CanDevice; import frc4388.utility.structs.Gains; @@ -37,6 +39,9 @@ public final class SwerveDriveConstants { public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + // TODO: Replace with a constant + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; @@ -75,7 +80,7 @@ public final class SwerveDriveConstants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -83,7 +88,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -91,7 +96,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -99,7 +104,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -156,6 +161,14 @@ public final class SwerveDriveConstants { public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); + + + + // TODO: Replace this with a static constant + public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 15); + public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0); + public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1); + // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1); } public static final class Configurations { @@ -196,7 +209,7 @@ public final class SwerveDriveConstants { } public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(IDs.DRIVE_PIGEON.id); + .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(Constants.CANIVORE_CANBUS.getName()); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() // holy verbosity batman. diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java index a069af7..9ab223e 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -2,15 +2,11 @@ package frc4388.robot.subsystems.vision; import java.util.ArrayList; import java.util.List; -import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,20 +14,22 @@ import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; import frc4388.utility.status.Status; +import frc4388.utility.structs.LEDPatterns; -public class Vision extends SubsystemBase implements Queryable { +public class Vision extends SubsystemBase implements Queryable{ VisionIO[] io; VisionStateAutoLogged[] state; public Pose2d lastVisionPose = new Pose2d(); public Pose2d lastPhysOdomPose = new Pose2d(); + public LED m_robotLED; public Vision(VisionIO... devices) { FaultReporter.register(this); io = devices; state = new VisionStateAutoLogged[io.length]; - + m_robotLED = new LED(); for(int i = 0; i < io.length; i++) { state[i] = new VisionStateAutoLogged(); } @@ -43,6 +41,11 @@ public class Vision extends SubsystemBase implements Queryable { io[i].updateInputs(state[i]); Logger.processInputs("Vision/Camera" + i , state[i]); } + Logger.recordOutput("Vision/isTagDectected", isTag()); + + if (isTag()){ + m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK); + } } public List getPosesToAdd(){ @@ -92,4 +95,10 @@ public class Vision extends SubsystemBase implements Queryable { // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); } + // Simple LED helper class for compilation and basic usage; replace with real implementation if available. + private static class LED { + public void setMode(LEDPatterns mode) { + // no-op stub for compilation; integrate with hardware driver as needed + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 1c12437..6f70542 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -8,7 +8,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; -import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; public class VisionReal implements VisionIO { @@ -45,12 +45,12 @@ public class VisionReal implements VisionIO { var result = results.get(results.size()-1); - state.isTagDetected = state.isTagDetected | result.hasTargets(); - // If there are no tags if(!result.hasTargets()) return; + state.isTagDetected = state.isTagDetected | result.hasTargets(); + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); // If the tag was failed to be processed diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java new file mode 100644 index 0000000..19457d8 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -0,0 +1,21 @@ +package frc4388.utility.compute; + +import edu.wpi.first.math.geometry.Translation2d; + +public class FieldPositions { + public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + + // We set the default position to one just in case it doesn't update + // Setting to null could cause a null pointer, and setting to (0,0) could cause problems + // I would rather have the 50/50 chance than a code error + public static Translation2d HUB_POSITION = RED_HUB_POSITION; + + public static void update() { + if(TimesNegativeOne.isRed) { + HUB_POSITION = RED_HUB_POSITION; + } else { + HUB_POSITION = BLUE_HUB_POSITION; + } + } +} diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java index 34c0290..69b144b 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableString.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -11,6 +11,7 @@ public class ConfigurableString { * @param name the name of the Smart Dashboard key. * @param defualtValue the initilization value */ + public ConfigurableString(String name, String defualtValue) { this.name = name; this.defualtValue = defualtValue; @@ -20,4 +21,5 @@ public class ConfigurableString { public String get() { return SmartDashboard.getString(name, defualtValue); } + } diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java index afd4dd4..f4066f8 100644 --- a/src/main/java/frc4388/utility/status/FaultReporter.java +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -85,13 +85,12 @@ public class FaultReporter { } } - // CAN header System.out.println(CAN_HEADER); - CANBus canBus = new CANBus(Constants.CANBUS_NAME); + // CANBus canBus = new CANBus(); - CANBusStatus canInfo = canBus.getStatus(); + CANBusStatus canInfo = Constants.RIO_CANBUS.getStatus(); System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount); System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 7b78472..9699738 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.0.1-beta" + "version": "v2025.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.0.1-beta" + "version": "v2025.3.1" } ] } \ No newline at end of file