Package swervelib
Class SwerveDriveTest
java.lang.Object
swervelib.SwerveDriveTest
Class to perform tests on the swerve drive.
-
Constructor Summary
-
Method Summary
Modifier and TypeMethodDescriptionstatic void
angleModules
(SwerveDrive swerveDrive, Rotation2d moduleAngle) Set the angle of the modules to a givenRotation2d
static void
centerModules
(SwerveDrive swerveDrive) Set the modules to center to 0.static SysIdRoutine.Config
createConfigCustomTimeout
(double timeout) Creates a SysIdRoutine.Config with a custom final timeoutstatic double
findCouplingRatio
(SwerveDrive swerveDrive, double volts, boolean automatic) Find the coupling ratio for all modules.static double
findDriveMotorKV
(SwerveDrive swerveDrive, double minMovement, double testDelaySeconds, double maxVolts) Find the minimum amount of power required to move the swerve drive motors.static Command
generateSysIdCommand
(SysIdRoutine sysIdRoutine, double delay, double quasiTimeout, double dynamicTimeout) Creates a command that can be mapped to a button or other trigger.static void
logAngularMotorActivity
(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied) Logs info about the angle motor to the SysIdRoutineLogstatic void
logAngularMotorDutyCycle
(SwerveModule module, SysIdRoutineLog log) Logs info about the angle motor to the SysIdRoutineLog.static void
logAngularMotorVoltage
(SwerveModule module, SysIdRoutineLog log) Logs info about the angle motor to the SysIdRoutineLogstatic void
logDriveMotorActivity
(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied) Logs power, position and velocuty info form the drive motor to the SysIdRoutineLogstatic void
logDriveMotorDutyCycle
(SwerveModule module, SysIdRoutineLog log) Logs output, position and velocuty info form the drive motor to the SysIdRoutineLog
Although SysIdRoutine expects to be logging Voltage, this function logs in Duty-Cycle (percent output) because it results in correctly adjusted values in the analysis for use in this library.static void
logDriveMotorVoltage
(SwerveModule module, SysIdRoutineLog log) Logs voltage, position and velocuty info form the drive motor to the SysIdRoutineLogstatic void
powerAngleMotorsDutyCycle
(SwerveDrive swerveDrive, double percentage) Power the angle motors for the swerve drive to a set percentage.static void
powerAngleMotorsVoltage
(SwerveDrive swerveDrive, double volts) Power the angle motors for the swerve drive to a set voltage.static void
powerDriveMotorsDutyCycle
(SwerveDrive swerveDrive, double percentage) Power the drive motors for the swerve drive to a set duty cycle percentage.static void
powerDriveMotorsVoltage
(SwerveDrive swerveDrive, double volts) Power the drive motors for the swerve drive to a set voltage.static SysIdRoutine
setAngleSysIdRoutine
(SysIdRoutine.Config config, SubsystemBase swerveSubsystem, SwerveDrive swerveDrive) Sets up the SysId runner and logger for the angle motorsstatic SysIdRoutine
setDriveSysIdRoutine
(SysIdRoutine.Config config, SubsystemBase swerveSubsystem, SwerveDrive swerveDrive, double maxVolts) Sets up the SysId runner and logger for the drive motors
-
Constructor Details
-
SwerveDriveTest
public SwerveDriveTest()
-
-
Method Details
-
angleModules
Set the angle of the modules to a givenRotation2d
- Parameters:
swerveDrive
-SwerveDrive
to use.moduleAngle
-Rotation2d
to set every module to.
-
powerDriveMotorsDutyCycle
Power the drive motors for the swerve drive to a set duty cycle percentage.- Parameters:
swerveDrive
-SwerveDrive
to control.percentage
- Duty cycle percentage of voltage to send to drive motors.
-
powerAngleMotorsDutyCycle
Power the angle motors for the swerve drive to a set percentage.- Parameters:
swerveDrive
-SwerveDrive
to control.percentage
- DutyCycle percentage to send to angle motors.
-
powerDriveMotorsVoltage
Power the drive motors for the swerve drive to a set voltage.- Parameters:
swerveDrive
-SwerveDrive
to control.volts
- DutyCycle percentage of voltage to send to drive motors.
-
powerAngleMotorsVoltage
Power the angle motors for the swerve drive to a set voltage.- Parameters:
swerveDrive
-SwerveDrive
to control.volts
- Voltage to send to angle motors.
-
centerModules
Set the modules to center to 0.- Parameters:
swerveDrive
- Swerve Drive to control.
-
findDriveMotorKV
public static double findDriveMotorKV(SwerveDrive swerveDrive, double minMovement, double testDelaySeconds, double maxVolts) Find the minimum amount of power required to move the swerve drive motors.- Parameters:
swerveDrive
-SwerveDrive
to control.minMovement
- Minimum amount of movement to drive motors.testDelaySeconds
- Time in seconds for the motor to move.maxVolts
- The maximum voltage to send to drive motors.- Returns:
- minimum voltage required.
-
findCouplingRatio
Find the coupling ratio for all modules.- Parameters:
swerveDrive
-SwerveDrive
to operate with.volts
- Voltage to send to angle motors to spin.automatic
- Attempt to automatically spin the modules.- Returns:
- Average coupling ratio.
-
createConfigCustomTimeout
Creates a SysIdRoutine.Config with a custom final timeout- Parameters:
timeout
- - the most a SysIdRoutine should run- Returns:
- A custom SysIdRoutine.Config
-
logDriveMotorDutyCycle
Logs output, position and velocuty info form the drive motor to the SysIdRoutineLog
Although SysIdRoutine expects to be logging Voltage, this function logs in Duty-Cycle (percent output) because it results in correctly adjusted values in the analysis for use in this library.- Parameters:
module
- - the swerve module being loggedlog
- - the logger
-
logDriveMotorVoltage
Logs voltage, position and velocuty info form the drive motor to the SysIdRoutineLog- Parameters:
module
- - the swerve module being loggedlog
- - the logger
-
logDriveMotorActivity
public static void logDriveMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied) Logs power, position and velocuty info form the drive motor to the SysIdRoutineLog- Parameters:
module
- - the swerve module being loggedlog
- - the loggerpowerSupplied
- - a functional supplier of the power to be logged
-
setDriveSysIdRoutine
public static SysIdRoutine setDriveSysIdRoutine(SysIdRoutine.Config config, SubsystemBase swerveSubsystem, SwerveDrive swerveDrive, double maxVolts) Sets up the SysId runner and logger for the drive motors- Parameters:
config
- - The SysIdRoutine.Config to useswerveSubsystem
- - the subsystem to add to requirementsswerveDrive
- - the SwerveDrive from which to access motor infomaxVolts
- - The maximum voltage that should be applied to the drive motors.- Returns:
- A SysIdRoutine runner
-
logAngularMotorDutyCycle
Logs info about the angle motor to the SysIdRoutineLog.
Although SysIdRoutine expects to be logging Voltage, this function logs in Duty-Cycle (percent output) because it results in correctly adjusted values in the analysis for use in this library.- Parameters:
module
- - the swerve module being loggedlog
- - the logger
-
logAngularMotorVoltage
Logs info about the angle motor to the SysIdRoutineLog- Parameters:
module
- - the swerve module being loggedlog
- - the logger
-
logAngularMotorActivity
public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied) Logs info about the angle motor to the SysIdRoutineLog- Parameters:
module
- - the swerve module being loggedlog
- - the loggerpowerSupplied
- - a functional supplier of the power to be logged
-
setAngleSysIdRoutine
public static SysIdRoutine setAngleSysIdRoutine(SysIdRoutine.Config config, SubsystemBase swerveSubsystem, SwerveDrive swerveDrive) Sets up the SysId runner and logger for the angle motors- Parameters:
config
- - The SysIdRoutine.Config to useswerveSubsystem
- - the subsystem to add to requirementsswerveDrive
- - the SwerveDrive from which to access motor info- Returns:
- A SysIdRoutineRunner
-
generateSysIdCommand
public static Command generateSysIdCommand(SysIdRoutine sysIdRoutine, double delay, double quasiTimeout, double dynamicTimeout) Creates a command that can be mapped to a button or other trigger. Delays can be set to customize the length of each part of the SysId Routine- Parameters:
sysIdRoutine
- - The Sys Id routine runnerdelay
- - seconds between each portion to allow motors to spin down, etc...quasiTimeout
- - seconds to run the Quasistatic routines, so robot doesn't get too fardynamicTimeout
- - seconds to run the Dynamic routines, 2-3 secs should be enough- Returns:
- A command that can be mapped to a button or other trigger
-