Examples with SwerveModuleState edu.wpi.first.math.kinematics.SwerveModuleState used on opensource projects

Example 11 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project Pathfinder2 by Wobblyyyy.

the class WPISwerveOdometry method update.

/**
 * Update the odometry once. This will use the robot's translation over
 * time to determine its position based on elapsed time.
 */
public void update() {
    Rotation2d rotation = WPIAdapter.rotationFromAngle(gyroscope.getAngle());
    SwerveModuleState[] states = new SwerveModuleState[driveEncoders.length];
    WPISwerveModule[] modules = chassis.getModules();
    for (int i = 0; i < modules.length; i++) states[i] = new SwerveModuleState(driveEncoders[i].getVelocity() * velocityToUnitsPerSec, modules[i].getTurnRotation());
    pose = odometry.update(rotation, states);
    position = WPIAdapter.pointXYZFromPose(pose);
}