From 4b73b5d29fe7294dc8a12522d08cd95bc0bb1f8d Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 24 Jan 2026 13:58:46 -0500 Subject: [PATCH 1/4] Created to pose command --- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/RobotContainer.java | 9 ++++ .../robot/commands/drive/DriveToPosition.java | 54 +++++++++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 src/main/java/frc/robot/commands/drive/DriveToPosition.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d88c015..834bb25 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.RobotBase; /** @@ -55,6 +56,7 @@ public enum Mode { public static final double INTAKE_SPEED = -0.5; public static final double SPIN_TIMEOUT = 5; public static final double TILT_TIMEOUT = 5; + public static final double DRIVE_TO_POSE_TIMEOUT = 10; public static final double TILT_LENGTH = 0.2; public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45); public static final Rotation2d TILT_MAX_ANGLE = Rotation2d.fromDegrees(90); @@ -75,5 +77,7 @@ public enum Mode { public static final int DRIVE_JOYSTICK_PORT = 0; public static final int STEER_JOYSTICK_PORT = 1; + + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f6f319..404bf22 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,12 +7,16 @@ import java.io.File; import edu.wpi.first.wpilibj.Filesystem; +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.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.drive.DriveToPosition; import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.roller.SpinRoller; import frc.robot.commands.tilt.TiltDown; @@ -99,6 +103,11 @@ private void configureBindings() { // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + Translation2d translation = new Translation2d(16,5); + Rotation2d rotation = new Rotation2d(0); + Pose2d pose = new Pose2d(translation,rotation); + Command DriveToPosition = new DriveToPosition(drivebase, pose, true); + driveJoystick.button(1).whileTrue(DriveToPosition); } public void putShuffleboardCommands() { if (Constants.DEBUG) { diff --git a/src/main/java/frc/robot/commands/drive/DriveToPosition.java b/src/main/java/frc/robot/commands/drive/DriveToPosition.java new file mode 100644 index 0000000..dde42f9 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveToPosition.java @@ -0,0 +1,54 @@ +// 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.drive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +public class DriveToPosition extends Command { + + private final SwerveSubsystem drivebase; + private final Pose2d pose; + private final boolean fieldRelative; + private Timer timer; + public DriveToPosition(SwerveSubsystem drivebase, Pose2d pose, boolean fieldRelative) { + timer = new Timer(); + this.drivebase = drivebase; + this.pose = pose; + this.fieldRelative = fieldRelative; + addRequirements(drivebase); + } + + + @Override + public void initialize() { + timer.restart(); + } + + + @Override + public void execute() { + drivebase.drive(pose.getTranslation(),pose.getRotation().getDegrees(), fieldRelative); + + } + + @Override + public void end(boolean interrupted) { + + } + + + @Override + public boolean isFinished() { + if (timer.hasElapsed(Constants.DRIVE_TO_POSE_TIMEOUT) || drivebase.getPose().equals(pose)) { + return true; + } else{ + return false; + } + } +} From 0d1bb370661b28b0004fe97459424f9be31330d7 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 24 Jan 2026 14:27:48 -0500 Subject: [PATCH 2/4] added drive command --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++------ .../drive/{DriveToPosition.java => Drive.java} | 8 ++++---- 3 files changed, 15 insertions(+), 10 deletions(-) rename src/main/java/frc/robot/commands/drive/{DriveToPosition.java => Drive.java} (80%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 047283c..56fa49d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -101,6 +101,7 @@ public void robotPeriodic() { } SmartDashboard.putNumber("driverXbox.getLeftY()",driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); + Logger.recordOutput("MyPose", robotContainer.getdrivebase().getPose()); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 404bf22..6e98959 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.drive.DriveToPosition; +import frc.robot.commands.drive.Drive; import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.roller.SpinRoller; import frc.robot.commands.tilt.TiltDown; @@ -103,11 +103,11 @@ private void configureBindings() { // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - Translation2d translation = new Translation2d(16,5); - Rotation2d rotation = new Rotation2d(0); - Pose2d pose = new Pose2d(translation,rotation); - Command DriveToPosition = new DriveToPosition(drivebase, pose, true); - driveJoystick.button(1).whileTrue(DriveToPosition); + + //basic command + Pose2d pose = new Pose2d(new Translation2d(1,1),new Rotation2d(5)); + Command drive = new Drive(drivebase, pose, true); + driveJoystick.button(1).whileTrue(drive); } public void putShuffleboardCommands() { if (Constants.DEBUG) { @@ -140,4 +140,8 @@ public Command getAutonomousCommand() { public RobotVisualizer getRobotVisualizer() { return robotVisualizer; } + public SwerveSubsystem getdrivebase(){ + return drivebase; + } } + diff --git a/src/main/java/frc/robot/commands/drive/DriveToPosition.java b/src/main/java/frc/robot/commands/drive/Drive.java similarity index 80% rename from src/main/java/frc/robot/commands/drive/DriveToPosition.java rename to src/main/java/frc/robot/commands/drive/Drive.java index dde42f9..9b36269 100644 --- a/src/main/java/frc/robot/commands/drive/DriveToPosition.java +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -10,13 +10,13 @@ import frc.robot.Constants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; -public class DriveToPosition extends Command { +public class Drive extends Command { private final SwerveSubsystem drivebase; private final Pose2d pose; private final boolean fieldRelative; private Timer timer; - public DriveToPosition(SwerveSubsystem drivebase, Pose2d pose, boolean fieldRelative) { + public Drive(SwerveSubsystem drivebase, Pose2d pose, boolean fieldRelative) { timer = new Timer(); this.drivebase = drivebase; this.pose = pose; @@ -33,7 +33,7 @@ public void initialize() { @Override public void execute() { - drivebase.drive(pose.getTranslation(),pose.getRotation().getDegrees(), fieldRelative); + drivebase.drive(pose.getTranslation(),pose.getRotation().getRadians(), fieldRelative); } @@ -45,7 +45,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - if (timer.hasElapsed(Constants.DRIVE_TO_POSE_TIMEOUT) || drivebase.getPose().equals(pose)) { + if (timer.hasElapsed(Constants.DRIVE_TO_POSE_TIMEOUT)) { return true; } else{ return false; From cf51eae04e77e4f66f6ac964de281a87c2d64114 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 24 Jan 2026 15:31:40 -0500 Subject: [PATCH 3/4] added a drive command to drive givin a speed and a time --- src/main/deploy/YAGSL/modules/backright.json | 2 +- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 5 ++-- .../java/frc/robot/commands/drive/Drive.java | 27 ++++++++++++++----- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/YAGSL/modules/backright.json b/src/main/deploy/YAGSL/modules/backright.json index 824b0e6..bf49ebe 100644 --- a/src/main/deploy/YAGSL/modules/backright.json +++ b/src/main/deploy/YAGSL/modules/backright.json @@ -18,7 +18,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": -108.896, + "absoluteEncoderOffset": 107.22, "location": { "front": -12, "left": -12 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 834bb25..3ac6b6f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,7 +56,6 @@ public enum Mode { public static final double INTAKE_SPEED = -0.5; public static final double SPIN_TIMEOUT = 5; public static final double TILT_TIMEOUT = 5; - public static final double DRIVE_TO_POSE_TIMEOUT = 10; public static final double TILT_LENGTH = 0.2; public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45); public static final Rotation2d TILT_MAX_ANGLE = Rotation2d.fromDegrees(90); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6e98959..5002639 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,9 +104,8 @@ private void configureBindings() { Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - //basic command - Pose2d pose = new Pose2d(new Translation2d(1,1),new Rotation2d(5)); - Command drive = new Drive(drivebase, pose, true); + //basic drive command + Command drive = new Drive(drivebase, 0.1,0.1,2, true, 1); driveJoystick.button(1).whileTrue(drive); } public void putShuffleboardCommands() { diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java index 9b36269..cb43168 100644 --- a/src/main/java/frc/robot/commands/drive/Drive.java +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -5,21 +5,34 @@ package frc.robot.commands.drive; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; - +/** + * Drives the robot + * @param speedX Speed in the x direction in meters per second + * @param speedY Speed in the Y direction in meters per second + * @param radians Rotational speed in radians per second + * @param fieldRelative True for field-relative, false for robot-relative. + * @param time Amount of time before the command ends + */ public class Drive extends Command { private final SwerveSubsystem drivebase; - private final Pose2d pose; + private final double speedX; + private final double speedY; + private final double radians; private final boolean fieldRelative; + private final double time; private Timer timer; - public Drive(SwerveSubsystem drivebase, Pose2d pose, boolean fieldRelative) { + public Drive(SwerveSubsystem drivebase, double speedX, double speedY,double radians, boolean fieldRelative, double time) { timer = new Timer(); + this.time = time; this.drivebase = drivebase; - this.pose = pose; + this.speedX = speedX; + this.speedY = speedY; + this.radians = radians; this.fieldRelative = fieldRelative; addRequirements(drivebase); } @@ -33,7 +46,7 @@ public void initialize() { @Override public void execute() { - drivebase.drive(pose.getTranslation(),pose.getRotation().getRadians(), fieldRelative); + drivebase.drive(new Translation2d(speedX,speedY),radians, fieldRelative); } @@ -45,7 +58,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - if (timer.hasElapsed(Constants.DRIVE_TO_POSE_TIMEOUT)) { + if (timer.hasElapsed(time)) { return true; } else{ return false; From 8f8484566ff32a61c1ba8f7a6a1f4cc92b6bbb49 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 24 Jan 2026 15:37:36 -0500 Subject: [PATCH 4/4] update --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 3 --- 2 files changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 56fa49d..047283c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -101,7 +101,6 @@ public void robotPeriodic() { } SmartDashboard.putNumber("driverXbox.getLeftY()",driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); - Logger.recordOutput("MyPose", robotContainer.getdrivebase().getPose()); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5002639..e1b7242 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -139,8 +139,5 @@ public Command getAutonomousCommand() { public RobotVisualizer getRobotVisualizer() { return robotVisualizer; } - public SwerveSubsystem getdrivebase(){ - return drivebase; - } }