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

Example 21 with Angle

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

the class AngleSplineTrajectory method nextMarker.

@Override
public PointXYZ nextMarker(PointXYZ current) {
    double x = current.x() + step;
    PointXY interpolatedPoint = spline.interpolate(x);
    Angle interpolatedAngle = angleSpline.getAngleTarget(x);
    return interpolatedPoint.withHeading(interpolatedAngle);
}

Example 22 with Angle

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

the class MultiTargetTrajectory method nextMarker.

@Override
public PointXYZ nextMarker(PointXYZ current) {
    // set up some values
    if (!hasRan) {
        lastStartX = current.x();
        lastStartY = current.y();
        lastStartZ = current.z();
        PointXYZ next = targets[index].target();
        requiredTranslationX = next.x() - current.x();
        requiredTranslationY = next.y() - current.y();
        requiredTranslationZ = next.z().subtract(current.z()).fix().deg();
        hasRan = true;
    }
    TrajectoryTarget target = targets[index];
    // current values
    double currentX = current.x();
    double currentY = current.y();
    Angle currentAngle = current.z();
    // sort of "delta" values - difference between current and start
    double translationX = currentX - lastStartX;
    double translationY = currentY - lastStartY;
    double translationZ = currentAngle.subtract(lastStartZ).fix().deg();
    boolean hasCompletedX = Magnitude.higherMagnitude(translationX, requiredTranslationX);
    boolean hasCompletedY = Magnitude.higherMagnitude(translationY, requiredTranslationY);
    boolean hasCompletedZ = Magnitude.higherMagnitude(translationZ, requiredTranslationZ);
    if (hasCompletedX && hasCompletedY && hasCompletedZ) {
        // to handle tolerance values
        if (target.precision() == TargetPrecision.PRECISE) {
            PointXYZ targetPoint = target.target();
            double distance = current.absDistance(targetPoint);
            Angle angleDistance = Angle.angleDelta(current.z(), targetPoint.z());
            boolean validDistance = distance < target.tolerance();
            boolean validAngle = angleDistance.lessThanOrEqualTo(target.angleTolerance());
            if (!(validDistance && validAngle)) {
                return targetPoint;
            }
        }
        // care about tolerance
        if (index == targets.length - 1) {
            // if it's the last target point, the entire trajectory is done
            hasFinished = true;
            return current;
        } else {
            lastStartX = currentX;
            lastStartY = currentY;
            lastStartZ = currentAngle;
            index++;
            PointXYZ next = targets[index].target();
            requiredTranslationX = next.x() - currentX;
            requiredTranslationY = next.y() - currentY;
            requiredTranslationZ = next.z().subtract(currentAngle).fix().deg();
        }
    }
    return targets[index].target();
}

Example 23 with Angle

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

the class Pathfinder method splineTo.

/**
 * Create a spline trajectory to a certain target point, and then follow
 * that aforementioned trajectory.
 *
 * <p>
 * If this method is called on a set of points with non-monotonic Y
 * values, this will instead invoke
 * {@link #multiSplineTo(double, double, Angle, PointXYZ...)}, which
 * supports non-monotonic Y values.
 * </p>
 *
 * @param speed          the speed at which the robot should move. This
 *                       is a constant value.
 * @param tolerance      the tolerance used for determining whether the
 *                       robot is at the target point.
 * @param angleTolerance same thing as {@code tolerance}, but for the
 *                       robot's angle.
 * @param points         a set of control points for the spline. This
 *                       will automatically insert the robot's current
 *                       position into this array. This array must have
 *                       AT LEAST two points.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder splineTo(double speed, double tolerance, Angle angleTolerance, PointXYZ... points) {
    if (points.length < 2)
        throw new IllegalArgumentException("At least two control points are required to use the " + "splineTo method.");
    InvalidSpeedException.throwIfInvalid("Invalid speed value provided! Speed must be between 0 and 1.", speed);
    InvalidToleranceException.throwIfInvalid("Invalid tolerance! Tolerance must be a positive number.", tolerance);
    if (angleTolerance.deg() < 0)
        throw new InvalidToleranceException("Invalid angle tolerance! " + "Angle tolerance must be greater than 0 degrees.");
    NotNull.throwExceptionIfNull("One or more points provided to splineTo was null!", (Object[]) points);
    // non-monotonic Y values means we need to use a multi spline instead
    if (!Spline.areMonotonicY(points))
        return multiSplineTo(speed, tolerance, angleTolerance, points);
    // length is the total distance of the spline (NOT all control points)
    double length = PointXY.distance(points[0], points[points.length - 1]);
    // step should be relatively small - by default, it's 1/20th of
    // the spline's length.
    double step = length / Core.pathfinderStepDivisor;
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setSpeed(speed).setTolerance(tolerance).setAngleTolerance(angleTolerance).setStep(step);
    PointXYZ robotPosition = getPosition();
    // to the trajectory
    if (!robotPosition.equals(points[0]))
        builder.add(robotPosition);
    PointXYZ lastPoint = points[0];
    for (int i = 1; i < points.length; i++) {
        PointXYZ point = points[i];
        if (point == null)
            throw new NullPointException("Cannot use the splineTo method with a null " + "control point!");
        if (!point.equals(lastPoint)) {
            builder.add(point);
            lastPoint = point;
        } else {
            throw new SplineException("There were duplicate adjacent points in the set " + "of control points! This means there's the same " + "point, twice in a row. The points were: " + Arrays.toString(points));
        }
    }
    Trajectory trajectory = builder.build();
    return followTrajectory(trajectory);
}

Example 24 with Angle

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

the class Pathfinder method multiSplineTo.

/**
 * Use a {@link MultiSplineBuilder} to construct a spline trajectory.
 *
 * @param speed          the speed at which the robot should move. This
 *                       is a constant value.
 * @param tolerance      the tolerance used for determining whether the
 *                       robot is at the target point.
 * @param angleTolerance same thing as {@code tolerance}, but for the
 *                       robot's angle.
 * @param points         a set of control points for the spline. This
 *                       will automatically insert the robot's current
 *                       position into this array. This array must have
 *                       AT LEAST two points.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder multiSplineTo(double speed, double tolerance, Angle angleTolerance, PointXYZ... points) {
    if (points.length < 2)
        throw new IllegalArgumentException("At least two control points are required to use the " + "splineTo method.");
    checkForMissingDefaultValues();
    InvalidSpeedException.throwIfInvalid("Invalid speed value provided! Speed must be between 0 and 1.", speed);
    InvalidToleranceException.throwIfInvalid("Invalid tolerance! Tolerance must be a positive number.", tolerance);
    if (angleTolerance.deg() < 0)
        throw new InvalidToleranceException("Invalid angle tolerance! " + "Angle tolerance must be greater than 0 degrees.");
    if (!Spline.areMonotonicX(points))
        throw new SplineException("Cannot create a spline with non-" + "monotonic X values! X values can only be either " + "increasing or decreasing, but not a combination of both.");
    double totalDistanceX = points[points.length - 1].distanceX(points[0]);
    double step = totalDistanceX / (points.length * Core.pathfinderSplineStepCoefficient);
    MultiSplineBuilder builder = new MultiSplineBuilder().setDefaultSpeed(speed).setDefaultTolerance(tolerance).setDefaultAngleTolerance(angleTolerance).setDefaultStep(step);
    for (PointXYZ point : points) builder.add(point, speed, step);
    Trajectory trajectory = builder.build();
    followTrajectory(trajectory);
    return this;
}

Example 25 with Angle

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

the class TankDriveOdometry method getRawPosition.

@Override
public PointXYZ getRawPosition() {
    Angle angle = getGyroAngle.get();
    double right = getRightDistance.get();
    double left = getLeftDistance.get();
    return odometry.update(angle, right, left);
}