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

Updated YAGSL

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-22 20:50:16 -06:00
parent 8bfa4a8824
commit dd7d432e90
111 changed files with 497 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
swervelib/SwerveDrive.java
View File

@@ -122,11 +122,21 @@ public class SwerveDrive
SwerveDriveTelemetry.maxSpeed = swerveDriveConfiguration.maxSpeed;
SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity;
SwerveDriveTelemetry.moduleCount = swerveModules.length;
SwerveDriveTelemetry.sizeLeftRight = SwerveMath.getSwerveModule(swerveModules, true, false).moduleLocation.getX() -
SwerveMath.getSwerveModule(swerveModules, false, false).moduleLocation.getX();
SwerveDriveTelemetry.sizeFrontBack = SwerveMath.getSwerveModule(swerveModules, false, true).moduleLocation.getY() -
SwerveMath.getSwerveModule(swerveModules, false, false).moduleLocation.getY();
SwerveDriveTelemetry.sizeFrontBack = (SwerveMath.getSwerveModule(swerveModules, true, false).moduleLocation.getX() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getX()) / 2;
SwerveDriveTelemetry.sizeLeftRight = (SwerveMath.getSwerveModule(swerveModules, false, true).moduleLocation.getY() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getY()) / 2;
SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2];
for (SwerveModule module : swerveModules)
{
SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = module.configuration.moduleLocation.getX() / 2;
SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] =
module.configuration.moduleLocation.getY() / 2;
}
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
// TODO: Might need to flip X and Y.
@@ -184,6 +194,10 @@ public class SwerveDrive
// Sets states
for (SwerveModule module : swerveModules)
{
SwerveModuleState2 moduleState = module.getState();
SwerveDriveTelemetry.desiredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop);
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Speed Setpoint: ",
@@ -306,7 +320,7 @@ public class SwerveDrive
{
simIMU.setAngle(0);
}
swerveController.lastAngle = 0;
swerveController.lastAngleScalar = 0;
resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d()));
}
@@ -469,7 +483,7 @@ public class SwerveDrive
}
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
ChassisSpeeds measuredChassisSpeeds = kinematics.toChassisSpeeds(getStates());
ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
@@ -481,8 +495,6 @@ public class SwerveDrive
SwerveModuleState2 moduleState = module.getState();
SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = modulePoses[module.moduleNumber].getX();
SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = modulePoses[module.moduleNumber].getY();
sumOmega += Math.abs(moduleState.omegaRadPerSecond);