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

Update

Browse Source
This commit is contained in:
thenetworkgrinch
2024-06-12 15:10:29 -05:00
parent 8c3470f453
commit 2d992a453a
7 changed files with 60 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
swervelib/SwerveDrive.java
View File

@@ -101,6 +101,10 @@ public class SwerveDrive
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
/**
* Amount of seconds the duration of the timestep the speeds should be applied for.
*/
private double discretizationdtSeconds = 0.02;
/**
* Deadband for speeds in heading correction.
*/
@@ -185,12 +189,12 @@ public class SwerveDrive
setMaximumSpeed(maxSpeedMPS);
// Initialize Telemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
{
SmartDashboard.putData("Field", field);
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.maxSpeed = maxSpeedMPS;
SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity;
@@ -342,6 +346,19 @@ public class SwerveDrive
HEADING_CORRECTION_DEADBAND = deadband;
}
/**
* Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same time.
* The inputs are added together so this is not intneded to be used to give the driver both methods of control.
*
* @param fieldOrientedVelocity The field oriented velocties to use
* @param robotOrientedVelocity The robot oriented velocties to use
*/
public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity)
{
ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()).plus(robotOrientedVelocity);
drive(TotalVelocties);
}
/**
* Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
*
@@ -464,7 +481,7 @@ public class SwerveDrive
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (chassisVelocityCorrection)
{
velocity = ChassisSpeeds.discretize(velocity, 0.02);
velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
}
// Heading Angular Velocity Deadband, might make a configuration option later.
@@ -485,11 +502,11 @@ public class SwerveDrive
}
// Display commanded speed for testing
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO)
{
SmartDashboard.putString("RobotVelocity", velocity.toString());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond;
@@ -502,7 +519,6 @@ public class SwerveDrive
setRawModuleStates(swerveModuleStates, isOpenLoop);
}
/**
* Set the maximum speeds for desaturation.
*
@@ -660,7 +676,7 @@ public class SwerveDrive
*/
public void postTrajectory(Trajectory trajectory)
{
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
{
field.getObject("Trajectory").setTrajectory(trajectory);
}
@@ -871,7 +887,7 @@ public class SwerveDrive
{
SwerveModuleState desiredState =
new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle());
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] =
desiredState.angle.getDegrees();
@@ -932,7 +948,7 @@ public class SwerveDrive
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
// Update angle accumulator if the robot is simulated
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
if (SwerveDriveTelemetry.isSimulation)
@@ -951,7 +967,7 @@ public class SwerveDrive
SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
{
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
}
@@ -967,7 +983,7 @@ public class SwerveDrive
SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
@@ -983,7 +999,7 @@ public class SwerveDrive
moduleSynchronizationCounter = 0;
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
SwerveDriveTelemetry.updateData();
}
@@ -1122,11 +1138,11 @@ public class SwerveDrive
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
* internal offsets to prevent double offsetting.
*/
public void pushOffsetsToControllers()
public void pushOffsetsToEncoders()
{
for (SwerveModule module : swerveModules)
{
module.pushOffsetsToControllers();
module.pushOffsetsToEncoders();
}
}
@@ -1156,4 +1172,15 @@ public class SwerveDrive
}
}
/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
*
* @param enable
* @param dtSeconds
*/
public void setChassisDiscretization(boolean enable, double dtSeconds){
chassisVelocityCorrection = enable;
discretizationdtSeconds = dtSeconds;
}
}