From e3519ff1417eabba8f68189e205fa1f5ec928a2d Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Sun, 24 Mar 2024 23:14:43 -0400 Subject: [PATCH] added current limits to swerve drive motors (make sure it works tho) --- src/main/java/frc/robot/Constants.java | 1 - .../java/frc/robot/utils/SwerveModule.java | 28 ++++++++++++------- .../robot/utils/configs/TalonFXConfig.java | 2 -- 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f051805..6ba0c55 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -149,7 +149,6 @@ public static class Presets { public static final double SHOOTER_AMP_HANDOFF = 50; public static final double ELEVATOR_AMP_HANDOFF = 0.04; - // TODO: get values for these public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap(); static { diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java index 768f583..d77befb 100644 --- a/src/main/java/frc/robot/utils/SwerveModule.java +++ b/src/main/java/frc/robot/utils/SwerveModule.java @@ -1,6 +1,7 @@ /* Copyright (C) 2024 Team 334. All Rights Reserved.*/ package frc.robot.utils; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.MathUtil; @@ -51,18 +52,8 @@ public class SwerveModule { public SwerveModule(String name, int driveMotorId, int rotationMotorId, int encoderId) { _driveMotor = new TalonFX(driveMotorId); _rotationMotor = new TalonFX(rotationMotorId); - - // NO NEED FOR THIS CODE ANYMORE, FIGURED HOW TO DO OFFSETS IN PHOENIX TUNER - // new stuff because CTRE update - // MagnetSensorConfigs encoderConfig = new MagnetSensorConfigs(); - // encoderConfig.AbsoluteSensorRange = - // AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - // encoderConfig.MagnetOffset = (angleOffset / 180) / 2; - _encoder = new CANcoder(encoderId); - // _encoder.getConfigurator().apply(encoderConfig); - _name = name; _driveController = new PIDController(PID.MODULE_DRIVE_KP, 0, 0); @@ -72,12 +63,29 @@ public SwerveModule(String name, int driveMotorId, int rotationMotorId, int enco TalonFXConfig.configureFalcon(_driveMotor, false); TalonFXConfig.configureFalcon(_rotationMotor, true); + + // TODO: make sure this works + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + + currentConfig.SupplyCurrentLimit = 60; + currentConfig.SupplyCurrentThreshold = 30; + currentConfig.SupplyTimeThreshold = 0.9; + + currentConfig.StatorCurrentLimit = 120; + + currentConfig.SupplyCurrentLimitEnable = false; + currentConfig.StatorCurrentLimitEnable = false; + + _driveMotor.getConfigurator().apply(currentConfig); } /** Display's this module's info on SmartDashboard through a supplied builder. */ public void displayInfo(SendableBuilder builder) { builder.addDoubleProperty(_name + " Angle", () -> getAngle(), null); builder.addDoubleProperty(_name + " Velocity", () -> getDriveVelocity(), null); + + builder.addDoubleProperty(_name + " Drive Supply Current", () -> _driveMotor.getSupplyCurrent().getValueAsDouble(), null); + builder.addDoubleProperty(_name + " Drive Stator Current", () -> _driveMotor.getStatorCurrent().getValueAsDouble(), null); } /** diff --git a/src/main/java/frc/robot/utils/configs/TalonFXConfig.java b/src/main/java/frc/robot/utils/configs/TalonFXConfig.java index 085c476..b9b277c 100644 --- a/src/main/java/frc/robot/utils/configs/TalonFXConfig.java +++ b/src/main/java/frc/robot/utils/configs/TalonFXConfig.java @@ -43,8 +43,6 @@ public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean inver config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - // config.CurrentLimits.StatorCurrentLimit = 80; // TODO: Find - falcon.getConfigurator().apply(config); falcon.setPosition(0);