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

Example 26 with Angle

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

the class Follower method getAbsoluteTranslation.

/**
 * Get an absolute translation based on a couple of parameters.
 *
 * <p>
 * This method functions as follows.
 * <ul>
 *     <li>
 *         Determine the angle from the robot's current position to
 *         the robot's target position.
 *     </li>
 *     <li>
 *         Create an imaginary point using the lovely
 *         {@link PointXYZ#inDirection(double, Angle)} method. This point
 *         will always be drawn at a distance of 1. The point's angle
 *         is equal to the angle we just calculated.
 *     </li>
 *     <li>
 *         Convert that point into a translation by getting the X and Y
 *         values of the point. The generated translation simply
 *         re-uses the {@code turn} parameter that's passed into
 *         this method.
 *     </li>
 * </ul>
 * </p>
 *
 * @param current the robot's current position.
 * @param target  the robot's target point / marker.
 * @param speed   the speed at which the robot should move.
 * @param turn    how much the robot should turn.
 * @return an absolute translation. This translation will always have X
 * and Y values that fit within the bounds (-1.0, 1.0). This translation
 * is also absolute, NOT relative.
 */
static Translation getAbsoluteTranslation(PointXYZ current, PointXYZ target, double speed, double turn) {
    ValidationUtils.validate(current, "current");
    ValidationUtils.validate(target, "target");
    ValidationUtils.validate(speed, "speed");
    ValidationUtils.validate(turn, "turn");
    if (PointXY.equals(current, target))
        return Translation.ZERO.withVz(turn);
    Angle angle = current.angleTo(target).fix();
    PointXYZ targetPoint = PointXYZ.ZERO.inDirection(speed, angle);
    return new Translation(targetPoint.x(), targetPoint.y(), turn);
}

Example 27 with Angle

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

the class MecanumDriveOdometry method getRawPosition.

@Override
public PointXYZ getRawPosition() {
    double currentTimeMs = Time.ms();
    Angle gyroAngle = getGyroAngle.get();
    MecanumState state = getState();
    return odometry.updateWithTime(currentTimeMs, gyroAngle, state);
}

Example 28 with Angle

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

the class GenericOdometry method updateWithTime.

/**
 * Update the position based on the system's current time (as dictated
 * by {@code Time#ms()} or {@code System#currentTimeMillis()}), the
 * angle of the gyroscope on the robot, and the current state of the robot.
 *
 * <p>
 * This method integrates the velocity of the robot (as determined by
 * the parameter of type {@code T}) over time to determine the position
 * of the robot.
 * </p>
 *
 * @param currentTimeMs the system's current time, in milliseconds.
 *                      Use either {@code Time#ms()} or
 *                      {@code System#currentTimeMillis()} to get this
 *                      value.
 * @param gyroAngle     the angle of the robot's gyroscope.
 * @param state         the current state of the robot. This value
 *                      should usually be determined by using encoders to
 *                      determine the robot's velocity.
 * @return the robot's updated position.
 */
public PointXYZ updateWithTime(double currentTimeMs, Angle gyroAngle, T state) {
    double period = previousTimeMs > -1 ? (currentTimeMs - previousTimeMs) : 0.0;
    // instead of updating the position
    if (period < updateIntervalMs) {
        previousTimeMs = currentTimeMs;
        return position;
    }
    previousTimeMs = currentTimeMs;
    period /= 1_000;
    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, 0)).withZ(angle);
    previousAngle = angle;
    position = newPosition;
    return position;
}

Example 29 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle 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();
}

Example 30 with Angle

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

the class AngleDeltaController method calculate.

@Override
public double calculate(double value) {
    boolean isDegrees = angleUnit == Angle.AngleUnit.DEGREES;
    Angle current = Angle.fixedDeg(value);
    Angle target = Angle.fixedRad(this.getTarget());
    double delta = isDegrees ? Angle.angleDeltaDeg(current, target) : Angle.angleDeltaRad(current, target);
    return delta * this.coefficient;
}