From 299d2acaebcd05dee912448f61b2ad92944b78d6 Mon Sep 17 00:00:00 2001 From: SeizedThoughts Date: Fri, 14 May 2021 20:16:02 -0700 Subject: [PATCH 1/2] switched drive to CANPIDController --- .../org/team199/robot2021/SwerveModule.java | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/team199/robot2021/SwerveModule.java b/src/main/java/org/team199/robot2021/SwerveModule.java index aa14a1a..ab80511 100644 --- a/src/main/java/org/team199/robot2021/SwerveModule.java +++ b/src/main/java/org/team199/robot2021/SwerveModule.java @@ -4,6 +4,8 @@ import com.ctre.phoenix.sensors.AbsoluteSensorRange; import com.ctre.phoenix.sensors.CANCoder; +import com.revrobotics.ControlType; +import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; @@ -30,7 +32,7 @@ public enum ModuleType {FL, FR, BL, BR}; private String moduleString; private CANSparkMax drive, turn; private CANCoder turnEncoder; - private PIDController drivePIDController; + private CANPIDController drivePIDController; private ProfiledPIDController turnPIDController; private TrapezoidProfile.Constraints turnConstraints; private double driveModifier, maxSpeed, turnZero; @@ -86,9 +88,12 @@ public SwerveModule(ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCod Constants.DriveConstants.kBackwardVels[arrIndex], Constants.DriveConstants.kBackwardAccels[arrIndex]); - drivePIDController = new PIDController(2 * Constants.DriveConstants.drivekP[arrIndex], - Constants.DriveConstants.drivekI[arrIndex], - Constants.DriveConstants.drivekD[arrIndex]); + drivePIDController = drive.drivePIDController(); + + drivePIDController.setP(Constants.DriveConstants.drivekP[arrIndex]); + drivePIDController.setI(Constants.DriveConstants.drivekI[arrIndex]); + drivePIDController.setD(Constants.DriveConstants.drivekD[arrIndex]); + drivePIDController.setOutputRange(-1, 1); //System.out.println("Velocity Constant: " + (positionConstant / 60)); @@ -194,17 +199,9 @@ private void setSpeed(double speed) { // Compute desired and actual speeds in m/s double desiredSpeed = maxSpeed * speed * driveModifier; - double actualSpeed = getCurrentSpeed(); SmartDashboard.putNumber(moduleString + " Desired Speed (mps)", desiredSpeed); - SmartDashboard.putNumber(moduleString + " Actual Speed (mps)", actualSpeed); - double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : - backwardSimpleMotorFF).calculate(desiredSpeed, calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()));//clippedAcceleration); - - // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. - // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) - targetVoltage += drivePIDController.calculate(actualSpeed, desiredSpeed); - double appliedVoltage = MathUtil.clamp(targetVoltage / 12, -1, 1); - drive.set(appliedVoltage); + + drivePIDController.setReference(desiredSpeed, ControlType.kVelocity); } /** From abab2237f3312c2435010282fa0f278cff158123 Mon Sep 17 00:00:00 2001 From: SeizedThoughts Date: Fri, 14 May 2021 20:45:35 -0700 Subject: [PATCH 2/2] getPIDController not drivePIDController --- lib199 | 2 +- src/main/java/org/team199/robot2021/SwerveModule.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lib199 b/lib199 index bf37637..a2a01f6 160000 --- a/lib199 +++ b/lib199 @@ -1 +1 @@ -Subproject commit bf376375a22488cb5f56f7576bc40087cb752083 +Subproject commit a2a01f6e6c1338f86e01ee6e86b8a13da554ceaa diff --git a/src/main/java/org/team199/robot2021/SwerveModule.java b/src/main/java/org/team199/robot2021/SwerveModule.java index ab80511..c163754 100644 --- a/src/main/java/org/team199/robot2021/SwerveModule.java +++ b/src/main/java/org/team199/robot2021/SwerveModule.java @@ -88,7 +88,7 @@ public SwerveModule(ModuleType type, CANSparkMax drive, CANSparkMax turn, CANCod Constants.DriveConstants.kBackwardVels[arrIndex], Constants.DriveConstants.kBackwardAccels[arrIndex]); - drivePIDController = drive.drivePIDController(); + drivePIDController = drive.getPIDController(); drivePIDController.setP(Constants.DriveConstants.drivekP[arrIndex]); drivePIDController.setI(Constants.DriveConstants.drivekI[arrIndex]);