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

[wpimath] Add optimize() to SwerveModuleState (#3065)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2021-01-10 22:49:46 -08:00
committed by GitHub
parent fb99910c23
commit e955037980
9 changed files with 79 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -79,9 +79,13 @@ public class SwerveModule {
/**
* Sets the desired state for the module.
*
* @param state Desired state with speed and angle.
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState state) {
public void setDesiredState(SwerveModuleState desiredState) {
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);

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

@@ -87,11 +87,15 @@ public class SwerveModule {
/**
* Sets the desired state for the module.
*
* @param state Desired state with speed and angle.
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState state) {
public void setDesiredState(SwerveModuleState desiredState) {
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
// Calculate the drive output from the drive PID controller.
final var driveOutput =
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
// Calculate the turning motor output from the turning PID controller.

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervesdriveposeestimator/SwerveModule.java
View File

@@ -79,9 +79,13 @@ public class SwerveModule {
/**
* Sets the desired state for the module.
*
* @param state Desired state with speed and angle.
* @param desiredState Desired state with speed and angle.
*/
public void setDesiredState(SwerveModuleState state) {
public void setDesiredState(SwerveModuleState desiredState) {
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state =
SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);