Examples with WaypointSequence com.team254.lib_2014.trajectory.WaypointSequence used on opensource projects

Example 6 with WaypointSequence

use of com.team254.lib_2014.trajectory.WaypointSequence in project Relic_Main by TeamOverdrive.

the class RedFar_Paths method calculateRedFar.

public static void calculateRedFar(final FieldConfig field) {
    if (bsToRight == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        /**
         * Right *
         */
        // Path from Red Far Stone to Right Column
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 20, 29 - field.getFarRedCenterToWall(), Math.toRadians(15)));
        bsToRight = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
        /**
         * Center *
         */
        // Path from Red Fat Stone to Center Column
        waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 18, 29 - field.getFarRedCenterToWall(), -15));
        bsToCenter = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
        /**
         * Left To Glyph Pit*
         */
        waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(33, -10, 0));
        leftToGP = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
        leftToGP.goLeft();
    }
}

Example 7 with WaypointSequence

use of com.team254.lib_2014.trajectory.WaypointSequence in project Relic_Main by TeamOverdrive.

the class HoldMyPaths method calculateBlueFarRightToGlyphPit.

public static void calculateBlueFarRightToGlyphPit() {
    if (farBlueRightToGlyphPit == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(22, -11, Math.toRadians(-10.3)));
        farBlueRightToGlyphPit = PathGenerator.makePath(waypointSequence, Constants.aggressiveTrajectoryConfig, WHEEL_BASE, "");
    }
}

Example 8 with WaypointSequence

use of com.team254.lib_2014.trajectory.WaypointSequence in project Relic_Main by TeamOverdrive.

the class HoldMyPaths method calculateBlueFarCenterToGlyphPit.

public static void calculateBlueFarCenterToGlyphPit() {
    if (farBlueCenterToGlyphPit == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(34, -10, Math.toRadians(-26)));
        farBlueCenterToGlyphPit = PathGenerator.makePath(waypointSequence, Constants.aggressiveTrajectoryConfig, WHEEL_BASE, "");
    }
}

Example 9 with WaypointSequence

use of com.team254.lib_2014.trajectory.WaypointSequence in project Relic_Main by TeamOverdrive.

the class BlueFar_Paths method calculateBlueFar.

public static void calculateBlueFar(final FieldConfig field) {
    if (bsToRight == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        // Path from Red Far Stone to Right Column
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 21, 23 - field.getFarRedCenterToWall(), Math.toRadians(14)));
        bsToRight = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
    }
}

Example 10 with WaypointSequence

use of com.team254.lib_2014.trajectory.WaypointSequence in project Relic_Main by TeamOverdrive.

the class TestCombinePaths method main.

public static void main(String... args) {
    TrajectoryGenerator.Config RedConfig = new TrajectoryGenerator.Config();
    RedConfig.max_vel = 23.23;
    RedConfig.max_acc = 200;
    RedConfig.max_jerk = 10000;
    RedConfig.dt = 0.01;
    double WHEEL_BASE = 12.625;
    WaypointSequence waypointSequence = new WaypointSequence(10);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(6, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(48.0 - 33, 36.0 - 28.5, Math.toRadians(-10)));
    Path pathToRightColumnRedFar = PathGenerator.makePath(waypointSequence, RedConfig, WHEEL_BASE, "");
    pathToRightColumnRedFar.goRight();
    waypointSequence = new WaypointSequence(10);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(10, 5, Math.toRadians(20)));
    Path fromRedRightToBox = PathGenerator.makePath(waypointSequence, RedConfig, WHEEL_BASE, "");
    fromRedRightToBox.getLeftWheelTrajectory().scale(-1);
    fromRedRightToBox.getRightWheelTrajectory().scale(-1);
    Trajectory.Pair base = new Trajectory.Pair(pathToRightColumnRedFar.getLeftWheelTrajectory(), pathToRightColumnRedFar.getRightWheelTrajectory());
    base.left.append(fromRedRightToBox.getLeftWheelTrajectory());
    base.right.append(fromRedRightToBox.getRightWheelTrajectory());
    Path output = new Path("Hello", base);
    System.out.println(output.getLeftWheelTrajectory().toStringEuclidean());
}