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

Example 16 with PointXYZ

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

the class TestLinearTrajectory method testSimulation.

@Test
public void testSimulation() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory a = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.5, 2, Angle.fromDeg(5));
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectory(a);
    odometry.setPosition(new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.353, 0.353, 0.0), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(0, 0, 15));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 45));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 40));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.4), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 3));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.0), pathfinder.getTranslation());
    Assertions.assertFalse(pathfinder.isActive());
    Trajectory b = new LinearTrajectory(new PointXYZ(10, 10, 45), 0.5, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(b);
    odometry.setPosition(new PointXYZ(10, 10, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 10));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.35), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 20));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.25), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(12, 12, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(-0.353, -0.353, -0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(12, 12, 45));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 45));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.0), pathfinder.getTranslation());
    Assertions.assertFalse(pathfinder.isActive());
}

Example 17 with PointXYZ

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

the class TestSequentialLinearTrajectory method testMovingTurns.

@Test
public void testMovingTurns() {
    testTrajectoryTo(new PointXYZ(10, 10, Angle.fixedDeg(45)));
    testTrajectoryTo(new PointXYZ(-10, -10, Angle.fixedDeg(-45)));
    testTrajectoryTo(new PointXYZ(10, 10, Angle.fixedDeg(90)));
    testTrajectoryTo(new PointXYZ(-10, -10, Angle.fixedDeg(-90)));
    testTrajectoryTo(new PointXYZ(15, 15, Angle.fixedDeg(450)));
    testTrajectoryTo(new PointXYZ(-15, -15, Angle.fixedDeg(-450)));
    testTrajectoryTo(new PointXYZ(15, 15, Angle.fixedDeg(900)));
    testTrajectoryTo(new PointXYZ(-15, -15, Angle.fixedDeg(-900)));
}

Example 18 with PointXYZ

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

the class TestSequentialLinearTrajectory method testTurnTo.

private void testTurnTo(Angle... angles) {
    for (Angle angle : angles) {
        PointXYZ point = new PointXYZ().addZ(angle);
        testTrajectoryTo(point);
    }
}

Example 19 with PointXYZ

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

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

the class TestStatTracker method testTicksPerSecond.

public void testTicksPerSecond() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01).setSpeed(0.75).setTolerance(2).setAngleTolerance(Angle.fixedDeg(10));
    pathfinder.loadPlugin(new StatTracker());
    pathfinder.goTo(new PointXYZ(10, 10, 0));
    pathfinder.splineTo(new PointXYZ(10, 10, 90), new PointXYZ(12, 11, 45), new PointXYZ(13, 15, 180));
    ElapsedTimer timer = new ElapsedTimer(true);
    SimulatedOdometry odometry = (SimulatedOdometry) pathfinder.getOdometry();
    AtomicInteger i = new AtomicInteger(0);
    try {
        while (timer.elapsedSeconds() < 5) {
            if (Math.random() > 0.5)
                Thread.sleep(2);
            pathfinder.tick();
            Translation translation = pathfinder.getTranslation();
            odometry.setRawPosition(odometry.getRawPosition().add(new PointXYZ(translation.vx() / 50, translation.vy() / 50, Angle.fixedRad(translation.vz() / 50))));
        }
    } catch (Exception ignored) {
    }
    pathfinder.tick();
    System.out.println("tps: " + pathfinder.ticksPerSecond());
    System.out.println("ticks: " + pathfinder.getData("pf_ticks"));
    System.out.println("position: " + pathfinder.getPosition());
    System.out.println("condition met: " + i.get());
    System.out.println("total PointXY: " + PointXY.COUNT);
    System.out.println("total PointXYZ: " + PointXYZ.COUNT);
    System.out.println("total Angle: " + Angle.COUNT);
}