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

[wpimath] Add ChassisSpeeds::ToTwist2d() to ChassisSpeeds (#6634)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
ncorrea210
2024-05-20 15:19:17 -04:00
committed by GitHub
parent 820f68dc08
commit 0b5aec82ff
2 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.Seconds;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
import edu.wpi.first.units.Angle;
@@ -77,6 +78,19 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
this(vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond));
}
/**
* Creates a Twist2d from ChassisSpeeds.
*
* @param dtSeconds The duration of the timestep.
* @return Twist2d.
*/
public Twist2d toTwist2d(double dtSeconds) {
return new Twist2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
omegaRadiansPerSecond * dtSeconds);
}
/**
* Discretizes a continuous-time chassis speed.
*