Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Updated YAGSL

Browse Source
This commit is contained in:
thenetworkgrinch
2023-04-05 09:03:10 -05:00
parent b1689f3ec2
commit d37a38bb7b
112 changed files with 396 additions and 325 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
swervelib/SwerveDrive.java
View File

@@ -5,12 +5,7 @@ import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
@@ -20,9 +15,6 @@ import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveKinematics2;
import swervelib.math.SwerveMath;
@@ -33,6 +25,10 @@ import swervelib.simulation.SwerveIMUSimulation;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
/**
* Swerve Drive class representing and controlling the swerve drive.
*/
@@ -253,34 +249,54 @@ public class SwerveDrive
setRawModuleStates(swerveModuleStates, isOpenLoop);
}
/**
* Set the maximum speeds for desaturation.
*
* @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach in meters per second.
* @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while
* translating in meters per second.
* @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in radians per second.
*/
public void setMaximumSpeeds(
double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond) {
setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond);
swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
}
/**
* Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.
*
* @param desiredStates A list of SwerveModuleStates to send to the modules.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
{
private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop) {
// Desaturates wheel speeds
SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
if (swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond != 0 ||
swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond != 0)
SwerveKinematics2.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
swerveDriveConfiguration.maxSpeed,
swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
else
SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
// Sets states
for (SwerveModule module : swerveModules)
{
for (SwerveModule module : swerveModules) {
module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
SwerveDriveTelemetry.desiredStates[module.moduleNumber *
2] = module.lastState.angle.getDegrees();
2] = module.lastState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) +
1] = module.lastState.speedMetersPerSecond;
1] = module.lastState.speedMetersPerSecond;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) {
SmartDashboard.putNumber(
"Module[" + module.moduleNumber + "] Speed Setpoint: ",
module.lastState.speedMetersPerSecond);
"Module[" + module.moduleNumber + "] Speed Setpoint: ",
module.lastState.speedMetersPerSecond);
SmartDashboard.putNumber(
"Module[" + module.moduleNumber + "] Angle Setpoint: ",
module.lastState.angle.getDegrees());