diff --git a/bin/main/frc/robot/Constants$FieldConstants.class b/bin/main/frc/robot/Constants$FieldConstants.class new file mode 100644 index 0000000..0ff10f6 Binary files /dev/null and b/bin/main/frc/robot/Constants$FieldConstants.class differ diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index 434dd6e..c1e5731 100644 Binary files a/bin/main/frc/robot/Constants$RobotConstants.class and b/bin/main/frc/robot/Constants$RobotConstants.class differ diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index 901da05..7307486 100644 Binary files a/bin/main/frc/robot/Constants$SuperstructureConstants.class and b/bin/main/frc/robot/Constants$SuperstructureConstants.class differ diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index 248fca2..c286ab4 100644 Binary files a/bin/main/frc/robot/Constants$SwerveConstants.class and b/bin/main/frc/robot/Constants$SwerveConstants.class differ diff --git a/bin/main/frc/robot/Constants.class b/bin/main/frc/robot/Constants.class index db02e98..32011da 100644 Binary files a/bin/main/frc/robot/Constants.class and b/bin/main/frc/robot/Constants.class differ diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 402aec1..6f1557b 100644 Binary files a/bin/main/frc/robot/RobotContainer.class and b/bin/main/frc/robot/RobotContainer.class differ diff --git a/bin/main/frc/robot/commands/GeneratePath.class b/bin/main/frc/robot/commands/GeneratePath.class new file mode 100644 index 0000000..9e3ff25 Binary files /dev/null and b/bin/main/frc/robot/commands/GeneratePath.class differ diff --git a/bin/main/frc/robot/commands/TeleopSwerve.class b/bin/main/frc/robot/commands/TeleopSwerve.class index af250cb..257942f 100644 Binary files a/bin/main/frc/robot/commands/TeleopSwerve.class and b/bin/main/frc/robot/commands/TeleopSwerve.class differ diff --git a/bin/main/frc/robot/subsystems/Grabber.class b/bin/main/frc/robot/subsystems/Grabber.class index 75ac9b5..8660d7b 100644 Binary files a/bin/main/frc/robot/subsystems/Grabber.class and b/bin/main/frc/robot/subsystems/Grabber.class differ diff --git a/bin/main/frc/robot/subsystems/Intake.class b/bin/main/frc/robot/subsystems/Intake.class index 41d732b..3c3e492 100644 Binary files a/bin/main/frc/robot/subsystems/Intake.class and b/bin/main/frc/robot/subsystems/Intake.class differ diff --git a/bin/main/frc/robot/subsystems/Swerve.class b/bin/main/frc/robot/subsystems/Swerve.class index dcf5668..5197c7d 100644 Binary files a/bin/main/frc/robot/subsystems/Swerve.class and b/bin/main/frc/robot/subsystems/Swerve.class differ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index efa081f..504472d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,10 +3,13 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.pathplanner.lib.path.PathConstraints; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import frc.FSLib2025.swerve.SwerveModuleConstants; @@ -16,6 +19,7 @@ public static final class RobotConstants { public static final double PERIODIC_INTERVAL = 0.02; // the periodic ,in seconds public static final String CANBUS_NAME = "GTX7130"; public static final int DRIVE_CONTROLLER_PORT = 0; + public static final int OPERATOR_CONTROLLER_PORT = 1; } public static final class SwerveConstants { @@ -85,6 +89,12 @@ public static final class SwerveConstants { STEER_MOTOR_CONFIGURATION.inverted(false); STEER_MOTOR_CONFIGURATION.idleMode(IdleMode.kBrake); } + + public static final PathConstraints constraints = new PathConstraints( + 4.0, + 4.0, + Rotation2d.fromDegrees(180).getRotations(), + Rotation2d.fromDegrees(360).getRotations()); } public static final class SuperstructureConstants { @@ -224,4 +234,36 @@ public static final class SuperstructureConstants { public static final double INTAKE_KI = 0; public static final double INTAKE_KD = 0; } + + public static final class FieldConstants { + // starting pose + public static final Pose2d Left = new Pose2d(new Translation2d(7.231, 7.615), Rotation2d.fromDegrees(180)); + public static final Pose2d Mid = new Pose2d(new Translation2d(8.068, 6.178), Rotation2d.fromDegrees(180)); + public static final Pose2d Right = new Pose2d(new Translation2d(8.068, 5.067), Rotation2d.fromDegrees(180)); + // REEF scoring pose + public static final Pose2d A = new Pose2d(new Translation2d(3.180, 4.082), Rotation2d.fromDegrees(0)); + public static final Pose2d B = new Pose2d(new Translation2d(3.109, 3.847), Rotation2d.fromDegrees(0)); + public static final Pose2d C = new Pose2d(new Translation2d(3.654, 2.885), Rotation2d.fromDegrees(60)); + public static final Pose2d D = new Pose2d(new Translation2d(3.920, 2.731), Rotation2d.fromDegrees(60)); + public static final Pose2d E = new Pose2d(new Translation2d(5.058, 2.713), Rotation2d.fromDegrees(120)); + public static final Pose2d F = new Pose2d(new Translation2d(5.325, 2.880), Rotation2d.fromDegrees(120)); + public static final Pose2d G = new Pose2d(new Translation2d(5.883, 3.851), Rotation2d.fromDegrees(180)); + public static final Pose2d H = new Pose2d(new Translation2d(5.883, 4.190), Rotation2d.fromDegrees(180)); + public static final Pose2d I = new Pose2d(new Translation2d(5.341, 5.341), Rotation2d.fromDegrees(-120)); + public static final Pose2d J = new Pose2d(new Translation2d(5.072, 5.320), Rotation2d.fromDegrees(-120)); + public static final Pose2d K = new Pose2d(new Translation2d(3.887, 5.375), Rotation2d.fromDegrees(-60)); + public static final Pose2d L = new Pose2d(new Translation2d(3.623, 5.215), Rotation2d.fromDegrees(-60)); + // Algae & Coral + public static final Pose2d LA = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180)); + public static final Pose2d LC = new Pose2d(new Translation2d(1.843, 5.848), Rotation2d.fromDegrees(180)); + public static final Pose2d MA = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180)); + public static final Pose2d MC = new Pose2d(new Translation2d(1.843, 4.020), Rotation2d.fromDegrees(180)); + public static final Pose2d RA = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180)); + public static final Pose2d RC = new Pose2d(new Translation2d(1.843, 2.166), Rotation2d.fromDegrees(180)); + // PROCESSOR + public static final Pose2d PRO = new Pose2d(new Translation2d(6.347, 0.573), Rotation2d.fromDegrees(-90)); + // Coral Station + public static final Pose2d CSR = new Pose2d(new Translation2d(1.666, 0.693), Rotation2d.fromDegrees(-126)); + public static final Pose2d CSL = new Pose2d(new Translation2d(1.666, 7.338), Rotation2d.fromDegrees(126)); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1260dcc..2bc54e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,37 +1,39 @@ package frc.robot; +import java.util.function.BooleanSupplier; + import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.RobotConstants; +import frc.robot.commands.GeneratePath; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.Swerve; public class RobotContainer { - - public CommandXboxController controller = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT); - private final Swerve swerve = new Swerve(); + public CommandXboxController m_Driver = new CommandXboxController(RobotConstants.DRIVE_CONTROLLER_PORT); + public CommandXboxController m_Operator = new CommandXboxController(RobotConstants.OPERATOR_CONTROLLER_PORT); + + private final Swerve m_Swerve = new Swerve(); - private final TeleopSwerve teleopSwerve = new TeleopSwerve(swerve, controller); + private final TeleopSwerve teleopSwerve = new TeleopSwerve(m_Swerve, m_Driver); private final SendableChooser autoChooser; public RobotContainer() { - swerve.setDefaultCommand(teleopSwerve); + m_Swerve.setDefaultCommand(teleopSwerve); - configureButtonBindings(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); } - private void configureButtonBindings() { - } - public Command getAutonomousCommand() { return autoChooser.getSelected(); } diff --git a/src/main/java/frc/robot/commands/GeneratePath.java b/src/main/java/frc/robot/commands/GeneratePath.java new file mode 100644 index 0000000..8115f49 --- /dev/null +++ b/src/main/java/frc/robot/commands/GeneratePath.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.io.IOException; + +import org.json.simple.parser.ParseException; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.FileVersionException; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.SwerveConstants; + +public class GeneratePath { + + public Command pathFindToPose(Pose2d targetPose) { + return AutoBuilder.pathfindToPose(targetPose, SwerveConstants.CONCTRAINTS); + } + + public Command pathFindThenFollow(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.pathfindThenFollowPath(PathPlannerPath.fromPathFile(path), SwerveConstants.CONCTRAINTS); + } + + public Command followPath(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.followPath(PathPlannerPath.fromPathFile(path)); + } + + public Command followChoreoPath(String path) throws FileVersionException, IOException, ParseException { + return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(path)); + } +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index e426f25..d9d2470 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,17 +1,19 @@ package frc.robot.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; 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.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.subsystems.Swerve; public class TeleopSwerve extends Command { - private final Swerve swerve; + private final Swerve m_Swerve; private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); @@ -23,45 +25,46 @@ public class TeleopSwerve extends Command { private double reduction = 1; - private CommandXboxController controller; + private CommandXboxController m_Driver; - public TeleopSwerve(Swerve swerve, CommandXboxController controller) { - this.swerve = swerve; - this.controller = controller; - addRequirements(swerve); + public TeleopSwerve(Swerve m_Swerve, CommandXboxController m_Driver) { + this.m_Swerve = m_Swerve; + this.m_Driver = m_Driver; + addRequirements(m_Swerve); } @Override public void execute() { - if (controller.getHID().getAButtonPressed()) { - swerve.setOdometryPosition(new Pose2d()); - swerve.setGyroYaw(new Rotation2d()); + if (m_Driver.getHID().getBackButtonPressed()) { + m_Swerve.setOdometryPosition(new Pose2d()); + m_Swerve.setGyroYaw(new Rotation2d()); } - if (controller.getHID().getRightBumperButton()) { - reduction = 0.3; - } else { - reduction = 1; + double slow = 1; + if (m_Driver.getHID().getLeftBumperButton() || m_Driver.getHID().getRightBumperButton()) { + slow = 0.5; } - xSpeed = xLimiter.calculate(-controller.getLeftY() * reduction); - ySpeed = yLimiter.calculate(-controller.getLeftX() * reduction); - rotSpeed = rotLimiter.calculate(-controller.getRightX() * reduction); + double deadband = 0.04; + double xyMultiplier = 1; + double zMultiplier = 0.75; - // square the input to inprove driving experience + xSpeed = xLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getLeftY(), deadband)); + ySpeed = yLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getLeftX(), deadband)); + rotSpeed = rotLimiter.calculate(MathUtil.applyDeadband(-m_Driver.getRightX(), deadband)); xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); - ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); - rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); - - swerve.drive( - new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED), - rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED, + ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); + // rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); + m_Swerve.drive( + new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED).times(xyMultiplier).times(slow), + rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED * zMultiplier * slow, + // 0, true); } @Override public void end(boolean interrupted) { - swerve.drive(new Translation2d(), 0, false); + m_Swerve.drive(new Translation2d(), 0, false); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 640a7eb..e50fe29 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,14 @@ public class Swerve extends SubsystemBase { + private static Swerve m_Instance = null; + public static Swerve getInstance() { + if (m_Instance == null) { + m_Instance = new Swerve(); + } + return m_Instance; + } + private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); private SwerveModule[] modules = new SwerveModule[] {