From 0c22aa354223601b4e4c201501d4bf72bcffd714 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 11 Jan 2025 20:33:35 -0500 Subject: [PATCH] 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 + }); + }); + } +}