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);
}