From e65de10e34e3eecca4c9a495bd0ec23159fe5f32 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Mon, 10 Mar 2025 19:01:10 -0400 Subject: [PATCH 1/2] wha --- .../frc/robot/drivetrain/TunerConstants.java | 40 ++++++++++++++++- .../drivetrain/commands/DriveToPose.java | 43 +++++++++++++++++++ 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/drivetrain/commands/DriveToPose.java diff --git a/src/main/java/frc/robot/drivetrain/TunerConstants.java b/src/main/java/frc/robot/drivetrain/TunerConstants.java index 2545aa8..6e10530 100644 --- a/src/main/java/frc/robot/drivetrain/TunerConstants.java +++ b/src/main/java/frc/robot/drivetrain/TunerConstants.java @@ -14,8 +14,9 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.*; - +import frc.robot.RobotContainer; import frc.robot.drivetrain.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator @@ -291,6 +292,43 @@ public TunerSwerveDrivetrain( ); } } + + public static class DriveCommandsConstants{ + + //TODO: tune these constants correctly so robot doesn't go haywire + public static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); + public static final Angle THETA_TOLERANCE = Degrees.of(1.0); + + public static final LinearVelocity MAX_DRIVE_POSE_TRANS_VELOCITY = MetersPerSecond.of(RobotContainer.kMaxVelocity); + public static final LinearAcceleration MAX_DRIVE_POSE_TRANS_ACCELERATION = MetersPerSecondPerSecond.of(3.0); + + public static final AngularVelocity MAX_DRIVE_POSE_ANGULAR_VELOCITY = RadiansPerSecond.of(RobotContainer.kMaxAngularVelocity); + public static final AngularAcceleration MAX_DRIVE_POSE_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(3 * Math.PI); + + public static final double THETA_kP = 0.0; + public static final double THETA_kI = 0.0; + public static final double THETA_kD = 0.0; + + public static final double X_kP = 0.0; + public static final double X_kI = 0.0; + public static final double X_kD = 0.0; + + public static final double Y_kP = 0.0; + public static final double Y_kI = 0.0; + public static final double Y_kD = 0.0; + + public static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = + new TrapezoidProfile.Constraints( + MAX_DRIVE_POSE_TRANS_VELOCITY.in(MetersPerSecond), + MAX_DRIVE_POSE_TRANS_ACCELERATION.in(MetersPerSecondPerSecond)); + + public static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = + new TrapezoidProfile.Constraints( + MAX_DRIVE_POSE_ANGULAR_VELOCITY.in(RadiansPerSecond), + MAX_DRIVE_POSE_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); + + + } } diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java b/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java new file mode 100644 index 0000000..226cf93 --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java @@ -0,0 +1,43 @@ +package frc.robot.drivetrain.commands; + +import static com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType.MotionMagicExpo; +import static com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.Velocity; +import static frc.robot.drivetrain.TunerConstants.DriveCommandsConstants; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.drivetrain.CommandSwerveDrivetrain; + +public class DriveToPose extends Command{ + + private final ProfiledPIDController xController; + private final ProfiledPIDController yController; + private final ProfiledPIDController omegaController; + private final ForwardPerspectiveValue perspectiveValue; + + private final CommandSwerveDrivetrain swerveDrivetrain; + private final SwerveRequest.FieldCentric fieldCentricSwerveRequest; + + private final Supplier poseSupplier; + // = new SwerveRequest.FieldCentric() + // .withSteerRequestType(MotionMagicExpo) + // .withDriveRequestType(Velocity) + // .withForwardPerspective(perspectiveValue); + + + /** + * Constructs a DriveToPoseCommand + * + * @param drivetrainSubsystem drivetrain subsystem + * @param goalPose goal pose to drive to + */ + public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider) { + this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); + } +} From 8be96e70e73270ed4f471f97cac1d1cdae4c142e Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Mon, 17 Mar 2025 07:03:11 -0400 Subject: [PATCH 2/2] disabled webserver --- .../roborioteamnumbersetter.json | 1 + .../drivetrain/CommandSwerveDrivetrain.java | 4 +- .../drivetrain/commands/DriveToPose.java | 66 +++++++++---------- 3 files changed, 36 insertions(+), 35 deletions(-) create mode 100644 .roboRIOTeamNumberSetter/roborioteamnumbersetter.json diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java index 6655d4a..8c5cdaa 100644 --- a/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java @@ -276,8 +276,8 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; } - System.out.println("Estimated Pose X: " + PoseEstimatorSubsystem.getInstance().getCurrentPose().getX()); - System.out.println("Estimated Pose Y: " + PoseEstimatorSubsystem.getInstance().getCurrentPose().getY()); + // System.out.println("Estimated Pose X: " + PoseEstimatorSubsystem.getInstance().getCurrentPose().getX()); + // System.out.println("Estimated Pose Y: " + PoseEstimatorSubsystem.getInstance().getCurrentPose().getY()); // SmartDashboard.putNumber("Encoder Pos FL", getState().ModulePositions[0].angle.getRotations()); // SmartDashboard.putNumber("Encoder Pos FR", getState().ModulePositions[1].angle.getRotations()); diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java b/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java index 226cf93..eaa2735 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveToPose.java @@ -1,43 +1,43 @@ -package frc.robot.drivetrain.commands; +// package frc.robot.drivetrain.commands; -import static com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType.MotionMagicExpo; -import static com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.Velocity; -import static frc.robot.drivetrain.TunerConstants.DriveCommandsConstants; +// import static com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType.MotionMagicExpo; +// import static com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.Velocity; +// import static frc.robot.drivetrain.TunerConstants.DriveCommandsConstants; -import java.util.function.Supplier; +// import java.util.function.Supplier; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +// import com.ctre.phoenix6.swerve.SwerveRequest; +// import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.drivetrain.CommandSwerveDrivetrain; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.drivetrain.CommandSwerveDrivetrain; -public class DriveToPose extends Command{ +// public class DriveToPose extends Command{ - private final ProfiledPIDController xController; - private final ProfiledPIDController yController; - private final ProfiledPIDController omegaController; - private final ForwardPerspectiveValue perspectiveValue; +// private final ProfiledPIDController xController; +// private final ProfiledPIDController yController; +// private final ProfiledPIDController omegaController; +// private final ForwardPerspectiveValue perspectiveValue; - private final CommandSwerveDrivetrain swerveDrivetrain; - private final SwerveRequest.FieldCentric fieldCentricSwerveRequest; +// private final CommandSwerveDrivetrain swerveDrivetrain; +// private final SwerveRequest.FieldCentric fieldCentricSwerveRequest; - private final Supplier poseSupplier; - // = new SwerveRequest.FieldCentric() - // .withSteerRequestType(MotionMagicExpo) - // .withDriveRequestType(Velocity) - // .withForwardPerspective(perspectiveValue); +// private final Supplier poseSupplier; +// // = new SwerveRequest.FieldCentric() +// // .withSteerRequestType(MotionMagicExpo) +// // .withDriveRequestType(Velocity) +// // .withForwardPerspective(perspectiveValue); - /** - * Constructs a DriveToPoseCommand - * - * @param drivetrainSubsystem drivetrain subsystem - * @param goalPose goal pose to drive to - */ - public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider) { - this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); - } -} +// /** +// * Constructs a DriveToPoseCommand +// * +// * @param drivetrainSubsystem drivetrain subsystem +// * @param goalPose goal pose to drive to +// */ +// public DriveToPoseCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider) { +// this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); +// } +// }