From 55af6a4cc8c4e19a722f1a4d060dc9190c624ca5 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 11 Jan 2025 13:02:36 -0500 Subject: [PATCH 1/3] ElevatorSysID subsystem added plus other minor constant changes --- .../frc/robot/constants/ArmConstants.java | 2 + .../constants/ClimberPivotConstants.java | 2 + .../robot/constants/ElevatorConstants.java | 42 +++++++++++++ .../java/frc/robot/subsystems/Elevator.java | 16 +++-- .../robot/subsystems/sysid/ElevatorSysID.java | 62 +++++++++++++++++++ 5 files changed, 120 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 8ab9aa3..1dfe976 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -3,4 +3,6 @@ package frc.robot.constants; public class ArmConstants { public static final int kArmMotorID = 0; public static final int kCANcoderID = 0; + + public static final double kEncoderConversionFactor = 0; } diff --git a/src/main/java/frc/robot/constants/ClimberPivotConstants.java b/src/main/java/frc/robot/constants/ClimberPivotConstants.java index 557d574..0138ad2 100644 --- a/src/main/java/frc/robot/constants/ClimberPivotConstants.java +++ b/src/main/java/frc/robot/constants/ClimberPivotConstants.java @@ -5,6 +5,8 @@ public class ClimberPivotConstants { public static final int kClimberLimitSwitchID = 0; + public static final double kEncoderConversionFactor = 0; + public static final double kPIDControllerP = 0; public static final double kPIDControllerI = 0; public static final double kPIDControllerD = 0; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 687edf3..bc439f0 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -1,8 +1,23 @@ package frc.robot.constants; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; + +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; + public class ElevatorConstants { public static final int kElevatorMotorID = 0; + public static final double kEncoderConversionFactor = 0; + + public static final int kMotorAmpsMax = 0; + public static final double kPIDControllerP = 0; public static final double kPIDControllerI = 0; public static final double kPIDControllerD = 0; @@ -10,4 +25,31 @@ public class ElevatorConstants { public static final double kFeedForwardS = 0; public static final double kFeedForwardG = 0; public static final double kFeedForwardV = 0; + + // 1, 7, 10 are the defaults for these, change as necessary + public static final double kSysIDRampRate = 1; + public static final double kSysIDStepVolts = 7; + public static final double kSysIDTimeout = 10; + + // This should be kCoast for SysID, any other time it can be kBrake if you want + public static final IdleMode kIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM + + public static final SysIdRoutine.Config kSysIDConfig = new Config( + Volts.of(kSysIDRampRate).per(Second), + Volts.of(kSysIDStepVolts), + Seconds.of(kSysIDTimeout) + ); + + public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); + + static { + motorConfig + .smartCurrentLimit(kMotorAmpsMax) + .idleMode(kIdleMode); + motorConfig.encoder + .positionConversionFactor(kEncoderConversionFactor) + .velocityConversionFactor(kEncoderConversionFactor / 60.0); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index baf11b4..6e78bec 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,7 +1,9 @@ package frc.robot.subsystems; -import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.controller.ElevatorFeedforward; @@ -11,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; public class Elevator extends SubsystemBase { - private SparkMax elevatorMotor1; + protected SparkMax elevatorMotor1; - private AbsoluteEncoder encoder; + protected RelativeEncoder encoder; private PIDController pidController; @@ -25,7 +27,13 @@ public class Elevator extends SubsystemBase { MotorType.kBrushless ); - encoder = elevatorMotor1.getAbsoluteEncoder(); + elevatorMotor1.configure( + ElevatorConstants.motorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + encoder = elevatorMotor1.getEncoder(); pidController = new PIDController( ElevatorConstants.kPIDControllerP, diff --git a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java new file mode 100644 index 0000000..6d3b70d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.sysid; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.Elevator; + +public class ElevatorSysID extends Elevator { + private MutVoltage appliedVoltage; + + private MutDistance elevatorPosition; + + private MutLinearVelocity elevatorVelocity; + + private SysIdRoutine routine; + + public ElevatorSysID() { + super(); + + appliedVoltage = Volts.mutable(0); + + elevatorPosition = Meters.mutable(0); + + elevatorVelocity = MetersPerSecond.mutable(0); + + routine = new SysIdRoutine( + ElevatorConstants.kSysIDConfig, + new SysIdRoutine.Mechanism( + elevatorMotor1::setVoltage, + (log) -> { + log.motor("elevatorMotor1") + .voltage(appliedVoltage.mut_replace( + elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts + )) + .linearPosition(elevatorPosition.mut_replace( + encoder.getPosition(), Meters + )) + .linearVelocity(elevatorVelocity.mut_replace( + encoder.getVelocity(), MetersPerSecond + )); + }, + this + ) + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} From 0c22aa354223601b4e4c201501d4bf72bcffd714 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 11 Jan 2025 20:33:35 -0500 Subject: [PATCH 2/3] A ton of changes to make DrivetrainSysID subsystem a thing --- src/main/java/frc/robot/RobotContainer.java | 6 +- ...onstants.java => DrivetrainConstants.java} | 45 +++++- .../robot/constants/ElevatorConstants.java | 4 +- .../frc/robot/constants/ModuleConstants.java | 7 +- .../{DriveSubsystem.java => Drivetrain.java} | 54 +++---- .../frc/robot/subsystems/MAXSwerveModule.java | 18 +++ .../subsystems/sysid/DrivetrainSysID.java | 151 ++++++++++++++++++ 7 files changed, 244 insertions(+), 41 deletions(-) rename src/main/java/frc/robot/constants/{DriveConstants.java => DrivetrainConstants.java} (60%) rename src/main/java/frc/robot/subsystems/{DriveSubsystem.java => Drivetrain.java} (79%) create mode 100644 src/main/java/frc/robot/subsystems/sysid/DrivetrainSysID.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9bbe31..dbbb378 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,7 @@ import frc.robot.constants.OIConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberRollers; -import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Manipulator; @@ -23,7 +23,7 @@ public class RobotContainer { private ClimberRollers climberRollers; - private DriveSubsystem drivetrain; + private Drivetrain drivetrain; private Elevator elevator; @@ -41,7 +41,7 @@ public class RobotContainer { climberRollers = new ClimberRollers(); - drivetrain = new DriveSubsystem(); + drivetrain = new Drivetrain(); elevator = new Elevator(); diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java similarity index 60% rename from src/main/java/frc/robot/constants/DriveConstants.java rename to src/main/java/frc/robot/constants/DrivetrainConstants.java index dc4d824..b3ecead 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -1,10 +1,16 @@ package frc.robot.constants; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -public class DriveConstants { +public class DrivetrainConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds public static final double kMaxSpeedMetersPerSecond = 4.8; @@ -14,12 +20,6 @@ public class DriveConstants { public static final double kTrackWidth = Units.inchesToMeters(26.5); // Distance between centers of right and left wheels on robot public static final double kWheelBase = Units.inchesToMeters(26.5); - // Distance between front and back wheels on robot - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); // Angular offsets of the modules relative to the chassis in radians public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2; @@ -27,6 +27,16 @@ public class DriveConstants { public static final double kBackLeftChassisAngularOffset = Math.PI; public static final double kBackRightChassisAngularOffset = Math.PI / 2; + // 1, 7, 10 is the default for these three values + public static final double kSysIDDrivingRampRate = 1; + public static final double kSysIDDrivingStepVolts = 7; + public static final double kSysIDDrivingTimeout = 10; + + // 1, 7, 10 is the default for these three values + public static final double kSysIDTurningRampRate = 1; + public static final double kSysIDTurningStepVolts = 7; + public static final double kSysIDTurningTimeout = 10; + // SPARK MAX CAN IDs public static final int kFrontLeftDrivingCanId = 11; public static final int kRearLeftDrivingCanId = 13; @@ -39,4 +49,25 @@ public class DriveConstants { public static final int kRearRightTurningCanId = 16; public static final boolean kGyroReversed = false; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM + + // Distance between front and back wheels on robot + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + + public static final SysIdRoutine.Config kSysIDDrivingConfig = new Config( + Volts.of(kSysIDDrivingRampRate).per(Second), + Volts.of(kSysIDDrivingStepVolts), + Seconds.of(kSysIDDrivingTimeout) + ); + + public static final SysIdRoutine.Config kSysIDTurningConfig = new Config( + Volts.of(kSysIDTurningRampRate).per(Second), + Volts.of(kSysIDTurningStepVolts), + Seconds.of(kSysIDTurningTimeout) + ); } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index bc439f0..7d75029 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -7,7 +7,6 @@ import static edu.wpi.first.units.Units.Seconds; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; @@ -31,8 +30,7 @@ public class ElevatorConstants { public static final double kSysIDStepVolts = 7; public static final double kSysIDTimeout = 10; - // This should be kCoast for SysID, any other time it can be kBrake if you want - public static final IdleMode kIdleMode = IdleMode.kCoast; + public static final IdleMode kIdleMode = IdleMode.kBrake; // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index a586023..1b3de28 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -20,6 +20,11 @@ public class ModuleConstants { public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; + public static final int kDriveMotorCurrentLimit = 40; + public static final int kTurnMotorCurrentLimit = 20; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM + public static final SparkMaxConfig drivingConfig = new SparkMaxConfig(); public static final SparkMaxConfig turningConfig = new SparkMaxConfig(); @@ -31,7 +36,7 @@ public class ModuleConstants { drivingConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit(50); + .smartCurrentLimit(kDriveMotorCurrentLimit); drivingConfig.encoder .positionConversionFactor(drivingFactor) // meters .velocityConversionFactor(drivingFactor / 60.0); // meters per second diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/Drivetrain.java similarity index 79% rename from src/main/java/frc/robot/subsystems/DriveSubsystem.java rename to src/main/java/frc/robot/subsystems/Drivetrain.java index 17b6c7b..693d5e4 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -19,15 +19,15 @@ import edu.wpi.first.wpilibj.ADIS16470_IMU; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.DriveConstants; +import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; -public class DriveSubsystem extends SubsystemBase { +public class Drivetrain extends SubsystemBase { // Create MAXSwerveModules - private MAXSwerveModule m_frontLeft; - private MAXSwerveModule m_frontRight; - private MAXSwerveModule m_rearLeft; - private MAXSwerveModule m_rearRight; + protected MAXSwerveModule m_frontLeft; + protected MAXSwerveModule m_frontRight; + protected MAXSwerveModule m_rearLeft; + protected MAXSwerveModule m_rearRight; // The gyro sensor private ADIS16470_IMU m_gyro; @@ -36,35 +36,35 @@ public class DriveSubsystem extends SubsystemBase { private SwerveDriveOdometry m_odometry; /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { + public Drivetrain() { m_frontLeft = new MAXSwerveModule( - DriveConstants.kFrontLeftDrivingCanId, - DriveConstants.kFrontLeftTurningCanId, - DriveConstants.kFrontLeftChassisAngularOffset + DrivetrainConstants.kFrontLeftDrivingCanId, + DrivetrainConstants.kFrontLeftTurningCanId, + DrivetrainConstants.kFrontLeftChassisAngularOffset ); m_frontRight = new MAXSwerveModule( - DriveConstants.kFrontRightDrivingCanId, - DriveConstants.kFrontRightTurningCanId, - DriveConstants.kFrontRightChassisAngularOffset + DrivetrainConstants.kFrontRightDrivingCanId, + DrivetrainConstants.kFrontRightTurningCanId, + DrivetrainConstants.kFrontRightChassisAngularOffset ); m_rearLeft = new MAXSwerveModule( - DriveConstants.kRearLeftDrivingCanId, - DriveConstants.kRearLeftTurningCanId, - DriveConstants.kBackLeftChassisAngularOffset + DrivetrainConstants.kRearLeftDrivingCanId, + DrivetrainConstants.kRearLeftTurningCanId, + DrivetrainConstants.kBackLeftChassisAngularOffset ); m_rearRight = new MAXSwerveModule( - DriveConstants.kRearRightDrivingCanId, - DriveConstants.kRearRightTurningCanId, - DriveConstants.kBackRightChassisAngularOffset + DrivetrainConstants.kRearRightDrivingCanId, + DrivetrainConstants.kRearRightTurningCanId, + DrivetrainConstants.kBackRightChassisAngularOffset ); m_gyro = new ADIS16470_IMU(); m_odometry = new SwerveDriveOdometry( - DriveConstants.kDriveKinematics, + DrivetrainConstants.kDriveKinematics, Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)), new SwerveModulePosition[] { m_frontLeft.getPosition(), @@ -137,17 +137,17 @@ public class DriveSubsystem extends SubsystemBase { */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { // Convert the commanded speeds into the correct units for the drivetrain - double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond; - double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond; - double rotDelivered = rot * DriveConstants.kMaxAngularSpeed; + double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond; + double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond; + double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed; - var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ))) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); + swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); @@ -177,7 +177,7 @@ public class DriveSubsystem extends SubsystemBase { */ public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); m_frontLeft.setDesiredState(desiredStates[0]); m_frontRight.setDesiredState(desiredStates[1]); m_rearLeft.setDesiredState(desiredStates[2]); @@ -212,6 +212,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); } } diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 1c562d0..1002802 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -7,6 +7,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.RobotController; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; @@ -14,6 +15,7 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; @@ -107,6 +109,22 @@ public class MAXSwerveModule { m_desiredState = desiredState; } + public void setVoltageDrive(double voltage){ + m_drivingSpark.setVoltage(voltage); + } + + public void setVoltageTurn(double voltage) { + m_turningSpark.setVoltage(voltage); + } + + public double getVoltageDrive() { + return m_drivingSpark.get() * RobotController.getBatteryVoltage(); + } + + public double getVoltageTurn() { + return m_turningSpark.get() * RobotController.getBatteryVoltage(); + } + /** Zeroes all the SwerveModule encoders. */ public void resetEncoders() { m_drivingEncoder.setPosition(0); diff --git a/src/main/java/frc/robot/subsystems/sysid/DrivetrainSysID.java b/src/main/java/frc/robot/subsystems/sysid/DrivetrainSysID.java new file mode 100644 index 0000000..1d769bc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sysid/DrivetrainSysID.java @@ -0,0 +1,151 @@ +package frc.robot.subsystems.sysid; + +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.constants.DrivetrainConstants; +import frc.robot.subsystems.Drivetrain; + +public class DrivetrainSysID extends Drivetrain { + public enum RoutineType { + kDriving, + kTurning + } + + private MutVoltage drivingAppliedVoltage; + private MutVoltage turningAppliedVoltage; + + private MutDistance drivingPosition; + + private MutAngle turningPosition; + + private MutLinearVelocity drivingVelocity; + + private SysIdRoutine drivingRoutine; + private SysIdRoutine turningRoutine; + + public DrivetrainSysID() { + super(); + + drivingAppliedVoltage = Volts.mutable(0); + turningAppliedVoltage = Volts.mutable(0); + + drivingPosition = Meters.mutable(0); + + turningPosition = Radians.mutable(0); + + drivingVelocity = MetersPerSecond.mutable(0); + + // TODO Does there need to be a mutable for each metric for each motor, or are reusing the three OK + drivingRoutine = new SysIdRoutine( + DrivetrainConstants.kSysIDDrivingConfig, + new SysIdRoutine.Mechanism( + this::setDriveVoltageAll, + (log) -> { + log.motor("frontLeftDrive") + .voltage(drivingAppliedVoltage.mut_replace(m_frontLeft.getVoltageDrive(), Volts)) + .linearPosition(drivingPosition.mut_replace(m_frontLeft.getPosition().distanceMeters, Meters)) + .linearVelocity(drivingVelocity.mut_replace(m_frontLeft.getState().speedMetersPerSecond, MetersPerSecond)); + log.motor("rearLeftDrive") + .voltage(drivingAppliedVoltage.mut_replace(m_rearLeft.getVoltageDrive(), Volts)) + .linearPosition(drivingPosition.mut_replace(m_rearLeft.getPosition().distanceMeters, Meters)) + .linearVelocity(drivingVelocity.mut_replace(m_rearLeft.getState().speedMetersPerSecond, MetersPerSecond)); + log.motor("frontRightDrive") + .voltage(drivingAppliedVoltage.mut_replace(m_frontRight.getVoltageDrive(), Volts)) + .linearPosition(drivingPosition.mut_replace(m_frontRight.getPosition().distanceMeters, Meters)) + .linearVelocity(drivingVelocity.mut_replace(m_frontRight.getState().speedMetersPerSecond, MetersPerSecond)); + log.motor("rearRightDrive") + .voltage(drivingAppliedVoltage.mut_replace(m_rearRight.getVoltageDrive(), Volts)) + .linearPosition(drivingPosition.mut_replace(m_rearRight.getPosition().distanceMeters, Meters)) + .linearVelocity(drivingVelocity.mut_replace(m_rearRight.getState().speedMetersPerSecond, MetersPerSecond)); + }, + this + ) + ); + + // TODO Does there need to be a mutable for each metric for each motor, or are reusing the two OK + // TODO Angular velocity is not tracked because it's not easy to get by default, is this OK + turningRoutine = new SysIdRoutine( + DrivetrainConstants.kSysIDTurningConfig, + new SysIdRoutine.Mechanism( + this::setTurnVoltageAll, + (log) -> { + log.motor("frontLeftTurn") + .voltage(turningAppliedVoltage.mut_replace(m_frontLeft.getVoltageTurn(), Volts)) + .angularPosition(turningPosition.mut_replace(m_frontLeft.getState().angle.getRadians(), Radians)); + log.motor("rearLeftTurn") + .voltage(turningAppliedVoltage.mut_replace(m_rearLeft.getVoltageTurn(), Volts)) + .angularPosition(turningPosition.mut_replace(m_rearLeft.getState().angle.getRadians(), Radians)); + log.motor("frontRightTurn") + .voltage(turningAppliedVoltage.mut_replace(m_frontRight.getVoltageTurn(), Volts)) + .angularPosition(turningPosition.mut_replace(m_frontRight.getState().angle.getRadians(), Radians)); + log.motor("rearRightTurn") + .voltage(turningAppliedVoltage.mut_replace(m_rearRight.getVoltageTurn(), Volts)) + .angularPosition(turningPosition.mut_replace(m_rearRight.getState().angle.getRadians(), Radians)); + }, + this + ) + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction, RoutineType type) { + switch(type) { + case kDriving: + return makeDriveStraight().andThen(drivingRoutine.quasistatic(direction)); + case kTurning: + default: + return turningRoutine.quasistatic(direction); + } + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction, RoutineType type) { + switch(type) { + case kDriving: + return makeDriveStraight().andThen(drivingRoutine.dynamic(direction)); + case kTurning: + default: + return turningRoutine.dynamic(direction); + } + } + + public void setDriveVoltageAll(Voltage voltage) { + m_frontLeft.setVoltageDrive(voltage.magnitude()); + m_rearLeft.setVoltageDrive(voltage.magnitude()); + m_frontRight.setVoltageDrive(voltage.magnitude()); + m_rearRight.setVoltageDrive(voltage.magnitude()); + } + + public void setTurnVoltageAll(Voltage voltage) { + m_frontLeft.setVoltageTurn(voltage.magnitude()); + m_rearLeft.setVoltageTurn(voltage.magnitude()); + m_frontRight.setVoltageTurn(voltage.magnitude()); + m_rearRight.setVoltageTurn(voltage.magnitude()); + } + + // TODO Does this nonsense actually work + private Command makeDriveStraight() { + return runOnce(() -> { + SwerveModuleState state = new SwerveModuleState(0, Rotation2d.fromDegrees(0)); + + this.setModuleStates(new SwerveModuleState[] { + state, + state, + state, + state + }); + }); + } +} From f4908e4051cd2c1ce41df73debe5e7e2cc506c4a Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 11 Jan 2025 21:00:14 -0500 Subject: [PATCH 3/3] Minor cleanup of unused import --- src/main/java/frc/robot/subsystems/MAXSwerveModule.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 1002802..0b37934 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -15,7 +15,6 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder;