From: iefomit Date: Mon, 30 Mar 2026 23:43:05 +0000 (-0700) Subject: -imports, + auto translation paths X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=39712a190d552d2ed076f8d4070b2be34f51ff51;p=FRC2026.git -imports, + auto translation paths --- diff --git a/src/main/deploy/pathplanner/autos/LeftLiberalDoubleSwipeTranslation.auto b/src/main/deploy/pathplanner/autos/LeftLiberalDoubleSwipeTranslation.auto new file mode 100644 index 0000000..65ee476 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftLiberalDoubleSwipeTranslation.auto @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "W5 Left Trench Start" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + }, + { + "type": "path", + "data": { + "pathName": "Swipe1Translation" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "Stop Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Reset Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "path", + "data": { + "pathName": "Swipe2Translation" + } + }, + { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Swipe1Translation.path b/src/main/deploy/pathplanner/paths/Swipe1Translation.path new file mode 100644 index 0000000..d761d44 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Swipe1Translation.path @@ -0,0 +1,155 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.5, + "y": 7.623190984573506 + }, + "prevControl": null, + "nextControl": { + "x": 6.735631586835829, + "y": 7.2764780852994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.710521945432978, + "y": 6.697900355871886 + }, + "prevControl": { + "x": 7.529042978857038, + "y": 6.86984623308483 + }, + "nextControl": { + "x": 7.928919995080244, + "y": 6.490974754868527 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.710521945432978, + "y": 5.7188137603795965 + }, + "prevControl": { + "x": 7.925365275955983, + "y": 5.863911730255719 + }, + "nextControl": { + "x": 7.408579979016614, + "y": 5.514892297047377 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.860545670225386, + "y": 6.364365361803084 + }, + "prevControl": { + "x": 6.917316499510639, + "y": 6.120896528187229 + }, + "nextControl": { + "x": 6.574474152212791, + "y": 7.591219006502909 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6124792408066435, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.872250653983406, + "y": 7.333391289226129 + }, + "nextControl": { + "x": 5.300042806450162, + "y": 7.497750261334479 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": { + "x": 4.245580247742288, + "y": 7.453198911136212 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7761529808773961, + "maxWaypointRelativePos": 1.642294713160863, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 150.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stop Hood Down", + "waypointRelativePos": 4.968503937007888, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } + }, + { + "name": "Start Spindexer", + "waypointRelativePos": 5.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.5, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Swipe2Translation.path b/src/main/deploy/pathplanner/paths/Swipe2Translation.path new file mode 100644 index 0000000..22b175e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Swipe2Translation.path @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": null, + "nextControl": { + "x": 5.75, + "y": 7.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0213285883748515, + "y": 6.289051008303677 + }, + "prevControl": { + "x": 6.135976270876549, + "y": 7.282457223754833 + }, + "nextControl": { + "x": 5.929276488101032, + "y": 5.4914323686422035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.258030842230131, + "y": 4.449228944246738 + }, + "prevControl": { + "x": 5.653713425517404, + "y": 4.619355154982582 + }, + "nextControl": { + "x": 6.687014708632405, + "y": 4.3284622781876605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.892823250296561, + "y": 5.546666666666668 + }, + "prevControl": { + "x": 6.892823250296561, + "y": 4.907866997335997 + }, + "nextControl": { + "x": 6.892823250296561, + "y": 6.2753696930420535 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.258030842230131, + "y": 6.977639383155397 + }, + "prevControl": { + "x": 6.423269381273585, + "y": 6.7900328128685725 + }, + "nextControl": { + "x": 6.092792303186677, + "y": 7.165245953442221 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": { + "x": 5.5, + "y": 7.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae35617..cb80978 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,5 @@ package frc.robot; -import java.util.concurrent.ScheduledExecutorService; import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.Logger; @@ -20,7 +19,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ScheduleCommand; import frc.robot.commands.LogCommand; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.gpm.AutoShootCommand; @@ -272,6 +270,7 @@ public class RobotContainer { String rightSideAuto = "Right Week V1"; String shootOnlyAuto = "Shoot Only Left Week V1"; String leftLiberalSwipe = "LeftLiberalDoubleSwipe"; + String leftLiberalSwipeTranslation = "LeftLiberalDoubleSwipeTranslation"; String leftConservativeSwipe = "LeftConservativeDoubleSwipe"; autoChooser.setDefaultOption("Default", new PathPlannerAuto(defaultAuto)); @@ -280,6 +279,7 @@ public class RobotContainer { addAuto(shootOnlyAuto); addAuto(leftConservativeSwipe); addAuto(leftLiberalSwipe); + addAuto(leftLiberalSwipeTranslation); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/gpm/BrownOutControl.java b/src/main/java/frc/robot/commands/gpm/BrownOutControl.java index a48076a..cfa986c 100644 --- a/src/main/java/frc/robot/commands/gpm/BrownOutControl.java +++ b/src/main/java/frc/robot/commands/gpm/BrownOutControl.java @@ -1,7 +1,5 @@ package frc.robot.commands.gpm; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake.Intake; @@ -9,7 +7,6 @@ import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.spindexer.SpindexerConstants; import frc.robot.subsystems.turret.Turret; import frc.robot.util.BrownOut.BrownOutConstants; import frc.robot.util.BrownOut.BrownOutLevel; diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 0af0c6e..4146791 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.robot.Robot; -import frc.robot.commands.gpm.BrownOutControl; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.ReverseMotors; import frc.robot.commands.gpm.RunSpindexer; diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 5847e67..c1fca0a 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -29,7 +29,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; import frc.robot.constants.IntakeConstants; -import frc.robot.subsystems.shooter.ShooterConstants; public class Intake extends SubsystemBase implements IntakeIO{ // Mechanism Display... diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f6c704e..bdd1874 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; -import frc.robot.subsystems.spindexer.SpindexerConstants; import frc.robot.util.HubActive; public class Shooter extends SubsystemBase implements ShooterIO { diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 4c7754c..a4a1014 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase;