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

Added heading angular velocity deadband from 1466

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-23 22:46:43 -06:00
parent b484ed9bc0
commit 9b699291e8
107 changed files with 197 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
swervelib/SwerveDrive.java
View File

@@ -52,23 +52,27 @@ public class SwerveDrive
/**
* Field object.
*/
public Field2d field = new Field2d();
public Field2d field = new Field2d();
/**
* Swerve controller for controlling heading of the robot.
*/
public SwerveController swerveController;
public SwerveController swerveController;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
private SwerveIMU imu;
private SwerveIMU imu;
/**
* Simulation of the swerve drive.
*/
private SwerveIMUSimulation simIMU;
private SwerveIMUSimulation simIMU;
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -172,6 +176,17 @@ public class SwerveDrive
translation.getX(), translation.getY(), rotation, getYaw())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
if (Math.abs(rotation) < 0.01)
{
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
} else
{
lastHeadingRadians = getYaw().getRadians();
}
// Display commanded speed for testing
SmartDashboard.putString("RobotVelocity", velocity.toString());
SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;