diff --git a/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto b/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto deleted file mode 100644 index 633ebd4..0000000 --- a/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto +++ /dev/null @@ -1,94 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1 Depot side center" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "2 Center back depot" - } - }, - { - "type": "named", - "data": { - "name": "rev shooter" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "hopper retract" - } - }, - { - "type": "named", - "data": { - "name": "shoot long" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "path", - "data": { - "pathName": "3 to middle depot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center to back depot side.auto b/src/main/deploy/pathplanner/autos/center to back depot side.auto deleted file mode 100644 index c136d35..0000000 --- a/src/main/deploy/pathplanner/autos/center to back depot side.auto +++ /dev/null @@ -1,107 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1 Depot side center" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "2 Center back depot" - } - }, - { - "type": "named", - "data": { - "name": "rev shooter" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "hopper retract" - } - }, - { - "type": "named", - "data": { - "name": "shoot long" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "path", - "data": { - "pathName": "3 to middle depot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1 Depot side center.path b/src/main/deploy/pathplanner/paths/1 Depot side center.path index d07f9ee..08c5412 100644 --- a/src/main/deploy/pathplanner/paths/1 Depot side center.path +++ b/src/main/deploy/pathplanner/paths/1 Depot side center.path @@ -3,37 +3,32 @@ "waypoints": [ { "anchor": { - "x": 4.489717954282408, - "y": 7.530401114004629 + "x": 4.139596586501166, + "y": 7.549712955779675 }, "prevControl": null, "nextControl": { - "x": 6.6936413483796295, - "y": 7.439924189814815 + "x": 6.343519980598391, + "y": 7.459236031589861 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.753707692307692, - "y": 5.793184615384616 + "x": 7.87243987587277, + "y": 4.4607059736229635 }, "prevControl": { - "x": 7.802546153846153, - "y": 7.760676923076924 + "x": 7.921695112490301, + "y": 7.2964003103180755 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6025082236842106, - "rotationDegrees": -58.149754025069136 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -47,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -86.47765706868994 + "rotation": -93.09405805891707 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2 Center back depot.path b/src/main/deploy/pathplanner/paths/2 Center back depot.path index f146a1c..0b764c1 100644 --- a/src/main/deploy/pathplanner/paths/2 Center back depot.path +++ b/src/main/deploy/pathplanner/paths/2 Center back depot.path @@ -3,32 +3,41 @@ "waypoints": [ { "anchor": { - "x": 7.649053846153848, - "y": 5.96063076923077 + "x": 8.012834586466164, + "y": 4.447124060150376 }, "prevControl": null, "nextControl": { - "x": 7.537423076923078, - "y": 7.593230769230768 + "x": 8.09999176373828, + "y": 7.520768188897378 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9102779260481375, - "y": 7.456715595885093 + "x": 3.3376992481203005, + "y": 7.341883458646617 }, "prevControl": { - "x": 4.350690457723854, - "y": 7.556749418150089 + "x": 7.9468947368421095, + "y": 7.678176691729323 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -5.220589140086717 + }, + { + "waypointRelativePos": 0.9506024096385572, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +51,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -81.599031837346 + "rotation": 122.76203131319906 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -90.07765154134242 + "rotation": -87.20729763428666 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 deopt side.path b/src/main/deploy/pathplanner/paths/3 deopt side.path new file mode 100644 index 0000000..32b5e00 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 deopt side.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5394806134259262, + "y": 7.305488208912037 + }, + "prevControl": null, + "nextControl": { + "x": 6.734254267939816, + "y": 7.836736689814815 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.227675347222223, + "y": 4.233865523726852 + }, + "prevControl": { + "x": 6.016475043402778, + "y": 7.055512080439814 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6592348421926909, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.50938390003228 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -61.449999218114584 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 to middle depot.path b/src/main/deploy/pathplanner/paths/3 to middle depot.path index 0f58d28..ea4ce7e 100644 --- a/src/main/deploy/pathplanner/paths/3 to middle depot.path +++ b/src/main/deploy/pathplanner/paths/3 to middle depot.path @@ -3,32 +3,37 @@ "waypoints": [ { "anchor": { - "x": 4.027098090277778, - "y": 7.411908492476852 + "x": 3.3640751879699247, + "y": 7.308913533834587 }, "prevControl": null, "nextControl": { - "x": 7.8697757523148155, - "y": 7.872494429976852 + "x": 8.507383458646615, + "y": 7.658394736842105 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.4424543547453705, - "y": 5.424040581597223 + "x": 8.296375939849625, + "y": 5.060364661654136 }, "prevControl": { - "x": 7.478799149635494, - "y": 5.17669657846718 + "x": 8.42166165413534, + "y": 7.618830827067669 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -91.90233699047563 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -61.20160855081938 + "rotation": -89.22898482159874 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -69.91006596725617 + "rotation": 114.14063152070928 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 BACK.path b/src/main/deploy/pathplanner/paths/4 BACK.path new file mode 100644 index 0000000..cbb4879 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 BACK.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.939126085069445, + "y": 4.265883463541667 + }, + "prevControl": null, + "nextControl": { + "x": 7.928731574864237, + "y": 7.929681923972071 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9148384693287035, + "y": 7.351153139467592 + }, + "prevControl": { + "x": 8.139825446082234, + "y": 7.394910783553141 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.0990137586245 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -97.36924253129777 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 93db8a3..c5c65fd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -30,6 +31,7 @@ public class Robot extends TimedRobot { private final CommandSwerveDrivetrain drivetrain; StructPublisher publisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish(); + Field2d field = new Field2d(); public Robot() { m_robotContainer = new RobotContainer(); @@ -47,12 +49,15 @@ public void robotPeriodic() { SmartDashboard.putString("Hopper State", Hopper.hopperState); SmartDashboard.putString("Intake State", Intake.intakeState); SmartDashboard.putString("Kicker State", KickerCommand.kickerState); + SmartDashboard.putNumber("Gyro Yaw", drivetrain.getPigeon2().getRotation2d().getDegrees()); SmartDashboard.putBoolean( "Flywheel On", !(FlywheelCommand.flywheelState.equals("none") || FlywheelCommand.flywheelState.equals("Stopped"))); robotPose = drivetrain.getState().Pose; publisher.set(robotPose); + field.setRobotPose(robotPose); + SmartDashboard.putData("Field", field); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca31d9c..7a410ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; @@ -18,6 +19,7 @@ import frc.robot.config.HopperConfig; import frc.robot.config.IntakeConfig; import frc.robot.config.TunerConstants; +import frc.robot.helpers.ShotCalculator; import frc.robot.subsystems.*; @Logged @@ -28,8 +30,10 @@ public class RobotContainer { private Intake intake = new Intake(); private Kicker kicker = new Kicker(); private CommandXboxController controller = new CommandXboxController(1); + private CommandXboxController operator = new CommandXboxController(2); private final SendableChooser autoChooser; public static boolean isExtended = false; + private SwerveDrivetrain.SwerveDriveState driveState = drivetrain.getState(); private Command shootGroup = Commands.parallel(new KickerCommand(kicker, KickerCommand.Position.INTAKE)) @@ -92,8 +96,8 @@ public RobotContainer() { NamedCommands.registerCommand( "rev shooter", - new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain) - .withDeadline(Commands.waitSeconds(1))); + new FlywheelCommand(flywheel, FlywheelCommand.Position.DEPOT_SHOT, drivetrain) + .withDeadline(Commands.waitSeconds(3))); NamedCommands.registerCommand( "shoot long", @@ -101,7 +105,15 @@ public RobotContainer() { new KickerCommand(kicker, KickerCommand.Position.INTAKE), new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain)) - .withDeadline(Commands.waitSeconds(8))); + .withDeadline(Commands.waitSeconds(5))); + + NamedCommands.registerCommand( + "shoot depot", + Commands.parallel( + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + new FlywheelCommand(flywheel, FlywheelCommand.Position.DEPOT_SHOT, drivetrain)) + .withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "short shoot", @@ -176,21 +188,19 @@ private void configureBindings() { .povRight() .and(() -> !shootGroup.isScheduled()) .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE)); - // Right Stick Down = Extend/Retract Hopper + // Right Stick Down = Retract Hopper controller .rightStick() .and(() -> !shootGroup.isScheduled()) .onTrue( - Commands.runEnd( - () -> hopper.move(Hopper.getRotation(isExtended)), () -> hopper.stop(), hopper) - .withDeadline(Commands.waitSeconds(2)) - .andThen(Commands.runOnce(() -> isExtended = !isExtended))); + Commands.run(() -> hopper.move(HopperConfig.HOPPER_RETRACT_ROTATION), hopper) + .withDeadline(Commands.waitSeconds(2))); // Back button = Zero Pigeon - controller.back().onTrue(Commands.runOnce(drivetrain::seedFieldCentric)); - // Right Bumper = Flywheel Toggle for hub shot speeds + controller.back().onTrue(Commands.runOnce(drivetrain::zeroPigeon)); + // Right Bumper = Flywheel Toggle for pass controller .rightBumper() - .toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain)); + .toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.PASS, drivetrain)); // Left Bumper = Flywheel Toggle for trench shot speeds controller .leftBumper() @@ -203,19 +213,35 @@ private void configureBindings() { new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain)); // Down Plus = Zero hopper controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper)); - // B = Shoot on the move toggle - placeholder button + // Intake Extend Shoot controller .b() .toggleOnTrue( Commands.parallel( - new DrivetrainCommand( - drivetrain, - DrivetrainCommand.Position.SOTM, - controller::getLeftX, - controller::getLeftY, - controller::getRightX), - new FlywheelCommand( - flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain))); + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + Commands.run(() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION)))); + // Hopper Extend + controller + .a() + .onTrue( + Commands.run(() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION), hopper) + .withDeadline(Commands.waitSeconds(2))); + /* Operator */ + operator + .leftBumper() + .onTrue( + Commands.runOnce( + () -> drivetrain.setPose(ShotCalculator.calculateLeftCornerRobotPosition()))); + operator + .rightBumper() + .onTrue( + Commands.runOnce( + () -> drivetrain.setPose(ShotCalculator.calculateRightCornerRobotPosition()))); + operator + .y() + .onTrue( + Commands.runOnce(() -> drivetrain.setPose(ShotCalculator.calculateHubRobotPosition()))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index e1ac82c..2b49c5a 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -70,7 +70,7 @@ public DrivetrainCommand( this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; - double alignP = 75; + double alignP = 60; double alignI = 0; double alignD = 10; this.autoAlignPidController = diff --git a/src/main/java/frc/robot/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index 3198ba2..dd0dfe1 100644 --- a/src/main/java/frc/robot/commands/FlywheelCommand.java +++ b/src/main/java/frc/robot/commands/FlywheelCommand.java @@ -13,9 +13,11 @@ public static enum Position { HUB_SHOT, TOWER_SHOT, TRENCH_SHOT, + DEPOT_SHOT, CALCULATED_SHOT, DEFAULT_SHOT, - OUTTAKE + OUTTAKE, + PASS } private Flywheel subsystem; @@ -40,6 +42,11 @@ public void execute() { flywheelState = "Hub Shot"; break; + case PASS: + subsystem.shoot(FlywheelConfig.FLYWHEEL_PASS_SPEED); + flywheelState = "Pass"; + break; + // Spins flywheels at proper speed for shooting from tower case TOWER_SHOT: subsystem.shoot(FlywheelConfig.FLYWHEEL_TOWER_SHOT_SPEED); @@ -51,6 +58,11 @@ public void execute() { flywheelState = "Trench Shot"; break; + case DEPOT_SHOT: + subsystem.shoot(FlywheelConfig.FLYWHEEL_DEPOT_SHOT_SPEED); + flywheelState = "Trench Shot"; + break; + // Spins flywheels at estimated speed given distance from hub case CALCULATED_SHOT: flywheelState = "Calculated Shot"; diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index 5659ca6..4c23b3d 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -3,13 +3,13 @@ public class CANMappings { public static final int HOPPER_MOTOR_ID = 1; - public static final int KICKER_FRONT_MOTOR_ID = 15; // Bottom - public static final int KICKER_BACK_MOTOR_ID = 16; + public static final int KICKER_RIGHT_MOTOR_ID = 16; // Bottom + public static final int KICKER_LEFT_MOTOR_ID = 2; - public static final int INTAKE_MOTOR_ID = 3; + public static final int INTAKE_MOTOR_ID = 19; - public static final int FLYWHEEL_RIGHT_MOTOR_ID = 19; - public static final int FLYWHEEL_LEFT_MOTOR_ID = 2; + public static final int FLYWHEEL_RIGHT_MOTOR_ID = 15; + public static final int FLYWHEEL_LEFT_MOTOR_ID = 3; public static final int PIGEON_CAN_ID = 0; } diff --git a/src/main/java/frc/robot/config/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index 834d2fc..6b81429 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -1,40 +1,42 @@ package frc.robot.config; public class FlywheelConfig { - public static final double FLYWHEEL_RIGHT_SUPPLY_CURRENT_LIMIT = 50; - public static final double FLYWHEEL_RIGHT_STATOR_CURRENT_LIMIT = 60; + public static final double FLYWHEEL_RIGHT_SUPPLY_CURRENT_LIMIT = 35; + public static final double FLYWHEEL_RIGHT_STATOR_CURRENT_LIMIT = 45; public static final double FLYWHEEL_RIGHT_GEAR_RATIO = 1; - public static final double FLYWHEEL_LEFT_SUPPLY_CURRENT_LIMIT = 50; - public static final double FLYWHEEL_LEFT_STATOR_CURRENT_LIMIT = 60; + public static final double FLYWHEEL_LEFT_SUPPLY_CURRENT_LIMIT = 35; + public static final double FLYWHEEL_LEFT_STATOR_CURRENT_LIMIT = 45; public static final double FLYWHEEL_LEFT_GEAR_RATIO = 1; // Set public static final double FLYWHEEL_RIGHT_MAX_CRUISE_VELOCITY = 3000; public static final double FLYWHEEL_RIGHT_TARGET_ACCELERATION = 500; - public static final double FLYWHEEL_RIGHT_P = 0.5; + public static final double FLYWHEEL_RIGHT_P = 0.25; public static final double FLYWHEEL_RIGHT_I = 0; public static final double FLYWHEEL_RIGHT_D = 0; - public static final double FLYWHEEL_RIGHT_S = 0.18; - public static final double FLYWHEEL_RIGHT_V = 0.115; + public static final double FLYWHEEL_RIGHT_S = 0.4; + public static final double FLYWHEEL_RIGHT_V = 0.123; public static final double FLYWHEEL_RIGHT_A = 0; // Set public static final double FLYWHEEL_LEFT_MAX_CRUISE_VELOCITY = 3000; public static final double FLYWHEEL_LEFT_TARGET_ACCELERATION = 500; - public static final double FLYWHEEL_LEFT_P = 0.5; + public static final double FLYWHEEL_LEFT_P = 0.25; public static final double FLYWHEEL_LEFT_I = 0; public static final double FLYWHEEL_LEFT_D = 0; - public static final double FLYWHEEL_LEFT_S = 0.18; - public static final double FLYWHEEL_LEFT_V = 0.115; + public static final double FLYWHEEL_LEFT_S = 0.4; + public static final double FLYWHEEL_LEFT_V = 0.123; public static final double FLYWHEEL_LEFT_A = 0; // Set - public static final double FLYWHEEL_HUB_SHOT_SPEED = 43; - public static final double FLYWHEEL_TOWER_SHOT_SPEED = 65; - public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 67; + public static final double FLYWHEEL_HUB_SHOT_SPEED = 30; + public static final double FLYWHEEL_TOWER_SHOT_SPEED = 20; + public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 30; + public static final double FLYWHEEL_PASS_SPEED = 60; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40; public static final double FLYWHEEL_TOLERANCE = 5; + public static final double FLYWHEEL_DEPOT_SHOT_SPEED = 37; } diff --git a/src/main/java/frc/robot/config/HopperConfig.java b/src/main/java/frc/robot/config/HopperConfig.java index 2a1e41f..cc0ece1 100644 --- a/src/main/java/frc/robot/config/HopperConfig.java +++ b/src/main/java/frc/robot/config/HopperConfig.java @@ -22,7 +22,7 @@ public class HopperConfig { public static final double HOPPER_TOLERANCE = 0.2; // Set - public static final double HOPPER_EXTEND_ROTATION = -17; + public static final double HOPPER_EXTEND_ROTATION = -16.729; public static final double HOPPER_RETRACT_ROTATION = -0.1; public static final double HOPPER_RETRACT_FOR_NEUTRAL_MODE_ROTATION = 1.752; } diff --git a/src/main/java/frc/robot/config/KickerConfig.java b/src/main/java/frc/robot/config/KickerConfig.java index 470957f..47725c4 100644 --- a/src/main/java/frc/robot/config/KickerConfig.java +++ b/src/main/java/frc/robot/config/KickerConfig.java @@ -1,12 +1,12 @@ package frc.robot.config; public class KickerConfig { - public static final double KICKER_FRONT_STATOR_CURRENT_LIMIT = 60; - public static final double KICKER_FRONT_SUPPLY_CURRENT_LIMIT = 50; - public static final double KICKER_FRONT_GEAR_RATIO = 3; - public static final double KICKER_BACK_STATOR_CURRENT_LIMIT = 60; - public static final double KICKER_BACK_SUPPLY_CURRENT_LIMIT = 50; - public static final double KICKER_BACK_GEAR_RATIO = 3; + public static final double KICKER_RIGHT_STATOR_CURRENT_LIMIT = 60; + public static final double KICKER_RIGHT_SUPPLY_CURRENT_LIMIT = 50; + public static final double KICKER_RIGHT_GEAR_RATIO = 3; + public static final double KICKER_LEFT_STATOR_CURRENT_LIMIT = 60; + public static final double KICKER_LEFT_SUPPLY_CURRENT_LIMIT = 50; + public static final double KICKER_LEFT_GEAR_RATIO = 3; // Set public static final double KICKER_P = 0; @@ -16,12 +16,12 @@ public class KickerConfig { public static final double KICKER_V = 0; public static final double KICKER_A = 0; - public static final double KICKER_INTAKE_SPEED = 11; - public static final double KICKER_OUTTAKE_SPEED = 11; + public static final double KICKER_INTAKE_SPEED = 6; + public static final double KICKER_OUTTAKE_SPEED = 6; - public static final double KICKER_FRONT_INTAKE_SPEED = 0; - public static final double KICKER_FRONT_OUTTAKE_SPEED = 0; + public static final double KICKER_RIGHT_INTAKE_SPEED = 0; + public static final double KICKER_RIGHT_OUTTAKE_SPEED = 0; - public static final double KICKER_BACK_INTAKE_SPEED = 0; - public static final double KICKER_BACK_OUTTAKE_SPEED = 0; + public static final double KICKER_LEFT_INTAKE_SPEED = 0; + public static final double KICKER_LEFT_OUTTAKE_SPEED = 0; } diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 3c37a44..9feb12e 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -59,7 +59,15 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(55)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(55)) + .withSupplyCurrentLimitEnable(true)); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -68,7 +76,9 @@ public class TunerConstants { // low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(20)) // from 60 - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(55)) + .withSupplyCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index b6f85b9..73de801 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -1,5 +1,6 @@ package frc.robot.helpers; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; @@ -48,13 +49,66 @@ public static Translation2d calculateHubPosition() { return new Translation2d(Units.inchesToMeters(182.11), Units.inchesToMeters(158.84)); } + public static Pose2d calculateLeftCornerRobotPosition() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return new Pose2d(new Translation2d(0.6096, 7.5692), Rotation2d.k180deg); + } else if (alliance.get() == DriverStation.Alliance.Red) { + return new Pose2d(new Translation2d(15.9258, 0.4826), Rotation2d.kZero); + } + } + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.k180deg); + } + + public static Pose2d calculateRightCornerRobotPosition() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.k180deg); + } else if (alliance.get() == DriverStation.Alliance.Red) { + return new Pose2d(new Translation2d(15.9258, 7.5692), Rotation2d.kZero); + } + } + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.k180deg); + } + + public static Pose2d calculateHubRobotPosition() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return new Pose2d( + new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), + Rotation2d.k180deg); + } else if (alliance.get() == DriverStation.Alliance.Red) { + return new Pose2d( + new Translation2d(Units.inchesToMeters(461.84), (0.4826 + 7.5692) / 2), + Rotation2d.kZero); + } + } + return new Pose2d( + new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), Rotation2d.k180deg); + } + + public static Rotation2d calculatePigeonZero() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return Rotation2d.kZero; + } else if (alliance.get() == DriverStation.Alliance.Red) { + return Rotation2d.k180deg; + } + } + return Rotation2d.kZero; + } + public static Rotation2d getRotationTowardsHub(Translation2d hubpose, Translation2d currentpose) { double deltaX = hubpose.getX() - currentpose.getX(); double deltaY = hubpose.getY() - currentpose.getY(); double angleRadians = Math.atan2(deltaY, deltaX); - return new Rotation2d(angleRadians); + return new Rotation2d(angleRadians + Math.PI); } // Determines active based on current match time diff --git a/src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java new file mode 100644 index 0000000..3725d82 --- /dev/null +++ b/src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java @@ -0,0 +1,82 @@ +package frc.robot.loggers; + +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.RobotController; + +@CustomLoggerFor(SwerveDrivetrain.class) +public class SwerveDrivetrainLogger extends ClassSpecificLogger { + public SwerveDrivetrainLogger() { + super(SwerveDrivetrain.class); + } + + String[] moduleNames = {"FL", "FR", "BL", "BR"}; + + @Override + protected void update(EpilogueBackend backend, SwerveDrivetrain drivetrain) { + backend.log("Pose", drivetrain.getState().Pose, Pose2d.struct); + backend.log("Speeds", drivetrain.getState().Speeds, ChassisSpeeds.struct); + backend.log("ModuleStates", drivetrain.getState().ModuleStates, SwerveModuleState.struct); + backend.log("ModuleTargets", drivetrain.getState().ModuleTargets, SwerveModuleState.struct); + backend.log( + "ModulePositions", drivetrain.getState().ModulePositions, SwerveModulePosition.struct); + backend.log("RawHeading", drivetrain.getState().RawHeading, Rotation2d.struct); + backend.log("Timestamp", drivetrain.getState().Timestamp); + backend.log("OdometryPeriod", drivetrain.getState().OdometryPeriod); + backend.log("SuccessfulDaqs", drivetrain.getState().SuccessfulDaqs); + backend.log("FailedDaqs", drivetrain.getState().FailedDaqs); + backend.log("Pigeon Yaw", drivetrain.getPigeon2().getYaw().getValue()); + backend.log( + "Pigeon Angular Velocity Z World", + drivetrain.getPigeon2().getAngularVelocityZWorld().getValue()); + backend.log("Pigeon Supply Voltage", drivetrain.getPigeon2().getSupplyVoltage().getValue()); + backend.log( + "Operator Forward Direction", drivetrain.getOperatorForwardDirection(), Rotation2d.struct); + backend.log("Battery Voltage", RobotController.getBatteryVoltage()); + var modules = drivetrain.getModules(); + + for (int i = 0; i < modules.length; i++) { + var module = modules[i]; + backend.log( + moduleNames[i] + " Drive Supply Current", + module.getDriveMotor().getSupplyCurrent().getValue()); + backend.log( + moduleNames[i] + " Steer Supply Current", + module.getSteerMotor().getSupplyCurrent().getValue()); + backend.log( + moduleNames[i] + " Drive Stator Current", + module.getDriveMotor().getStatorCurrent().getValue()); + backend.log( + moduleNames[i] + " Steer Stator Current", + module.getSteerMotor().getStatorCurrent().getValue()); + backend.log( + moduleNames[i] + " Drive Supply Voltage", + module.getDriveMotor().getSupplyVoltage().getValue()); + backend.log( + moduleNames[i] + " Steer Supply Voltage", + module.getSteerMotor().getSupplyVoltage().getValue()); + backend.log( + moduleNames[i] + " Drive Closed Loop Error", + module.getDriveMotor().getClosedLoopError().getValue()); + backend.log( + moduleNames[i] + " Steer Closed Loop Error", + module.getSteerMotor().getClosedLoopError().getValue()); + } + + double totalDriveSupply = 0; + double totalSteerSupply = 0; + + for (var module : modules) { + totalDriveSupply += module.getDriveMotor().getSupplyCurrent().getValueAsDouble(); + totalSteerSupply += module.getSteerMotor().getSupplyCurrent().getValueAsDouble(); + } + backend.log("Total Supply Current", totalDriveSupply + totalSteerSupply); + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9fa0962..b386970 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.config.TunerConstants.TunerSwerveDrivetrain; import frc.robot.config.VisionConfig; +import frc.robot.helpers.ShotCalculator; import java.util.Optional; import java.util.function.Supplier; import org.photonvision.PhotonPoseEstimator; @@ -301,58 +302,58 @@ public void periodic() { }); } // 1. Get all results since the last loop and apply if estimator is available - if (m_FrontPhotonPoseEstimator != null) { - var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); + // if (m_FrontPhotonPoseEstimator != null) { + // var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); + // + // for (var result : frontResults) { + // // 2. Use the 2026 explicit methods to calculate pose + // var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + // + // // Fallback to single tag if multi-tag isn't available + // if (visionEst.isEmpty()) { + // var bestTarget = result.getBestTarget(); + // if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { + // visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); + // } + // } - for (var result : frontResults) { - // 2. Use the 2026 explicit methods to calculate pose - var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + // // 3. Apply the successful estimation to the CTRE odometry and log to file + // visionEst.ifPresent( + // est -> { + // Pose2d pose = est.estimatedPose.toPose2d(); + // double ts = est.timestampSeconds; + // SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); + // SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); + // addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); + // }); + // } + // } + // if (m_RearPhotonPoseEstimator != null) { + // var rearResults = Vision.RearCameraApril.getAllUnreadResults(); + // + // for (var result : rearResults) { + // // 2. Use the 2026 explicit methods to calculate pose + // var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + // + // // Fallback to single tag if multi-tag isn't available + // if (visionEst.isEmpty()) { + // var bestTarget = result.getBestTarget(); + // if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { + // visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); + // } + // } - // Fallback to single tag if multi-tag isn't available - if (visionEst.isEmpty()) { - var bestTarget = result.getBestTarget(); - if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { - visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); - } - } - - // 3. Apply the successful estimation to the CTRE odometry and log to file - visionEst.ifPresent( - est -> { - Pose2d pose = est.estimatedPose.toPose2d(); - double ts = est.timestampSeconds; - SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); - SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); - addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); - }); - } - } - if (m_RearPhotonPoseEstimator != null) { - var rearResults = Vision.RearCameraApril.getAllUnreadResults(); - - for (var result : rearResults) { - // 2. Use the 2026 explicit methods to calculate pose - var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); - - // Fallback to single tag if multi-tag isn't available - if (visionEst.isEmpty()) { - var bestTarget = result.getBestTarget(); - if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { - visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); - } - } - - // 3. Apply the successful estimation to the CTRE odometry and log to file - visionEst.ifPresent( - est -> { - Pose2d pose = est.estimatedPose.toPose2d(); - double ts = est.timestampSeconds; - SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); - SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); - addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); - }); - } - } + // // 3. Apply the successful estimation to the CTRE odometry and log to file + // visionEst.ifPresent( + // est -> { + // Pose2d pose = est.estimatedPose.toPose2d(); + // double ts = est.timestampSeconds; + // SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); + // SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); + // addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); + // }); + // } + // } } private void startSimThread() { @@ -416,4 +417,13 @@ public void addVisionMeasurement( public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } + + public void zeroPigeon() { + this.resetRotation(ShotCalculator.calculatePigeonZero()); + } + + public void setPose(Pose2d pose) { + this.resetPose(pose); + } + ; } diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index 9c8d2e6..034b2f8 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -1,8 +1,8 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -76,14 +76,14 @@ public Flywheel() { public void shoot(double speed) { speed = Math.abs(speed); - flywheelRight.setControl(new MotionMagicVelocityVoltage(speed)); - flywheelLeft.setControl(new MotionMagicVelocityVoltage(-speed)); + flywheelRight.setControl(new MotionMagicVelocityVoltage(-speed)); + flywheelLeft.setControl(new MotionMagicVelocityVoltage(speed)); } public void outtake(double speed) { speed = Math.abs(speed); - flywheelRight.setControl(new DutyCycleOut(-speed)); - flywheelLeft.setControl(new DutyCycleOut(speed)); + flywheelRight.setControl(new VoltageOut(speed)); + flywheelLeft.setControl(new VoltageOut(-speed)); } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 293c6fb..8f043e9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,12 +13,21 @@ @Logged public class Intake extends SubsystemBase { protected TalonFX intake; + // protected TalonFX swerveDrive; + // protected TalonFX swerveSteer; public static String intakeState = "Stopped"; public Intake() { intake = new TalonFX(CANMappings.INTAKE_MOTOR_ID); TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); + // swerveDrive = new TalonFX(31); + // swerveSteer = new TalonFX(30); + // TalonFXConfiguration swerveConfig = new TalonFXConfiguration(); + // swerveConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + // swerveDrive.getConfigurator().apply(swerveConfig); + // swerveSteer.getConfigurator().apply(swerveConfig); + intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker.java index e9c97b5..e4072e5 100644 --- a/src/main/java/frc/robot/subsystems/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker.java @@ -1,8 +1,6 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -14,88 +12,68 @@ @Logged public class Kicker extends SubsystemBase { - protected TalonFX kickerFront; - protected TalonFX kickerBack; + protected TalonFX kickerRight; + protected TalonFX kickerLeft; public Kicker() { - kickerFront = new TalonFX(CANMappings.KICKER_FRONT_MOTOR_ID); - kickerBack = new TalonFX(CANMappings.KICKER_BACK_MOTOR_ID); - TalonFXConfiguration kickerFrontConfig = new TalonFXConfiguration(); - TalonFXConfiguration kickerBackConfig = new TalonFXConfiguration(); + kickerRight = new TalonFX(CANMappings.KICKER_RIGHT_MOTOR_ID); + kickerLeft = new TalonFX(CANMappings.KICKER_LEFT_MOTOR_ID); + TalonFXConfiguration kickerRightConfig = new TalonFXConfiguration(); + TalonFXConfiguration kickerLeftConfig = new TalonFXConfiguration(); - kickerFrontConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - kickerFrontConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kickerBackConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - kickerBackConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kickerRightConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kickerRightConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kickerLeftConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kickerLeftConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kickerFrontConfig.CurrentLimits.SupplyCurrentLimit = - KickerConfig.KICKER_FRONT_SUPPLY_CURRENT_LIMIT; - kickerFrontConfig.CurrentLimits.StatorCurrentLimit = - KickerConfig.KICKER_FRONT_STATOR_CURRENT_LIMIT; - kickerBackConfig.CurrentLimits.SupplyCurrentLimit = - KickerConfig.KICKER_BACK_SUPPLY_CURRENT_LIMIT; - kickerBackConfig.CurrentLimits.StatorCurrentLimit = - KickerConfig.KICKER_BACK_STATOR_CURRENT_LIMIT; + kickerRightConfig.CurrentLimits.SupplyCurrentLimit = + KickerConfig.KICKER_RIGHT_SUPPLY_CURRENT_LIMIT; + kickerRightConfig.CurrentLimits.StatorCurrentLimit = + KickerConfig.KICKER_RIGHT_STATOR_CURRENT_LIMIT; + kickerLeftConfig.CurrentLimits.SupplyCurrentLimit = + KickerConfig.KICKER_LEFT_SUPPLY_CURRENT_LIMIT; + kickerLeftConfig.CurrentLimits.StatorCurrentLimit = + KickerConfig.KICKER_LEFT_STATOR_CURRENT_LIMIT; - kickerFrontConfig.Slot0.kP = KickerConfig.KICKER_P; - kickerFrontConfig.Slot0.kI = KickerConfig.KICKER_I; - kickerFrontConfig.Slot0.kD = KickerConfig.KICKER_D; - kickerFrontConfig.Slot0.kS = KickerConfig.KICKER_S; - kickerFrontConfig.Slot0.kV = KickerConfig.KICKER_V; - kickerFrontConfig.Slot0.kA = KickerConfig.KICKER_A; + kickerRightConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_RIGHT_GEAR_RATIO; + kickerRightConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + kickerRightConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kickerLeftConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_LEFT_GEAR_RATIO; + kickerLeftConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + kickerLeftConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - kickerFrontConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_FRONT_GEAR_RATIO; - kickerFrontConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - kickerFrontConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - kickerBackConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_BACK_GEAR_RATIO; - kickerBackConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - kickerBackConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - kickerFront.getConfigurator().apply(kickerFrontConfig); - kickerBack.getConfigurator().apply(kickerBackConfig); + kickerRight.getConfigurator().apply(kickerRightConfig); + kickerLeft.getConfigurator().apply(kickerLeftConfig); } public void intake(double speed) { speed = Math.abs(speed); - kickerFront.setControl(new VoltageOut(-speed)); - kickerBack.setControl(new VoltageOut(speed)); - } - - public void intake(double frontSpeed, double backSpeed) { - frontSpeed = Math.abs(frontSpeed); - backSpeed = Math.abs(backSpeed); - kickerFront.setControl(new MotionMagicVelocityVoltage(-frontSpeed)); - kickerBack.setControl(new MotionMagicVelocityVoltage(backSpeed)); - } - - public void intakeDutyCycleOut(double speed) { - speed = Math.abs(speed); - kickerFront.setControl(new DutyCycleOut(-speed)); - kickerBack.setControl(new DutyCycleOut(speed)); + kickerRight.setControl(new VoltageOut(speed)); + kickerLeft.setControl(new VoltageOut(-speed)); } - public void intakeDutyCycleOut(double frontSpeed, double backSpeed) { - frontSpeed = Math.abs(frontSpeed); - backSpeed = Math.abs(backSpeed); - kickerFront.setControl(new DutyCycleOut(-frontSpeed)); - kickerBack.setControl(new DutyCycleOut(backSpeed)); + public void intake(double rightSpeed, double leftSpeed) { + rightSpeed = Math.abs(rightSpeed); + leftSpeed = Math.abs(leftSpeed); + kickerRight.setControl(new VoltageOut(rightSpeed)); + kickerLeft.setControl(new VoltageOut(-leftSpeed)); } public void outtake(double speed) { speed = Math.abs(speed); - kickerFront.setControl(new DutyCycleOut(speed)); - kickerBack.setControl(new DutyCycleOut(-speed)); + kickerRight.setControl(new VoltageOut(-speed)); + kickerLeft.setControl(new VoltageOut(speed)); } - public void outtake(double frontSpeed, double backSpeed) { - frontSpeed = Math.abs(frontSpeed); - backSpeed = Math.abs(backSpeed); - kickerFront.setControl(new DutyCycleOut(frontSpeed)); - kickerBack.setControl(new DutyCycleOut(-backSpeed)); + public void outtake(double rightSpeed, double leftSpeed) { + rightSpeed = Math.abs(rightSpeed); + leftSpeed = Math.abs(leftSpeed); + kickerRight.setControl(new VoltageOut(-rightSpeed)); + kickerLeft.setControl(new VoltageOut(leftSpeed)); } public void stop() { - kickerFront.stopMotor(); - kickerBack.stopMotor(); + kickerRight.stopMotor(); + kickerLeft.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 7918de2..cc029f4 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -1,12 +1,10 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.config.VisionConfig; -import org.photonvision.PhotonCamera; public class Vision extends SubsystemBase { - public static final PhotonCamera FrontCameraApril = - new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); - public static final PhotonCamera RearCameraApril = - new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); + // public static final PhotonCamera FrontCameraApril = + // new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); + // public static final PhotonCamera RearCameraApril = + // new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); }