diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7079abf9..21d4a9dd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,7 @@ public final class Constants { public static final double DRIVE_PID_FF_S = 1; public static final double DRIVE_PID_FF_V = 2.8; - public static final double STEER_PID_P = 0.3; + public static final double STEER_PID_P = .4; public static final double STEER_PID_I = 0; public static final double STEER_PID_D = 0; public static final double STEER_PID_FF_S = 0;//0.2; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4ca48c7..3c1ee65d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,12 +14,15 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.autochooser.chooser.AutoChooser; import frc.robot.commands.RampMove; import frc.robot.commands.ReportErrorCommand; import frc.robot.autochooser.chooser.AutoChooser2024; import frc.robot.commands.SetInitOdom; +import frc.robot.commands.drive.WheelAlign; import frc.robot.subsystems.Ramp; import frc.robot.subsystems.swervev2.KinematicsConversionConfig; import frc.robot.subsystems.swervev2.SwerveDrivetrain; @@ -91,10 +94,17 @@ private void setupDriveTrain() { SwervePidConfig pidConfig = new SwervePidConfig(drivePid, steerPid, driveGain, steerGain, constraints); AHRS navxGyro = new AHRS(); this.drivetrain = new SwerveDrivetrain(frontLeftIdConf, frontRightIdConf, backLeftIdConf, backRightIdConf, kinematicsConversionConfig, pidConfig, navxGyro); + } public void putShuffleboardCommands() { SmartShuffleboard.putCommand("Ramp", "SetArmPID400", new RampMove(ramp, 400)); SmartShuffleboard.putCommand("Ramp", "SetArmPID500", new RampMove(ramp, 500)); + SmartShuffleboard.put("Test", "P", Constants.STEER_PID_P); + SmartShuffleboard.put("Test", "I", Constants.STEER_PID_I); + SmartShuffleboard.put("Test", "D", Constants.STEER_PID_D); + + + } diff --git a/src/main/java/frc/robot/subsystems/swervev2/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev2/SwerveDrivetrain.java index e93b3a66..05459eb4 100644 --- a/src/main/java/frc/robot/subsystems/swervev2/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev2/SwerveDrivetrain.java @@ -45,6 +45,19 @@ private double getGyro() { public void periodic() { gyroValue = getGyro(); poseEstimator.updatePosition(gyroValue); + frontLeft.setP(SmartShuffleboard.getDouble("Test", "P", Constants.STEER_PID_P)); + frontRight.setP(SmartShuffleboard.getDouble("Test", "P", Constants.STEER_PID_P)); + backRight.setP(SmartShuffleboard.getDouble("Test", "P", Constants.STEER_PID_P)); + backLeft.setP(SmartShuffleboard.getDouble("Test", "P", Constants.STEER_PID_P)); + frontLeft.setI(SmartShuffleboard.getDouble("Test", "I", Constants.STEER_PID_I)); + frontRight.setI(SmartShuffleboard.getDouble("Test", "I", Constants.STEER_PID_I)); + backRight.setI(SmartShuffleboard.getDouble("Test", "I", Constants.STEER_PID_I)); + backLeft.setI(SmartShuffleboard.getDouble("Test", "I", Constants.STEER_PID_I)); + frontLeft.setD(SmartShuffleboard.getDouble("Test", "D", Constants.STEER_PID_D)); + frontRight.setD(SmartShuffleboard.getDouble("Test", "D", Constants.STEER_PID_D)); + backRight.setD(SmartShuffleboard.getDouble("Test", "D", Constants.STEER_PID_D)); + backLeft.setD(SmartShuffleboard.getDouble("Test", "D", Constants.STEER_PID_D)); + } public SwerveDrivetrain(SwerveIdConfig frontLeftConfig, SwerveIdConfig frontRightConfig, SwerveIdConfig backLeftConfig, SwerveIdConfig backRightConfig, diff --git a/src/main/java/frc/robot/subsystems/swervev2/type/GenericSwerveModule.java b/src/main/java/frc/robot/subsystems/swervev2/type/GenericSwerveModule.java index 77704c03..3cde737f 100644 --- a/src/main/java/frc/robot/subsystems/swervev2/type/GenericSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev2/type/GenericSwerveModule.java @@ -63,4 +63,17 @@ public GenericEncodedSwerve getSwerveMotor(){ return swerveMotor; } + public void setP(double P) { + turningPIDController.setP(P); + } + + public void setI(double I) { + turningPIDController.setI(I); + } + + public void setD(double D) { + turningPIDController.setD(D); + } + + }