From 0c9879c851cd68d0bcbfb744ce978ad1332d755a Mon Sep 17 00:00:00 2001 From: PGgit08 <60079012+PGgit08@users.noreply.github.com> Date: Wed, 24 Jan 2024 22:53:28 -0500 Subject: [PATCH 1/2] switched to SimpleMotorFeedforward for velocity control in SwerveModule --- src/main/java/frc/robot/Constants.java | 6 +++++- .../subsystems/SwerveDriveSubsystem.java | 20 ++++++++++++++++++- .../java/frc/robot/utils/SwerveModule.java | 9 ++++++++- src/main/java/frc/robot/utils/UtilFuncs.java | 12 +++++++++++ 4 files changed, 44 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c739f1b..a39b37c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -76,7 +77,10 @@ public static class Physical { } public static class FeedForward { - public static final double ELEVATOR_KG = 0.0; // TODO: Find Kg constant. + public static final double ELEVATOR_KG = 0.0; // TODO: Find Kg constants + + public static final double MODULE_DRIVE_KS = 0.0; + public static final double MODULE_DRIVE_KV = 0.0; } public static class PID { diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 3745b14..cb4f55c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -14,6 +14,8 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.DriverStation; @@ -22,12 +24,14 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.utils.BNO055; import frc.robot.utils.SwerveModule; +import frc.robot.utils.UtilFuncs; + import java.util.Optional; -import javax.swing.text.html.Option; /** * @author Peter Gutkovich @@ -101,6 +105,20 @@ public class SwerveDriveSubsystem extends SubsystemBase { StructArrayPublisher publisher = NetworkTableInstance.getDefault().getStructArrayTopic("MyStates", SwerveModuleState.struct).publish(); + // private final SysIdRoutine _sysID = new SysIdRoutine( + // new SysIdRoutine.Config(), + // new SysIdRoutine.Mechanism( + // (volts) -> { + // driveTest(UtilFuncs.FromVolts(volts.in(Units.Volts))); + // }, + // (log) -> { + // new Measure() + // // log.motor("Front Left").linearVelocity(_frontLeft.getDriveVelocity()); + // }, + // this + // ) + // ); + // Pose Estimator -> Has built in odometry and uses supplied vision measurements private final SwerveDrivePoseEstimator _estimator = new SwerveDrivePoseEstimator( Constants.Physical.SWERVE_KINEMATICS, diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java index 7ea11fa..94e0cce 100644 --- a/src/main/java/frc/robot/utils/SwerveModule.java +++ b/src/main/java/frc/robot/utils/SwerveModule.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -24,6 +25,11 @@ public class SwerveModule { private final PIDController _driveController; private final PIDController _rotationController; + private final SimpleMotorFeedforward _driveFeedforward = new SimpleMotorFeedforward( + Constants.FeedForward.MODULE_DRIVE_KS, + Constants.FeedForward.MODULE_DRIVE_KV + ); + private final CANcoder _encoder; private final String _name; @@ -153,7 +159,8 @@ public void setState(SwerveModuleState state) { 0.150 ); - double drive_feedforward = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); + // double drive_feedforward = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); + double drive_feedforward = UtilFuncs.FromVolts(_driveFeedforward.calculate(speed)); double drive_pid = _driveController.calculate(getDriveVelocity(), speed); rotate(rotation_pid); diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index eddafb7..70251c2 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/src/main/java/frc/robot/utils/UtilFuncs.java @@ -3,6 +3,8 @@ package frc.robot.utils; +import edu.wpi.first.wpilibj.RobotController; + /** Any utility functions are here. */ public final class UtilFuncs { /** @@ -19,4 +21,14 @@ public static double ApplyDeadband(double val, double deadband) { return 0; } + + /** + * Control a motor controller with voltage by converting a voltage output into percent output. + * + * @param volts The voltage output. + * @return The percent output to set the controller to. + */ + public static double FromVolts(double volts) { + return volts / RobotController.getBatteryVoltage(); + } } From 86e253b27ce97173a5fedd69561743e2efb98cb9 Mon Sep 17 00:00:00 2001 From: Peter Gutkovich <60079012+PGgit08@users.noreply.github.com> Date: Thu, 25 Jan 2024 08:45:21 -0500 Subject: [PATCH 2/2] Update UtilFuncs.java --- src/main/java/frc/robot/utils/UtilFuncs.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index 70251c2..e3b2c71 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/src/main/java/frc/robot/utils/UtilFuncs.java @@ -23,12 +23,12 @@ public static double ApplyDeadband(double val, double deadband) { } /** - * Control a motor controller with voltage by converting a voltage output into percent output. + * Control a motor controller with voltage by converting a voltage output into percent output with a scale factor. * * @param volts The voltage output. * @return The percent output to set the controller to. */ public static double FromVolts(double volts) { - return volts / RobotController.getBatteryVoltage(); + return volts / 12.0; } }