-
Notifications
You must be signed in to change notification settings - Fork 139
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
# Conflicts: # src/main/java/swervelib/SwerveModule.java
- Loading branch information
Showing
12 changed files
with
206 additions
and
15 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,4 +1,9 @@ | ||
| { | ||
| "Keyboard 0 Settings": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| }, | ||
| "keyboardJoysticks": [ | ||
| { | ||
| "axisConfig": [ | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,118 @@ | ||
| package swervelib; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.kinematics.ChassisSpeeds; | ||
| import edu.wpi.first.math.kinematics.SwerveModuleState; | ||
| import edu.wpi.first.wpilibj.Timer; | ||
|
|
||
| /** | ||
| * Class to perform tests on the swerve drive. | ||
| */ | ||
| public class SwerveDriveTest | ||
| { | ||
|
|
||
| /** | ||
| * Set the modules to center to 0. | ||
| * | ||
| * @param swerveDrive Swerve Drive to control. | ||
| */ | ||
| public static void centerModules(SwerveDrive swerveDrive) | ||
| { | ||
| for (SwerveModule swerveModule : swerveDrive.getModules()) | ||
| { | ||
| swerveModule.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(0)), false, true); | ||
| } | ||
|
|
||
| // Update kinematics because we are not using setModuleStates | ||
| swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds()); | ||
| } | ||
|
|
||
| /** | ||
| * Set the angle of the modules to a given {@link Rotation2d} | ||
| * | ||
| * @param swerveDrive {@link SwerveDrive} to use. | ||
| * @param moduleAngle {@link Rotation2d} to set every module to. | ||
| */ | ||
| public static void angleModules(SwerveDrive swerveDrive, Rotation2d moduleAngle) | ||
| { | ||
| for (SwerveModule swerveModule : swerveDrive.getModules()) | ||
| { | ||
| swerveModule.setDesiredState(new SwerveModuleState(0, moduleAngle), false, true); | ||
| } | ||
| swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds()); | ||
| } | ||
|
|
||
| /** | ||
| * Power the drive motors for the swerve drive to a set percentage. | ||
| * | ||
| * @param swerveDrive {@link SwerveDrive} to control. | ||
| * @param percentage Percentage of voltage to send to drive motors. | ||
| */ | ||
| public static void powerDriveMotors(SwerveDrive swerveDrive, double percentage) | ||
| { | ||
| for (SwerveModule swerveModule : swerveDrive.getModules()) | ||
| { | ||
| swerveModule.getDriveMotor().set(percentage); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Power the angle motors for the swerve drive to a set percentage. | ||
| * | ||
| * @param swerveDrive {@link SwerveDrive} to control. | ||
| * @param percentage Percentage of voltage to send to angle motors. | ||
| */ | ||
| public static void powerAngleMotors(SwerveDrive swerveDrive, double percentage) | ||
| { | ||
| for (SwerveModule swerveModule : swerveDrive.getModules()) | ||
| { | ||
| swerveModule.getAngleMotor().set(percentage); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Find the minimum amount of power required to move the swerve drive motors. | ||
| * | ||
| * @param swerveDrive {@link SwerveDrive} to control. | ||
| * @param minMovement Minimum amount of movement to drive motors. | ||
| * @param testDelaySeconds Time in seconds for the motor to move. | ||
| * @return minimum voltage required. | ||
| */ | ||
| public static double findDriveMotorKV(SwerveDrive swerveDrive, double minMovement, double testDelaySeconds) | ||
| { | ||
| double[] startingEncoders = new double[4]; | ||
| double kV = 0; | ||
|
|
||
| SwerveDriveTest.powerDriveMotors(swerveDrive, 0); | ||
| SwerveModule[] modules = swerveDrive.getModules(); | ||
| for (int i = 0; i < modules.length; i++) | ||
| { | ||
| startingEncoders[i] = Math.abs(modules[i].getDriveMotor().getPosition()); | ||
| } | ||
|
|
||
| for (double kV_new = 0; kV_new < 0.1; kV_new += 0.0001) | ||
| { | ||
|
|
||
| SwerveDriveTest.powerDriveMotors(swerveDrive, kV); | ||
| boolean foundkV = false; | ||
| double startTimeSeconds = Timer.getFPGATimestamp(); | ||
| while ((Timer.getFPGATimestamp() - startTimeSeconds) < testDelaySeconds && !foundkV) | ||
| { | ||
| for (int i = 0; i < modules.length; i++) | ||
| { | ||
| if ((modules[i].getDriveMotor().getPosition() - startingEncoders[i]) > minMovement) | ||
| { | ||
| foundkV = true; | ||
| break; | ||
| } | ||
| } | ||
| } | ||
| if (foundkV) | ||
| { | ||
| SwerveDriveTest.powerDriveMotors(swerveDrive, 0); | ||
| kV = kV_new; | ||
| } | ||
| } | ||
| return kV; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters