Examples with SwerveModuleState me.wobblyyyy.pathfinder2.kinematics.SwerveModuleState used on opensource projects

Example 1 with SwerveModuleState

use of me.wobblyyyy.pathfinder2.kinematics.SwerveModuleState in project Pathfinder2 by Wobblyyyy.

the class SwerveDriveKinematics method calculate.

/**
 * Calculate a {@link SwerveState}, given a translation and a center
 * of rotation.
 *
 * @param translation      the translation to calculate a swerve state
 *                         based on.
 * @param centerOfRotation the center of rotation. To be entirely honest,
 *                         I have absolutely no idea what this means.
 * @return create a new swerve drive.
 */
public SwerveState calculate(Translation translation, PointXY centerOfRotation) {
    if (!centerOfRotation.equals(previousCenterOfRotation)) {
        for (int i = 0; i < moduleCount; i++) {
            PointXY modulePosition = modulePositions[i];
            double x = modulePosition.x() - centerOfRotation.x();
            double y = -modulePosition.y() + centerOfRotation.y();
            inverseKinematics.setRow(i * 2 + 0, 0, 1, 0, y);
            inverseKinematics.setRow(i * 2 + 1, 0, 0, 1, x);
        }
        previousCenterOfRotation = centerOfRotation;
    }
    SimpleMatrix speedVector = new SimpleMatrix(3, 1);
    speedVector.setColumn(0, 0, translation.vx(), translation.vy(), translation.vz());
    SimpleMatrix moduleStateMatrix = inverseKinematics.mult(speedVector);
    SwerveModuleState[] states = new SwerveModuleState[moduleCount];
    for (int i = 0; i < moduleCount; i++) {
        double x = moduleStateMatrix.get(i * 2 + 0, 0);
        double y = moduleStateMatrix.get(i * 2 + 1, 0);
        double speed = Math.hypot(x, y);
        Angle angle = Angle.atan2(y, x);
        states[i] = new SwerveModuleState(speed, angle);
    }
    return new SwerveState(states[0], states[1], states[2], states[3]);
}

Example 2 with SwerveModuleState

use of me.wobblyyyy.pathfinder2.kinematics.SwerveModuleState in project Pathfinder2 by Wobblyyyy.

the class SwerveDriveKinematics method toTranslation.

public Translation toTranslation(SwerveModuleState... states) {
    if (states.length != moduleCount) {
        throw new IllegalArgumentException("Number of modules is not consistent with number of " + "wheel locations provided in constructor");
    }
    SimpleMatrix matrix = new SimpleMatrix(moduleCount * 2, 1);
    for (int i = 0; i < moduleCount; i++) {
        SwerveModuleState state = states[i];
        matrix.set(i * 2 + 0, 0, state.speed(), state.direction().cos());
        matrix.set(i * 2 + 1, 0, state.speed(), state.direction().sin());
    }
    SimpleMatrix speedVector = forwardKinematics.mult(matrix);
    return new Translation(speedVector.get(0, 0), speedVector.get(1, 0), speedVector.get(2, 0));
}

Example 3 with SwerveModuleState

use of me.wobblyyyy.pathfinder2.kinematics.SwerveModuleState in project Pathfinder2 by Wobblyyyy.

the class SwerveChassisOdometry method getRawPosition.

@Override
public PointXYZ getRawPosition() {
    double currentTimeMs = Time.ms();
    Angle gyroAngle = getGyroAngle.get();
    SwerveModuleState frontRightState = frontRightOdometry.getState();
    SwerveModuleState frontLeftState = frontLeftOdometry.getState();
    SwerveModuleState backRightState = backRightOdometry.getState();
    SwerveModuleState backLeftState = backLeftOdometry.getState();
    SwerveState state = new SwerveState(frontRightState, frontLeftState, backRightState, backLeftState);
    return odometry.updateWithTime(currentTimeMs, gyroAngle, state);
}