disabled rio webserver

.roboRIOTeamNumberSetter/roborioteamnumbersetter.json

@@ -0,0 +1 @@
+{}

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());

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));
+
+
+    } 
 }
 
 

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<Pose2d> 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<Pose2d> poseProvider) {
+//         this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS);
+//   }
+// }