diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20194e84..71f71761 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.robot.commands.coral.IntakeCoral; import frc.robot.commands.coral.ShootCoral; import frc.robot.commands.drivetrain.Drive; +import frc.robot.commands.drivetrain.DriveVoltage; import frc.robot.commands.drivetrain.RobotSlide; import frc.robot.commands.elevator.*; import frc.robot.commands.hihi.*; @@ -302,7 +303,14 @@ public SwerveDrivetrain getDrivetrain() { } public void putShuffleboardCommands() { - + if (Constants.SWERVE_DEBUG) { + SmartShuffleboard.put("Drive", "VoltsToRunWith", 0); + SmartShuffleboard.putCommand( + "Drive", + "RunWithVolts", + new DriveVoltage( + drivetrain, () -> SmartShuffleboard.getDouble("Drive", "VoltsToRunWith", 0), 5)); + } if (Constants.CORAL_DEBUG) { SmartShuffleboard.putCommand( "Commands", "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java b/src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java new file mode 100644 index 00000000..02cda8a1 --- /dev/null +++ b/src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java @@ -0,0 +1,41 @@ +package frc.robot.commands.drivetrain; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; +import java.util.function.DoubleSupplier; + +/** + * This class can be used to calculate kS value for {@link + * edu.wpi.first.math.controller.SimpleMotorFeedforward SimpleMotorFeedforward}.
+ * Keep applying volts until the motors are able to move the robot at all.
+ * kS represents the volts need to overcome static friction + */ +public class DriveVoltage extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private final DoubleSupplier volts; + private final double time; + private final Timer timer; + + public DriveVoltage(SwerveDrivetrain drivetrain, DoubleSupplier volts, double time) { + this.drivetrain = drivetrain; + this.volts = volts; + this.time = time; + this.timer = new Timer(); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + drivetrain.applyVolts(volts.getAsDouble()); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(time); + } +} diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java b/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java index 40f8c279..47ebe7c9 100644 --- a/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java +++ b/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java @@ -22,7 +22,7 @@ public RobotSlide(SwerveDrivetrain drivetrain, DoubleSupplier horizSupplier) { @Override public void execute() { double horizVal = -horizSupplier.getAsDouble(); - double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY; + double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY / 2; ChassisSpeeds speeds = drivetrain.createChassisSpeeds(0, str, 0, DriveMode.ROBOT_CENTRIC); drivetrain.drive(speeds); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index f677ed1b..caea8e2a 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -98,9 +98,9 @@ public enum Mode { } // Limits - public static final int DRIVE_SMART_LIMIT = 38; // TODO: change later - public static final int DRIVE_SECONDARY_LIMIT = 48; // TODO: change later - public static final double DRIVE_RAMP_RATE_LIMIT = 0.1; // TODO: change later + public static final int DRIVE_SMART_LIMIT = 40; // TODO: change later + public static final int DRIVE_SECONDARY_LIMIT = 0; // TODO: change later + public static final double DRIVE_RAMP_RATE_LIMIT = 0; // TODO: change later public static final int NEO_CURRENT_LIMIT = 20; public static final int HIHI_CURRENT_LIMIT = 10; @@ -126,11 +126,13 @@ public enum Mode { public static final boolean ELEVATOR_USE_MAX_MOTION = true; // Drive PID - public static final double DRIVE_PID_P = 1; // TODO: change later + public static final double DRIVE_PID_P = 2; // TODO: change later public static final double DRIVE_PID_I = 0; // TODO: change later public static final double DRIVE_PID_D = 0; // TODO: change later - public static final double DRIVE_PID_FF_S = 1; // TODO: change later - public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later + public static final double DRIVE_PID_FF_S = 0.19; + public static final double DRIVE_PID_FF_V = 3.3; + public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later + public static final double DRIVE_PID_ALLOWED_ERROR = 0; // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 2eaef416..d384d6c5 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -17,8 +17,8 @@ import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; +import frc.robot.utils.logging.LoggedTunableNumber; import frc.robot.utils.logging.subsystem.LoggableSystem; -import frc.robot.utils.shuffleboard.SmartShuffleboard; import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { @@ -42,6 +42,20 @@ public class SwerveDrivetrain extends SubsystemBase { private final PoseEstimator poseEstimator; private boolean facingTarget = false; + private LoggedTunableNumber closedLoopTunable; + private LoggedTunableNumber smartLimitTunable; + private LoggedTunableNumber secondaryLimitTunable; + private LoggedTunableNumber drivePTunable; + private LoggedTunableNumber driveITunable; + private LoggedTunableNumber driveDTunable; + private LoggedTunableNumber driveKvTunable; + private LoggedTunableNumber driveKsTunable; + private LoggedTunableNumber steerPTunable; + private LoggedTunableNumber steerITunable; + private LoggedTunableNumber steerDTunable; + private LoggedTunableNumber steerMaxAccelerationTunable; + private LoggedTunableNumber steerMaxVelocityTunable; + public SwerveDrivetrain( SwerveModule frontLeftModule, SwerveModule frontRightModule, @@ -57,6 +71,30 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); + if (Constants.TUNING_MODE) { + closedLoopTunable = + new LoggedTunableNumber( + "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); + smartLimitTunable = + new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT); + secondaryLimitTunable = + new LoggedTunableNumber( + "Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); + drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); + driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); + driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); + + driveKvTunable = new LoggedTunableNumber("Swerve/drive/Kv", Constants.DRIVE_PID_FF_V); + driveKsTunable = new LoggedTunableNumber("Swerve/drive/Ks", Constants.DRIVE_PID_FF_S); + + steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P); + steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I); + steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D); + steerMaxAccelerationTunable = + new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150); + steerMaxVelocityTunable = + new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); + } } @Override @@ -81,11 +119,84 @@ public void periodic() { frontRight.getLatestState(), backLeft.getLatestState(), backRight.getLatestState()); - if (Constants.SWERVE_DEBUG) { - SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition()); - SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); + if (Constants.TUNING_MODE) { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateConfig( + closedLoopTunable.get(), + secondaryLimitTunable.get(), + (int) smartLimitTunable.get()); + }, + closedLoopTunable, + secondaryLimitTunable, + smartLimitTunable); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + frontRight.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + backLeft.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + backRight.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + }, + drivePTunable, + driveITunable, + driveDTunable, + driveKvTunable, + driveKsTunable); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + frontRight.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + backLeft.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + backRight.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + }, + steerPTunable, + steerITunable, + steerDTunable, + steerMaxAccelerationTunable, + steerMaxVelocityTunable); } } @@ -204,4 +315,24 @@ public void setFacingTarget(boolean facingTarget) { public boolean isFacingTarget() { return facingTarget; } + + /** + * @param closedLoopRampRate + * @param secondaryCurrentLimit + * @param smartCurrentLimit + */ + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + backLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + } + + public void applyVolts(double volts) { + frontLeft.applyVolts(volts); + frontRight.applyVolts(volts); + backLeft.applyVolts(volts); + backRight.applyVolts(volts); + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index 172fe9fb..0b29c47e 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -155,4 +155,26 @@ private double getSteerPosition() { return AngleUtils.normalizeSwerveAngle( steerSystem.getInputs().getEncoderPosition() - steerOffset); } + + public void setDrivePID(double p, double i, double d, double kV, double kS) { + drivePIDController.setPID(p, i, d); + driveFeedforward.setKv(kV); + driveFeedforward.setKs(kS); + } + + public void setSteerPID( + double p, double i, double d, double maxAcceleration, double maxVelocity) { + turningPIDController.setPID(p, i, d); + turningPIDController.setConstraints( + new TrapezoidProfile.Constraints(maxAcceleration, maxVelocity)); + } + + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + driveSystem.getIO().updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + } + + public void applyVolts(double volts) { + driveSystem.getIO().setDriveVoltage(volts); + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java index b0032e0d..ebb7c304 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java @@ -12,4 +12,8 @@ public void resetEncoder() {} @Override public void updateInputs(MotorInputs inputs) {} + + @Override + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {} } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index ca204a8d..234b2a61 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -2,7 +2,6 @@ import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.constants.Constants; @@ -13,14 +12,12 @@ public class SparkMaxDriveMotorIO implements SwerveDriveMotorIO { private final SparkMax driveMotor; - private final SparkBaseConfig driveConfig; private final SparkMaxInputProvider inputProvider; public SparkMaxDriveMotorIO( int driveMotorIO, KinematicsConversionConfig conversionConfig, boolean driveInverted) { driveMotor = new SparkMax(driveMotorIO, SparkMax.MotorType.kBrushless); inputProvider = new SparkMaxInputProvider(driveMotor); - driveConfig = new SparkMaxConfig(); setMotorConfig(driveInverted); setConversionFactors(conversionConfig); } @@ -31,6 +28,7 @@ public void setDriveVoltage(double volts) { } private void setConversionFactors(KinematicsConversionConfig conversionConfig) { + SparkMaxConfig driveConfig = new SparkMaxConfig(); double driveVelConvFactor = (2 * conversionConfig.getWheelRadius() * Math.PI) / (conversionConfig.getProfile().getDriveGearRatio() * 60); @@ -43,12 +41,12 @@ private void setConversionFactors(KinematicsConversionConfig conversionConfig) { .velocityConversionFactor(driveVelConvFactor); driveMotor.configure( driveConfig, - SparkBase.ResetMode.kResetSafeParameters, + SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kPersistParameters); } private void setMotorConfig(boolean driveInverted) { - // driveMotor.restoreFactoryDefaults(); //TODO: idk what to do + SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig .inverted(driveInverted) .idleMode(IdleMode.kBrake) @@ -58,7 +56,7 @@ private void setMotorConfig(boolean driveInverted) { Constants.DRIVE_SMART_LIMIT); // TODO: change current limiting because its different driveMotor.configure( driveConfig, - SparkBase.ResetMode.kResetSafeParameters, + SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kPersistParameters); } @@ -71,4 +69,18 @@ public void resetEncoder() { public void updateInputs(MotorInputs inputs) { inputs.process(inputProvider); } + + @Override + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig + .closedLoopRampRate(closedLoopRampRate) + .secondaryCurrentLimit(secondaryCurrentLimit) + .smartCurrentLimit(smartCurrentLimit); + driveMotor.configure( + driveConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java index 4e5ddb75..dae40c8c 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java @@ -7,4 +7,6 @@ public interface SwerveDriveMotorIO extends LoggableIO { void setDriveVoltage(double volts); void resetEncoder(); + + void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); }