diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java index f1b5714..5bba6eb 100644 --- a/src/main/java/frc/robot/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/drive/DriveSubsystem.java @@ -70,10 +70,21 @@ public class DriveSubsystem extends SubsystemBase { MAX_VELOCITY = Math.abs(maxChassisSpeedsForward.vxMetersPerSecond); MAX_ANGULAR_VELOCITY = Math.abs(maxChassisSpeedsSpinning.omegaRadiansPerSecond); + System.out.println("maxChassisSpeedsSpinning:"); + System.out.println(maxChassisSpeedsSpinning); + MAX_ACCELERATION = Wheel.MAX_ACCELERATION; // TODO: This is the wheel's angular acceleration, do we need a different value for the robot // itself? MAX_ANGULAR_ACCELERATION = Wheel.MAX_ANGULAR_ACCELERATION; + System.out.println("MAX_VELOCITY"); + System.out.println(MAX_VELOCITY); + System.out.println("MAX_ANGULAR_VELOCITY"); + System.out.println(MAX_ANGULAR_VELOCITY); + System.out.println("MAX_ACCELERATION"); + System.out.println(MAX_ACCELERATION); + System.out.println("MAX_ANGULAR_ACCELERATION"); + System.out.println(MAX_ANGULAR_ACCELERATION); } private static final TrapezoidProfile.Constraints MAX_ROTATION = @@ -85,7 +96,7 @@ public class DriveSubsystem extends SubsystemBase { public final TrajectoryConfig trajectoryConfig; private final ProfiledPIDController thetaController = - new ProfiledPIDController(1, 0, 0, MAX_ROTATION, Constants.PERIOD_SECONDS); + new ProfiledPIDController(2, 0.3, 0.2, MAX_ROTATION, Constants.PERIOD_SECONDS); // Used for following trajectories public final HolonomicDriveController driveController = @@ -151,7 +162,8 @@ private void driveTeleop( ChassisSpeeds.fromFieldRelativeSpeeds( yPercentage * MAX_VELOCITY, -xPercentage * MAX_VELOCITY, - thetaControllerVelocity, + // TODO: This ignores the PID output + thetaPercentage * MAX_ANGULAR_VELOCITY, robotHeading); // TODO: Stop logging this after debugging is finished