Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Make swerve examples multiply desired module speeds by cosine of heading error (#5758)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
kully
2023-10-13 09:20:57 +03:00
committed by GitHub
parent 1c724884ca
commit 9a0aafd8ab
6 changed files with 54 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -109,9 +109,15 @@ public class SwerveModule {
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState desiredState) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.getDistance()));
SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
// direction of travel that can occur when modules change directions. This results in smoother
// driving.
state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
// Calculate the drive output from the drive PID controller.
final double driveOutput =

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
View File

@@ -105,9 +105,15 @@ public class SwerveModule {
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState desiredState) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.getDistance()));
SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
// direction of travel that can occur when modules change directions. This results in smoother
// driving.
state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
// Calculate the drive output from the drive PID controller.
final double driveOutput =

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -109,9 +109,15 @@ public class SwerveModule {
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState desiredState) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.getDistance()));
SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
// direction of travel that can occur when modules change directions. This results in smoother
// driving.
state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
// Calculate the drive output from the drive PID controller.
final double driveOutput =