diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..3407179a --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,90 @@ +{ + "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 + ] + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ] + } + ] +} diff --git a/src/main/deploy/pathplanner/autos/Center Depot Alt.auto b/src/main/deploy/pathplanner/autos/Center Depot Alt.auto new file mode 100644 index 00000000..6e2db2db --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Depot Alt.auto @@ -0,0 +1,100 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakePivotOut" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Depot Path 1" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFuel" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "RunRollers" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakePivotStow" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Depot Alt Path 2" + } + }, + { + "type": "named", + "data": { + "name": "Align" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootFuel" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFuel" + } + }, + { + "type": "named", + "data": { + "name": "RunRollers" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Depot Auto.auto b/src/main/deploy/pathplanner/autos/Center Depot Auto.auto index 0bf2a44d..258a02eb 100644 --- a/src/main/deploy/pathplanner/autos/Center Depot Auto.auto +++ b/src/main/deploy/pathplanner/autos/Center Depot Auto.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "IntakePivotOut" + } + }, { "type": "deadline", "data": { @@ -17,7 +23,7 @@ { "type": "named", "data": { - "name": "IntakePivotTravel" + "name": "IntakeFuel" } } ] @@ -36,7 +42,7 @@ { "type": "named", "data": { - "name": "IntakeFuel" + "name": "RunRollers" } } ] @@ -61,9 +67,28 @@ } }, { - "type": "named", + "type": "parallel", "data": { - "name": "ShootFuel" + "commands": [ + { + "type": "named", + "data": { + "name": "ShootFuel" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFuel" + } + }, + { + "type": "named", + "data": { + "name": "RunRollers" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Center Depot to Center Auto.auto b/src/main/deploy/pathplanner/autos/Center Depot to Center Auto.auto new file mode 100644 index 00000000..2e5f1dc8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Depot to Center Auto.auto @@ -0,0 +1,112 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Depot Path 1" + } + }, + { + "type": "named", + "data": { + "name": "IntakePivotTravel" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "IntakeFuel" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakePivotStow" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Depot to Center Path 2" + } + }, + { + "type": "named", + "data": { + "name": "Align" + } + }, + { + "type": "named", + "data": { + "name": "ShootFuel" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Depot to Center Path 3" + } + }, + { + "type": "named", + "data": { + "name": "IntakePivotTravel" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Depot to Center Path 4" + } + }, + { + "type": "named", + "data": { + "name": "IntakeFuel" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakePivotStow" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Top Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Top Neutral Auto.auto index ed3d7563..dd3635cf 100644 --- a/src/main/deploy/pathplanner/autos/Top Neutral Auto.auto +++ b/src/main/deploy/pathplanner/autos/Top Neutral Auto.auto @@ -11,22 +11,9 @@ } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "IntakeFuel" - } - } - ] + "name": "IntakePivotOut" } }, { diff --git a/src/main/deploy/pathplanner/paths/Center Depot Alt Path 2.path b/src/main/deploy/pathplanner/paths/Center Depot Alt Path 2.path new file mode 100644 index 00000000..f21c442e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot Alt Path 2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.67, + "y": 5.95 + }, + "prevControl": null, + "nextControl": { + "x": 1.6699999999999986, + "y": 5.95 + }, + "isLocked": false, + "linkedName": "centerDepotPath" + }, + { + "anchor": { + "x": 1.5247677595628413, + "y": 5.95 + }, + "prevControl": { + "x": 0.5247677595628413, + "y": 5.95 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Depot Path 1.path b/src/main/deploy/pathplanner/paths/Center Depot Path 1.path index 69da05d9..8263c8d5 100644 --- a/src/main/deploy/pathplanner/paths/Center Depot Path 1.path +++ b/src/main/deploy/pathplanner/paths/Center Depot Path 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.3189561091340457, - "y": 5.98 + "x": 0.67, + "y": 5.95 }, "prevControl": { - "x": 2.0354848775654837, - "y": 6.174831955022433 + "x": 2.3865287684314382, + "y": 6.144831955022433 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center Depot Path 2.path b/src/main/deploy/pathplanner/paths/Center Depot Path 2.path index a206b4cd..098e76a5 100644 --- a/src/main/deploy/pathplanner/paths/Center Depot Path 2.path +++ b/src/main/deploy/pathplanner/paths/Center Depot Path 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.3189561091340457, - "y": 5.98 + "x": 0.67, + "y": 5.95 }, "prevControl": null, "nextControl": { - "x": 1.3189561091340445, - "y": 5.98 + "x": 1.6699999999999986, + "y": 5.95 }, "isLocked": false, "linkedName": "centerDepotPath" diff --git a/src/main/deploy/pathplanner/paths/Center Depot to Center Path 2.path b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 2.path new file mode 100644 index 00000000..ac11885c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.67, + "y": 5.95 + }, + "prevControl": null, + "nextControl": { + "x": 1.67, + "y": 5.95 + }, + "isLocked": false, + "linkedName": "centerDepotPath" + }, + { + "anchor": { + "x": 2.6094457831325304, + "y": 5.16055421686747 + }, + "prevControl": { + "x": 1.6094457831325304, + "y": 5.16055421686747 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Center Depot to Center Path 2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.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": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Depot to Center Path 3.path b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 3.path new file mode 100644 index 00000000..4ea40ea2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 3.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6094457831325304, + "y": 5.16055421686747 + }, + "prevControl": null, + "nextControl": { + "x": 2.554807228911401, + "y": 6.613939759036167 + }, + "isLocked": false, + "linkedName": "Center Depot to Center Path 2" + }, + { + "anchor": { + "x": 4.620144578308992, + "y": 5.510240963855443 + }, + "prevControl": { + "x": 3.8330280599413493, + "y": 5.791354006129602 + }, + "nextControl": { + "x": 5.385084337345135, + "y": 5.2370481927711054 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.17722891566265, + "y": 7.105686746987953 + }, + "prevControl": { + "x": 7.690831325301204, + "y": 7.357024096385543 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Center Depot to Center Path 3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -65.05609473657542 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Depot to Center Path 4.path b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 4.path new file mode 100644 index 00000000..9a1b3cef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.17722891566265, + "y": 7.105686746987953 + }, + "prevControl": null, + "nextControl": { + "x": 8.17722891566265, + "y": 7.105686746987953 + }, + "isLocked": false, + "linkedName": "Center Depot to Center Path 3" + }, + { + "anchor": { + "x": 7.647120481927709, + "y": 4.450253012048193 + }, + "prevControl": { + "x": 7.701759036144578, + "y": 4.810867469879518 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Center Depot to Center Path 4" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 170.61448173995697 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -65.05609473657542 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Depot to Center Path 5.path b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 5.path new file mode 100644 index 00000000..53cb3ca0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Depot to Center Path 5.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.647120481927709, + "y": 4.450253012048193 + }, + "prevControl": null, + "nextControl": { + "x": 8.64712048192771, + "y": 4.450253012048193 + }, + "isLocked": false, + "linkedName": "Center Depot to Center Path 4" + }, + { + "anchor": { + "x": 4.532722891566265, + "y": 5.466530120481927 + }, + "prevControl": { + "x": 6.319460843373493, + "y": 5.291686746987951 + }, + "nextControl": { + "x": 2.745984939759037, + "y": 5.6413734939759035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.500168674698795, + "y": 5.149626506024097 + }, + "prevControl": { + "x": 1.500168674698795, + "y": 5.149626506024097 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.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": 170.61448173995697 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Neutral To Depot Path.path b/src/main/deploy/pathplanner/paths/Top Neutral To Depot Path.path index c779322e..48fd67c8 100644 --- a/src/main/deploy/pathplanner/paths/Top Neutral To Depot Path.path +++ b/src/main/deploy/pathplanner/paths/Top Neutral To Depot Path.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.3189561091340457, - "y": 5.98 + "x": 0.67, + "y": 5.95 }, "prevControl": { - "x": 0.6347351748959692, - "y": 5.958144578313254 + "x": 0.9857790657619235, + "y": 5.928144578313254 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 79fc33b3..27d89ec2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -18,6 +19,8 @@ public class Robot extends TimedRobot { private final RobotContainer m_robotContainer; + public Timer timer = new Timer(); + public Robot() { m_robotContainer = new RobotContainer(); DataLogManager.start(); @@ -38,6 +41,8 @@ public void robotPeriodic() { m_robotContainer.latestPoseField.setRobotPose( m_robotContainer.getLatestCameraPose().toPose2d()); SmartDashboard.putData("latest 2d pose", m_robotContainer.latestPoseField); + + SmartDashboard.putNumber("timer", DriverStation.getMatchTime()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 921a8e3f..702fe91a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,6 +44,7 @@ import frc.robot.subsystems.intakerollers.IntakeRollerConstants; import frc.robot.subsystems.intakerollers.IntakeRollers; import frc.robot.subsystems.intakerollers.rolllercommands.IntakeFuel; +import frc.robot.subsystems.intakerollers.rolllercommands.IntakeFuelAlt; import frc.robot.subsystems.intakerollers.rolllercommands.SetIntakeVelocity; import frc.robot.subsystems.outtake.Shooter; import frc.robot.subsystems.outtake.commands.ShooterDefaultBehavior; @@ -83,9 +84,9 @@ public Pose3d getLatestCameraPose() { public Trigger slowMode = driver.b(); private boolean defenseMode = false; - @Logged + @Logged(name = "Defense Mode") public boolean getDefenseMode() { - return defenseMode; + return driver.leftTrigger().getAsBoolean(); } public void toggleDefenseMode() { @@ -128,26 +129,6 @@ public double getDriverTurn() { return rawJoystick; } - @Logged(name = "forwardVelocityValue") - public double getForwardVelocity() { - double forwardVelocity = - (getDefenseMode() && !RebuiltUtil.inDefenseZone(drivetrain.getPose())) - ? MathUtil.clamp( - getDriverForward(), 0, DrivetrainConstants.kMaxLinearVelocity.in(MetersPerSecond)) - : getDriverForward(); - return forwardVelocity; - } - - @Logged(name = "strafeVelocityValue") - public double getStrafeVelocity() { - return getDriverStrafe(); - } - - @Logged(name = "turnVelocityValue") - public double getTurnVelocity() { - return getDriverTurn(); - } - @Logged(name = "calculatedHubHeading") public Rotation2d getHubHeading() { return RebuiltUtil.getHubHeading(drivetrain::getPose); @@ -211,7 +192,8 @@ public RobotContainer() { } private void configureNamedAutoCommands() { - IntakeFuel intakeFuel = new IntakeFuel(intakeRollers, intakePivot); + // IntakeFuel intakeFuel = new IntakeFuel(intakeRollers, intakePivot); + IntakeFuelAlt intakeFuel = new IntakeFuelAlt(intakeRollers, () -> intakePivot.getAngle()); Command intakePivotStow = new GoToAngle(intakePivot, () -> IntakeConstants.kStowedPosition).withTimeout(2); Command intakePivotOut = @@ -224,6 +206,7 @@ private void configureNamedAutoCommands() { ParallelRaceGroup alignInAuto = new ParallelRaceGroup(align); ShootAndIndex shootInAuto = new ShootAndIndex(tunnel, shooter, hood, indexer, this::getHubDistance); + // ParallelRaceGroup runRollers = new ParallelRaceGroup(intakeFuel); NamedCommands.registerCommand("IntakePivotStow", intakePivotStow); NamedCommands.registerCommand("IntakeFuel", intakeInAuto); @@ -231,6 +214,7 @@ private void configureNamedAutoCommands() { NamedCommands.registerCommand("IntakePivotTravel", intakePivotTravel); NamedCommands.registerCommand("ShootFuel", shootInAuto); NamedCommands.registerCommand("Align", alignInAuto); + NamedCommands.registerCommand("RunRollers", intakeFuel); } private void configureAutoChooser() { @@ -267,9 +251,9 @@ private void configureBindings() { drivetrain.setDefaultCommand( drivetrain.defenseDrive( - this::getForwardVelocity, - this::getStrafeVelocity, - this::getTurnVelocity, + this::getDriverForward, + this::getDriverStrafe, + this::getDriverTurn, this::getDefenseMode)); driver.leftBumper().whileTrue(new IntakeFuel(intakeRollers, intakePivot)); @@ -297,16 +281,6 @@ private void configureBindings() { this::getHubHeading, drivetrain::getPose)))); - driver - .leftTrigger() - .whileTrue( - Align.lockOnHub( - drivetrain, - this::getDriverForward, - this::getDriverStrafe, - this::getHubHeading, - drivetrain::getPose)); - driver .rightBumper() .whileTrue( @@ -316,13 +290,13 @@ private void configureBindings() { driver.a().whileTrue(new StaticShoot(tunnel, shooter, indexer, hood)); driver.b().whileTrue(new Feed(tunnel, shooter, hood, indexer)); - driver - .x() - .onTrue( - Commands.runOnce( - () -> { - toggleDefenseMode(); - })); + // driver + // .x() + // .onTrue( + // Commands.runOnce( + // () -> { + // toggleDefenseMode(); + // })); driver.povLeft().whileTrue(new Release(tunnel, shooter, indexer)); driver diff --git a/src/main/java/frc/robot/commands/Align.java b/src/main/java/frc/robot/commands/Align.java index 23c5fec2..54aad981 100644 --- a/src/main/java/frc/robot/commands/Align.java +++ b/src/main/java/frc/robot/commands/Align.java @@ -131,7 +131,7 @@ public static Command faceAllianceZone( return drivetrain.driveFixedHeading( translationX, translationY, - () -> new Rotation2d(Degrees.of(RobotConstants.kShooterFaceOffset.in(Degrees)))); + () -> new Rotation2d(Degrees.of(180 + RobotConstants.kShooterFaceOffset.in(Degrees)))); } public static Command alignLeftClimb(Drivetrain drivetrain, Supplier robotPose) { diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 3209ee03..7fb57e22 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -44,6 +44,7 @@ import frc.robot.RobotConstants; import frc.robot.subsystems.vision.VisionEstimate; import frc.robot.util.AprilTagUtil; +import frc.robot.util.DefenseMode; import frc.robot.util.MyAlliance; import frc.robot.util.RebuiltUtil; import java.util.ArrayList; @@ -441,36 +442,98 @@ public void setAlignmentSetpoint(AlignmentSetpoint setpoint) { alignmentSetpoint = setpoint; } + @Logged(name = "Is defense line") + public boolean isDefenseLine() { + return DefenseMode.isDefenseLine(getPose()); + } + + double clampedVelocity = 0; + + @Logged(name = "Clamped velocity") + public double clampedVelocity() { + return clampedVelocity; + } + + @Logged(name = "Is red alliance line") + public boolean isRedAllianceLine() { + return DefenseMode.isRedAllianceLine(getPose()); + } + + @Logged(name = "Is blue alliance line") + public boolean isBlueAllianceLine() { + return DefenseMode.isBlueAllianceLine(getPose()); + } + + @Logged(name = "Is red neutral line") + public boolean isRedNeutralLine() { + return DefenseMode.isRedNeutralLine(getPose()); + } + + @Logged(name = "Is blue neutral line") + public boolean isBlueNeutralLine() { + return DefenseMode.isBlueNeutralLine(getPose()); + } + + @Logged(name = "Alliance Based Line Type") + public String AllianceBasedLineType() { + return DefenseMode.getAllianceBasedLine(DefenseMode.getDefenseLine(getPose())).toString(); + } + public Command defenseDrive( DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation, BooleanSupplier defenseMode) { return run( - (RebuiltUtil.inAllianceZone(getPose()) - && !RebuiltUtil.inDefenseZone(getPose()) - && defenseMode.getAsBoolean()) - ? () -> { - driveFixedHeading( - translationX.getAsDouble(), translationY.getAsDouble(), Rotation2d.kZero); - } - : () -> { - var speeds = - ChassisSpeeds.discretize( - translationX.getAsDouble(), - translationY.getAsDouble(), - rotation.getAsDouble(), - RobotConstants.kRobotLoopPeriod.in(Seconds)); - - setControl( - fieldCentricRequest - .withVelocityX(speeds.vxMetersPerSecond) - .withVelocityY(speeds.vyMetersPerSecond) - .withRotationalRate(speeds.omegaRadiansPerSecond) - .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); - }); + () -> { + if (DefenseMode.isDefenseLine(getPose()) && defenseMode.getAsBoolean()) { + driveFixedHeading( + DefenseMode.defenseClampVelocity(translationX.getAsDouble(), getPose()), + translationY.getAsDouble(), + Rotation2d.kZero); + clampedVelocity = + DefenseMode.defenseClampVelocity(translationX.getAsDouble(), getPose()); + } else { + var speeds = + ChassisSpeeds.discretize( + translationX.getAsDouble(), + translationY.getAsDouble(), + rotation.getAsDouble(), + RobotConstants.kRobotLoopPeriod.in(Seconds)); + + setControl( + fieldCentricRequest + .withVelocityX(speeds.vxMetersPerSecond) + .withVelocityY(speeds.vyMetersPerSecond) + .withRotationalRate(speeds.omegaRadiansPerSecond) + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); + } + }); } + // public Command defenseDrive( + // DoubleSupplier translationX, + // DoubleSupplier translationY, + // DoubleSupplier rotation, + // BooleanSupplier defenseMode) { + // return run( + // () -> { + // var speeds = + // ChassisSpeeds.discretize( + // translationX.getAsDouble(), + // translationY.getAsDouble(), + // rotation.getAsDouble(), + // RobotConstants.kRobotLoopPeriod.in(Seconds)); + + // setControl( + // fieldCentricRequest + // .withVelocityX(speeds.vxMetersPerSecond) + // .withVelocityY(speeds.vyMetersPerSecond) + // .withRotationalRate(speeds.omegaRadiansPerSecond) + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));} + // ); + // } + public boolean atPoseSetpoint(Distance tranTol, Angle rotTol, Supplier currentPose) { return currentPose.get().getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()) < tranTol.in(Meters) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/TunerConstants.java b/src/main/java/frc/robot/subsystems/drivetrain/TunerConstants.java index 74b115a3..67ca461b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/TunerConstants.java @@ -57,7 +57,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(50); + private static final Current kSlipCurrent = Amps.of(80); private static final double kClosedLoopDriveRampRate = 0.01; private static final double kClosedLoopSteerRampRate = 0.01; @@ -67,7 +67,7 @@ public class TunerConstants { new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(Amps.of(50)) + .withStatorCurrentLimit(Amps.of(80)) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(Amps.of(40)) .withSupplyCurrentLimitEnable(true)) @@ -167,7 +167,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 6; private static final int kFrontRightSteerMotorId = 8; private static final int kFrontRightEncoderId = 3; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.147216796875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.043212890625); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/intakePivot/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intakePivot/IntakeConstants.java index ecce69e1..c5191df2 100644 --- a/src/main/java/frc/robot/subsystems/intakePivot/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intakePivot/IntakeConstants.java @@ -18,22 +18,25 @@ public class IntakeConstants { public static final int kPivotMotorId = 16; public static final Current kCurrentLimit = Amps.of(60); + public static final String kPivotCanbus = "CANivore"; + public static final String kPivotEncoderCanbus = "CANivore"; public static final boolean kCurrentLimitEnable = true; public static final boolean kInverted = true; - public static final double kG = 0.24; - public static final double kP = 4.8; + public static final double kG = 0.21; + public static final double kP = 6.7; public static final double kI = 0; - public static final double kD = 0; - public static final double kSensorToMechanismRatio = 72; + public static final double kD = 0.1; + public static final double kSensorToMechanismRatio = 108; public static final double kRelativeEncoderId = 0; public static final Angle kIntakePosition = Degrees.of(0); public static final Angle kTravelPosition = Degrees.of(30); public static final Angle kStowedPosition = Degrees.of(120); - public static final int kEncoderID = 0; + public static final int kEncoderID = 5; public static final Voltage kHomingVoltage = Volts.of(-2.0); public static final Angle kAngleTolerance = Degrees.of(1.5); - public static final AngularVelocity kMaxVelocity = RadiansPerSecond.of(Math.PI); - public static final AngularAcceleration kMaxAcceleration = RadiansPerSecondPerSecond.of(Math.PI); + public static final AngularVelocity kMaxVelocity = RadiansPerSecond.of(4 * Math.PI); + public static final AngularAcceleration kMaxAcceleration = + RadiansPerSecondPerSecond.of(6 * Math.PI); public static final Constraints kMaxPivotConstraints = new Constraints( kMaxVelocity.in(RadiansPerSecond), kMaxAcceleration.in(RadiansPerSecondPerSecond)); diff --git a/src/main/java/frc/robot/subsystems/intakePivot/IntakePivot.java b/src/main/java/frc/robot/subsystems/intakePivot/IntakePivot.java index 8fbd1615..96904036 100644 --- a/src/main/java/frc/robot/subsystems/intakePivot/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/intakePivot/IntakePivot.java @@ -7,14 +7,17 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -27,8 +30,13 @@ public class IntakePivot extends SubsystemBase { - @Logged private TalonFX intakePivotMotor = new TalonFX(IntakeConstants.kPivotMotorId); - @Logged private CANcoder intakeEncoder = new CANcoder(IntakeConstants.kEncoderID); + @Logged + private TalonFX intakePivotMotor = + new TalonFX(IntakeConstants.kPivotMotorId, IntakeConstants.kPivotCanbus); + + @Logged + private CANcoder intakeEncoder = + new CANcoder(IntakeConstants.kEncoderID, IntakeConstants.kPivotEncoderCanbus); private MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); private FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); @@ -56,6 +64,14 @@ private void motorConfigurations() { motionMagicConfigs.withMotionMagicCruiseVelocity(IntakeConstants.kMaxVelocity); + intakeEncoder + .getConfigurator() + .apply( + new CANcoderConfiguration() + .withMagnetSensor( + new MagnetSensorConfigs() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withMagnetOffset(-0.47705))); intakePivotMotor.getConfigurator().apply(motorConfigs); intakePivotMotor.getConfigurator().apply(currentLimitsConfigs); intakePivotMotor.getConfigurator().apply(feedbackConfigs); diff --git a/src/main/java/frc/robot/subsystems/intakePivot/intakePivotCommands/Tune.java b/src/main/java/frc/robot/subsystems/intakePivot/intakePivotCommands/Tune.java index fb2b0fbb..a190f2c3 100644 --- a/src/main/java/frc/robot/subsystems/intakePivot/intakePivotCommands/Tune.java +++ b/src/main/java/frc/robot/subsystems/intakePivot/intakePivotCommands/Tune.java @@ -13,10 +13,10 @@ public class Tune extends Command { IntakePivot intakePivot; - TunableConstant kP = new TunableConstant("/IntakePivot/kP", 0); - TunableConstant kI = new TunableConstant("/IntakePivot/kI", 0); - TunableConstant kD = new TunableConstant("/IntakePivot/kD", 0); - TunableConstant kG = new TunableConstant("/IntakePivot/kG", 0); + TunableConstant kP = new TunableConstant("/IntakePivot/kP", IntakeConstants.kP); + TunableConstant kI = new TunableConstant("/IntakePivot/kI", IntakeConstants.kI); + TunableConstant kD = new TunableConstant("/IntakePivot/kD", IntakeConstants.kD); + TunableConstant kG = new TunableConstant("/IntakePivot/kG", IntakeConstants.kG); TunableConstant angle = new TunableConstant("/IntakePivot/angle", 0); TunableConstant acceleration = new TunableConstant( diff --git a/src/main/java/frc/robot/subsystems/intakerollers/rolllercommands/IntakeFuelAlt.java b/src/main/java/frc/robot/subsystems/intakerollers/rolllercommands/IntakeFuelAlt.java new file mode 100644 index 00000000..b5056961 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intakerollers/rolllercommands/IntakeFuelAlt.java @@ -0,0 +1,43 @@ +/* (C) RoboLancers 2026 */ +package frc.robot.subsystems.intakerollers.rolllercommands; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.intakerollers.IntakeRollerConstants; +import frc.robot.subsystems.intakerollers.IntakeRollers; +import java.util.function.Supplier; + +public class IntakeFuelAlt extends Command { + + IntakeRollers intakeRollers; + Supplier pivotAngleSupplier; + + public IntakeFuelAlt(IntakeRollers intakeRollers, Supplier pivotAngleSupplier) { + this.intakeRollers = intakeRollers; + this.pivotAngleSupplier = pivotAngleSupplier; + addRequirements(intakeRollers); + } + + @Override + public void execute() { + if (pivotAngleSupplier.get().in(Degrees) < 30) { + intakeRollers.setTargetVelocity(IntakeRollerConstants.kIntakeFuelVelocity); + intakeRollers.goToVelocity(IntakeRollerConstants.kIntakeFuelVelocity); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + intakeRollers.setVoltage(Volts.of(0)); + intakeRollers.setTargetVelocity(RPM.of(0)); + } +} diff --git a/src/main/java/frc/robot/util/DefenseMode.java b/src/main/java/frc/robot/util/DefenseMode.java new file mode 100644 index 00000000..53c4852c --- /dev/null +++ b/src/main/java/frc/robot/util/DefenseMode.java @@ -0,0 +1,120 @@ +/* (C) RoboLancers 2026 */ +package frc.robot.util; + +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.subsystems.drivetrain.DrivetrainConstants; + +public class DefenseMode { + + public enum DefenseLine { + RedAlliance, + RedNeutral, + BlueAlliance, + BlueNeutral, + None + } + + public enum AllianceBasedLine { + Alliance, + Neutral, + OppositeAlliance, + OppositeNeutral, + None + } + + public static boolean isRedAllianceLine(Pose2d pose) { + return RebuiltUtil.inRedAllianceZone(pose) && !RebuiltUtil.inRedDefenseZone(pose); + } + + public static boolean isBlueAllianceLine(Pose2d pose) { + return RebuiltUtil.inBlueAllianceZone(pose) && !RebuiltUtil.inBlueDefenseZone(pose); + } + + public static boolean isRedNeutralLine(Pose2d pose) { + return RebuiltUtil.inRedNeutralZone(pose) && !RebuiltUtil.inRedNeutralDefenseZone(pose); + } + + public static boolean isBlueNeutralLine(Pose2d pose) { + return RebuiltUtil.inBlueNeutralZone(pose) && !RebuiltUtil.inBlueNeutralDefenseZone(pose); + } + + public static DefenseLine getDefenseLine(Pose2d pose) { + DefenseLine line = DefenseLine.None; + // is red alliance + // get red lines + // flip logic for blue + // default None enum + if (isRedAllianceLine(pose)) { + line = DefenseLine.RedAlliance; + } else if (isBlueAllianceLine(pose)) { + line = DefenseLine.BlueAlliance; + } else if (isRedNeutralLine(pose)) { + line = DefenseLine.RedNeutral; + } else if (isBlueNeutralLine(pose)) { + line = DefenseLine.BlueNeutral; + } + return line; + } + + public static boolean isDefenseLine(Pose2d pose) { + return getDefenseLine(pose) != DefenseLine.None; + } + + public static AllianceBasedLine getAllianceBasedLine(DefenseLine line) { + + if (MyAlliance.isBlue()) { + switch (line) { + case BlueAlliance: + return AllianceBasedLine.Alliance; + case RedAlliance: + return AllianceBasedLine.OppositeAlliance; + case BlueNeutral: + return AllianceBasedLine.Neutral; + case RedNeutral: + return AllianceBasedLine.OppositeNeutral; + case None: + default: + return AllianceBasedLine.None; + } + } else { + switch (line) { + case BlueAlliance: + return AllianceBasedLine.OppositeAlliance; + case RedAlliance: + return AllianceBasedLine.Alliance; + case BlueNeutral: + return AllianceBasedLine.OppositeNeutral; + case RedNeutral: + return AllianceBasedLine.Neutral; + case None: + default: + return AllianceBasedLine.None; + } + } + } + + public static double defenseLineClamp(double velocity, AllianceBasedLine line) { + double processedVelocity = velocity; + + // Cant go backwards if on opposite alliance line or our neutral line + if (line == AllianceBasedLine.OppositeAlliance || line == AllianceBasedLine.Neutral) { + processedVelocity = + MathUtil.clamp( + processedVelocity, 0, DrivetrainConstants.kMaxLinearVelocity.in(MetersPerSecond)); + } else if (line == AllianceBasedLine.Alliance || line == AllianceBasedLine.OppositeNeutral) { + // cant go forwardds if on our alliance line or the opposing sides neutral line + processedVelocity = + MathUtil.clamp( + processedVelocity, -DrivetrainConstants.kMaxLinearVelocity.in(MetersPerSecond), 0); + } + // otherwise you are just good + return processedVelocity; + } + + public static double defenseClampVelocity(double velocity, Pose2d pose) { + return defenseLineClamp(velocity, getAllianceBasedLine(getDefenseLine(pose))); + } +} diff --git a/src/main/java/frc/robot/util/RebuiltUtil.java b/src/main/java/frc/robot/util/RebuiltUtil.java index 2c4285b2..3439c5a1 100644 --- a/src/main/java/frc/robot/util/RebuiltUtil.java +++ b/src/main/java/frc/robot/util/RebuiltUtil.java @@ -35,11 +35,24 @@ public class RebuiltUtil { public static final Rectangle2d blueAllianceZone = new Rectangle2d(new Translation2d(0, 0), new Translation2d(4, 8)); - public static final Rectangle2d redDefenseZone = + public static final Rectangle2d redAllianceDefenseZone = new Rectangle2d(new Translation2d(16.5, 0), new Translation2d(13, 8)); - public static final Rectangle2d blueDefenseZone = + public static final Rectangle2d blueAllianceDefenseZone = new Rectangle2d(new Translation2d(0, 0), new Translation2d(3.5, 8)); + public static final Rectangle2d neutralZone = + new Rectangle2d(new Translation2d(5.2, 0), new Translation2d(11.4, 8)); + public static final Rectangle2d blueNeutralZone = + new Rectangle2d(new Translation2d(5.2, 0), new Translation2d(8.3, 8)); + public static final Rectangle2d redNeutralZone = + new Rectangle2d(new Translation2d(8.3, 0), new Translation2d(11.4, 8)); + public static final Rectangle2d neutralDefenseZone = + new Rectangle2d(new Translation2d(5.7, 0), new Translation2d(10.9, 8)); + public static final Rectangle2d blueNeutralDefenseZone = + new Rectangle2d(new Translation2d(5.7, 0), new Translation2d(8.3, 8)); + public static final Rectangle2d redNeutralDefenseZone = + new Rectangle2d(new Translation2d(8.3, 0), new Translation2d(10.9, 8)); + public static final int redClimbTagID = 15; public static final int blueClimbTagID = 31; @@ -54,16 +67,64 @@ public static Rectangle2d getAllianceZone() { return MyAlliance.isRed() ? redAllianceZone : blueAllianceZone; } - public static Rectangle2d getDefenseZone() { - return MyAlliance.isRed() ? blueDefenseZone : redDefenseZone; + public static Rectangle2d getOppositeAllianceZone() { + return MyAlliance.isRed() ? blueAllianceZone : redAllianceZone; + } + + public static Rectangle2d getOppositeAllianceDefenseZone() { + return MyAlliance.isRed() ? blueAllianceDefenseZone : redAllianceDefenseZone; } public static boolean inAllianceZone(Pose2d pose) { return getAllianceZone().contains(pose.getTranslation()); } - public static boolean inDefenseZone(Pose2d pose) { - return getDefenseZone().contains(pose.getTranslation()); + public static boolean inOppositeAllianceZone(Pose2d pose) { + return getOppositeAllianceZone().contains(pose.getTranslation()); + } + + public static boolean inRedAllianceZone(Pose2d pose) { + return redAllianceZone.contains(pose.getTranslation()); + } + + public static boolean inBlueAllianceZone(Pose2d pose) { + return blueAllianceZone.contains(pose.getTranslation()); + } + + public static boolean inBlueDefenseZone(Pose2d pose) { + return blueAllianceDefenseZone.contains(pose.getTranslation()); + } + + public static boolean inRedDefenseZone(Pose2d pose) { + return redAllianceDefenseZone.contains(pose.getTranslation()); + } + + public static boolean inOppositeAllianceDefenseZone(Pose2d pose) { + return getOppositeAllianceDefenseZone().contains(pose.getTranslation()); + } + + public static boolean inNeutralZone(Pose2d pose) { + return neutralZone.contains(pose.getTranslation()); + } + + public static boolean inNeutralDefenseZone(Pose2d pose) { + return neutralDefenseZone.contains(pose.getTranslation()); + } + + public static boolean inRedNeutralDefenseZone(Pose2d pose) { + return redNeutralDefenseZone.contains(pose.getTranslation()); + } + + public static boolean inBlueNeutralDefenseZone(Pose2d pose) { + return blueNeutralDefenseZone.contains(pose.getTranslation()); + } + + public static boolean inBlueNeutralZone(Pose2d pose) { + return blueNeutralZone.contains(pose.getTranslation()); + } + + public static boolean inRedNeutralZone(Pose2d pose) { + return redNeutralZone.contains(pose.getTranslation()); } public static TunableConstant xTransform = new TunableConstant("X Transform", 0);