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

Example 26 with Translation

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

the class TestCompareChassisSpeedsWithTranslation method compare.

private static void compare(double vx, double vy, double vz, Angle angle) {
    ValidationUtils.validate(vx, "vx");
    ValidationUtils.validate(vy, "vy");
    ValidationUtils.validate(vz, "vz");
    ValidationUtils.validate(angle, "angle");
    ChassisSpeeds speeds = new ChassisSpeeds(vx, vy, vz);
    Translation translation = new Translation(vx, vy, vz);
    compare(speeds, translation, angle);
}

Example 27 with Translation

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

the class GenericOdometry method updateWithTime.

public PointXYZ updateWithTime(double currentTimeMs, Angle gyroAngle, T state) {
    double period = previousTimeMs > 0 ? (currentTimeMs - previousTimeMs) : 0.0;
    previousTimeMs = currentTimeMs;
    Angle angle = gyroAngle.add(gyroOffset);
    Translation translation = kinematics.toTranslation(state);
    double dx = translation.vx() * period;
    double dy = translation.vy() * period;
    Angle dta = angle.subtract(previousAngle);
    double dt = dta.rad();
    double sin = dta.sin();
    double cos = dta.cos();
    double s;
    double c;
    if (Math.abs(dt) < 1E-9) {
        s = 1.0 - 1.0 / 6.0 * dt * dt;
        c = 0.5 * dt;
    } else {
        s = sin / dt;
        c = (1 - cos) / dt;
    }
    PointXYZ newPosition = position.add(new PointXYZ(dx * s - dy * c, dx * c + dy * s, angle));
    previousAngle = angle;
    position = newPosition;
    return position;
}

Example 28 with Translation

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

the class MecanumKinematics method calculate.

public MecanumState calculate(Translation translation, PointXY centerOfRotation) {
    if (!centerOfRotation.equals(lastCenterOfRotation)) {
        PointXY fl = frontLeftPosition.subtract(centerOfRotation);
        PointXY fr = frontRightPosition.subtract(centerOfRotation);
        PointXY bl = backLeftPosition.subtract(centerOfRotation);
        PointXY br = backRightPosition.subtract(centerOfRotation);
        setInverseKinematics(fl, fr, bl, br);
        lastCenterOfRotation = centerOfRotation;
    }
    SimpleMatrix speedVector = new SimpleMatrix(3, 1);
    speedVector.setColumn(0, 0, translation.vx(), translation.vy(), translation.vz());
    SimpleMatrix wheelMatrix = inverseKinematics.mult(speedVector);
    double fl = wheelMatrix.get(0, 0);
    double fr = wheelMatrix.get(1, 0);
    double bl = wheelMatrix.get(2, 0);
    double br = wheelMatrix.get(3, 0);
    return new MecanumState(fr, fl, br, bl);
}

Example 29 with Translation

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

the class RelativeMecanumKinematics method calculate.

@Override
public MecanumState calculate(Translation translation) {
    double xyMagnitude = MinMax.clip(Math.hypot(translation.vx(), translation.vy()), minMagnitude, maxMagnitude);
    Angle angle = Angle.atan2(translation.vy(), translation.vx()).add(angleOffset);
    double turn = translation.vz() * turnMagnitude;
    double fl = (calculatePower(angle, WHEEL_ANGLES[0]) * xyMagnitude) + turn;
    double fr = (calculatePower(angle, WHEEL_ANGLES[1]) * xyMagnitude) - turn;
    double bl = (calculatePower(angle, WHEEL_ANGLES[2]) * xyMagnitude) + turn;
    double br = (calculatePower(angle, WHEEL_ANGLES[3]) * xyMagnitude) - turn;
    return new MecanumState(fl, fr, bl, br).normalizeFromMaxUnderOne();
}