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

[wpimath] Add cosineScale method to SwerveModuleState and instance optimize (#7114)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-09-30 15:23:30 -04:00
committed by GitHub
parent fde264b041
commit fe80d72fba
13 changed files with 311 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -115,24 +115,27 @@ public class SwerveModule {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
desiredState.optimize(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();
desiredState.cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
final double driveFeedforward =
m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
m_driveFeedforward
.calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond))
.in(Volts);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward

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

@@ -108,20 +108,21 @@ public class SwerveModule {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
desiredState.optimize(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();
desiredState.cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
// Calculate the turning motor output from the turning PID controller.
m_driveMotor.set(driveOutput);

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

@@ -115,23 +115,26 @@ public class SwerveModule {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState state = SwerveModuleState.optimize(desiredState, encoderRotation);
desiredState.optimize(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();
desiredState.cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
final double driveFeedforward =
m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
m_driveFeedforward
.calculate(MetersPerSecond.of(desiredState.speedMetersPerSecond))
.in(Volts);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward