From 9c0b33932efdc5a7808f208d32c160ba4ccec824 Mon Sep 17 00:00:00 2001 From: peachswag05 Date: Sat, 4 Feb 2023 12:36:50 -0700 Subject: [PATCH 1/3] pid shit --- src/main/java/frc/robot/Constants.java | 5 ++++ .../java/frc/robot/subsystems/Drivetrain.java | 30 ++++++++++++++----- 2 files changed, 28 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c958197..0694b0b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,5 +41,10 @@ public static class ControlConstants { public static final double MAX_ROBOT_SPEED = 1; public static final double MIN_ROBOT_SPEED = -1; public static final double MAX_TURN_SPEED = 0.7; + + public static final double kp = 0.2; + public static final double ki = 0.2; + public static final double kd = 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index aa7ed44..1eaafcb 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -29,6 +29,10 @@ import com.revrobotics.RelativeEncoder; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import edu.wpi.first.math.controller.PIDController; + + public class Drivetrain extends SubsystemBase { private CANSparkMax frontLeft; private CANSparkMax frontRight; @@ -39,6 +43,11 @@ public class Drivetrain extends SubsystemBase { private RelativeEncoder backRighte; private RelativeEncoder frontLefte; private boolean finetuned; + + private PIDController pidFR; + private PIDController pidBR; + private PIDController pidFL; + private PIDController pidBL; public Drivetrain() { @@ -52,10 +61,14 @@ public Drivetrain() { backLefte = backLeft.getEncoder(); backRighte = backRight.getEncoder(); + pidFR = new PIDController(kP,kI,kD); + pidBR = new PIDController(kP,kI,kD); + pidFL = new PIDController(kP,kI,kD); + pidBL = new PIDController(kP,kI,kD); - } + } @Override public void periodic() { @@ -75,6 +88,11 @@ public void periodic() { SmartDashboard.putNumber("CountsPerRevolution", CPR); SmartDashboard.putNumber("Revolutions", revolutions); + double kP = 0.2; + double kD = 0.1; + double kI = 0; + + } // 2020 mecanum drive code @@ -109,8 +127,6 @@ public void mecanumDrive(double joystickX, double joystickY, double rotation) { - - //finetuned driving system if (finetuned == true) { @@ -127,10 +143,10 @@ public void mecanumDrive(double joystickX, double joystickY, double rotation) { // Power the motors - frontLeft.set(frontLeftPower); - frontRight.set(frontRightPower); - backLeft.set(backLeftPower); - backRight.set(backRightPower); + frontLeft.set(pidFL.calculate(frontLefte.getVelocity(), frontLeftPower)); + frontRight.set(pidFR.calculate(frontRighte.getVelocity(), frontRightPower)); + backLeft.set(pidBL.calculate(backLefte.getVelocity(), frontLeftPower)); + backRight.set(pidBR.calculate(backRighte.getVelocity(), frontLeftPower)); } public double getBackRightPosition() { From f37062db1e1676ef7fa5f24e978e4bb8ee9dee8b Mon Sep 17 00:00:00 2001 From: peachswag05 Date: Sat, 4 Feb 2023 12:56:17 -0700 Subject: [PATCH 2/3] girlboss update --- src/main/java/frc/robot/Constants.java | 4 +--- src/main/java/frc/robot/subsystems/Drivetrain.java | 7 ++++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0694b0b..58c5714 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,9 +42,7 @@ public static class ControlConstants { public static final double MIN_ROBOT_SPEED = -1; public static final double MAX_TURN_SPEED = 0.7; - public static final double kp = 0.2; - public static final double ki = 0.2; - public static final double kd = 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 1eaafcb..37b8146 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -61,6 +61,10 @@ public Drivetrain() { backLefte = backLeft.getEncoder(); backRighte = backRight.getEncoder(); + double kP = 0.2; + double kI = 0.1; + double kD = 0.0; + pidFR = new PIDController(kP,kI,kD); pidBR = new PIDController(kP,kI,kD); pidFL = new PIDController(kP,kI,kD); @@ -88,9 +92,6 @@ public void periodic() { SmartDashboard.putNumber("CountsPerRevolution", CPR); SmartDashboard.putNumber("Revolutions", revolutions); - double kP = 0.2; - double kD = 0.1; - double kI = 0; } From 3b1025fa789349ffac572175958f70c175db3593 Mon Sep 17 00:00:00 2001 From: peachswag05 Date: Sat, 4 Feb 2023 14:24:11 -0700 Subject: [PATCH 3/3] . --- src/main/java/frc/robot/subsystems/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 37b8146..55d3b77 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -62,7 +62,7 @@ public Drivetrain() { backRighte = backRight.getEncoder(); double kP = 0.2; - double kI = 0.1; + double kI = 0.05; double kD = 0.0; pidFR = new PIDController(kP,kI,kD);