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

[wpimath] Add ChassisSpeeds method to fix drifting during compound swerve drive maneuvers (#5425)

Browse Source
This commit is contained in:
Joseph Eng
2023-07-18 21:19:55 -07:00
committed by GitHub
parent 1af224c21b
commit 657338715d
28 changed files with 227 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
/**
@@ -43,6 +44,54 @@ public class ChassisSpeeds {
this.omegaRadiansPerSecond = omegaRadiansPerSecond;
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed for continuous time.
*
* <p>The difference between applying a chassis speed for a discrete timestep vs. continuously is
* that applying for a discrete timestep is just scaling the velocity components by the time and
* adding, while when applying continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds.
*/
public static ChassisSpeeds fromDiscreteSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
double dtSeconds) {
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));
var twist = new Pose2d().log(desiredDeltaPose);
return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed for continuous time.
*
* <p>The difference between applying a chassis speed for a discrete timestep vs. continuously is
* that applying for a discrete timestep is just scaling the velocity components by the time and
* adding, while when applying continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @param discreteSpeeds The speeds for a discrete timestep.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return ChassisSpeeds that can be applied continuously to produce the discrete chassis speeds.
*/
public static ChassisSpeeds fromDiscreteSpeeds(ChassisSpeeds discreteSpeeds, double dtSeconds) {
return fromDiscreteSpeeds(
discreteSpeeds.vxMetersPerSecond,
discreteSpeeds.vyMetersPerSecond,
discreteSpeeds.omegaRadiansPerSecond,
dtSeconds);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.

50
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "units/angular_velocity.h"
#include "units/velocity.h"
@@ -38,6 +39,55 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*/
units::radians_per_second_t omega = 0_rad_per_s;
/**
* Converts from a chassis speed for a discrete timestep into chassis speed
* for continuous time.
*
* The difference between applying a chassis speed for a discrete timestep vs.
* continuously is that applying for a discrete timestep is just scaling the
* velocity components by the time and adding, while when applying
* continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
*
* @return ChassisSpeeds that can be applied continuously to produce the
* discrete ChassisSpeeds.
*/
static ChassisSpeeds FromDiscreteSpeeds(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
auto twist = Pose2d{}.Log(desiredDeltaPose);
return {twist.dx / dt, twist.dy / dt, twist.dtheta / dt};
}
/**
* Converts from a chassis speed for a discrete timestep into chassis speed
* for continuous time.
*
* The difference between applying a chassis speed for a discrete timestep vs.
* continuously is that applying for a discrete timestep is just scaling the
* velocity components by the time and adding, while when applying
* continuously the changes to the heading affect the direction the
* translational components are applied to relative to the field.
*
* @param discreteSpeeds The speeds for a discrete timestep.
* @param dt The duration of the timestep the speeds should be applied for.
*
* @return ChassisSpeeds that can be applied continuously to produce the
* discrete ChassisSpeeds.
*/
static ChassisSpeeds FromDiscreteSpeeds(const ChassisSpeeds& discreteSpeeds,
units::second_t dt) {
return FromDiscreteSpeeds(discreteSpeeds.vx, discreteSpeeds.vy,
discreteSpeeds.omega, dt);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* ChassisSpeeds object.