Examples with Translation me.wobblyyyy.pathfinder2.geometry.Translation used on opensource projects

Example 16 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation in project Pathfinder2 by Wobblyyyy.

the class RelativeSwerveDriveKinematics method calculateOptimized.

/**
 * Calculate an optimized swerve state based on a translation.
 *
 * <p>
 * My left hand feels like it's about to fall off from typing this much,
 * so I'm just gonna ask you to go read the documentation in the
 * swerve module state class about optimization to figure it out.
 * Good luck!
 * </p>
 *
 * @param translation the translation the robot should follow.
 * @return an optimized swerve state according to that translation.
 */
@SuppressWarnings("DuplicatedCode")
public RelativeSwerveState calculateOptimized(Translation translation) {
    Angle angle = translation.angle();
    double vz = translation.vz() * turnMultiplier;
    double frontRightTurn = frontRightKinematics.calculate(frontRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double frontLeftTurn = frontLeftKinematics.calculate(frontLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double backRightTurn = backRightKinematics.calculate(backRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double backLeftTurn = backLeftKinematics.calculate(backLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double frontRightDrive = translation.magnitude();
    double frontLeftDrive = translation.magnitude();
    double backRightDrive = translation.magnitude();
    double backLeftDrive = translation.magnitude();
    RelativeSwerveModuleState frontRightState = RelativeSwerveModuleState.optimized(angle, frontRightDrive, frontRightModuleAngle.get(), frontRightKinematics);
    RelativeSwerveModuleState frontLeftState = RelativeSwerveModuleState.optimized(angle, frontLeftDrive, frontLeftModuleAngle.get(), frontLeftKinematics);
    RelativeSwerveModuleState backRightState = RelativeSwerveModuleState.optimized(angle, backRightDrive, backRightModuleAngle.get(), backRightKinematics);
    RelativeSwerveModuleState backLeftState = RelativeSwerveModuleState.optimized(angle, backLeftDrive, backLeftModuleAngle.get(), backLeftKinematics);
    return new RelativeSwerveState(frontRightState, frontLeftState, backRightState, backLeftState);
}

Example 17 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation in project Pathfinder2 by Wobblyyyy.

the class RelativeSwerveDriveKinematics method calculate.

/**
 * Create a swerve drive chassis state based on a translation.
 *
 * @param translation the translation to create a state for.
 * @return a created relative swerve drive state based
 * on the provided translation.
 */
@SuppressWarnings("DuplicatedCode")
@Override
public RelativeSwerveState calculate(Translation translation) {
    Angle angle = translation.angle();
    double vz = translation.vz() * turnMultiplier;
    double frontRightTurn = frontRightKinematics.calculate(frontRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double frontLeftTurn = frontLeftKinematics.calculate(frontLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double backRightTurn = backRightKinematics.calculate(backRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double backLeftTurn = backLeftKinematics.calculate(backLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double frontRightDrive = translation.magnitude();
    double frontLeftDrive = translation.magnitude();
    double backRightDrive = translation.magnitude();
    double backLeftDrive = translation.magnitude();
    RelativeSwerveModuleState frontRightState = new RelativeSwerveModuleState(frontRightTurn, frontRightDrive);
    RelativeSwerveModuleState frontLeftState = new RelativeSwerveModuleState(frontLeftTurn, frontLeftDrive);
    RelativeSwerveModuleState backRightState = new RelativeSwerveModuleState(backRightTurn, backRightDrive);
    RelativeSwerveModuleState backLeftState = new RelativeSwerveModuleState(backLeftTurn, backLeftDrive);
    return new RelativeSwerveState(frontRightState, frontLeftState, backRightState, backLeftState);
}

Example 18 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 19 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 20 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method testHalfRotation.

@Test
public void testHalfRotation() {
    pathfinder.followTrajectory(new LinearTrajectory(pathfinder.getPosition().withHeading(Angle.fromDeg(180)), 0.5, 0.1, Angle.fromDeg(1)));
    pathfinder.tickUntil(500);
    odometry.setTranslation(new Translation(-1, -1, 0));
    odometry.updatePositionBasedOnVelocity(1000);
    odometry.setTranslation(new Translation(1, 1, 0));
    odometry.updatePositionBasedOnVelocity(1000);
}