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

Example 11 with WaypointSequence

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

the class Blue_Close_Test_Path method main.

public static void main(String[] artgs) {
    TrajectoryGenerator.Config defaultTrajectoryConfig = new TrajectoryGenerator.Config();
    // Found by using "FindPDVA" class
    // In/s
    defaultTrajectoryConfig.max_vel = 23.832;
    // In/s^2
    defaultTrajectoryConfig.max_acc = 100;
    // In/s^3
    defaultTrajectoryConfig.max_jerk = 100;
    // seconds, change of time in each update
    defaultTrajectoryConfig.dt = 0.01;
    WaypointSequence blueClose = new WaypointSequence(5);
    blueClose.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    blueClose.addWaypoint(new WaypointSequence.Waypoint(36 - 8, 0, 0));
    blueClose.addWaypoint(new WaypointSequence.Waypoint(36, -6, -89));
    // Path blueFirstPath = PathGenerator.makePath(blueClose, defaultTrajectoryConfig, 12.625, "");
    double WHEEL_BASE = 12.625;
    WaypointSequence RightColumnToGlyphPit = new WaypointSequence(5);
    RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(13, -2, 0));
    RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(39, 4, Math.toRadians(4)));
    Path farRedRightToGlyphPit = PathGenerator.makePath(RightColumnToGlyphPit, defaultTrajectoryConfig, WHEEL_BASE, "");
    // System.out.println(farRedRightToGlyphPit.getRightWheelTrajectory().toStringEuclidean());
    System.out.println(farRedRightToGlyphPit.getLeftWheelTrajectory().toString());
}

Example 12 with WaypointSequence

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

the class TestPathFromBalanceStoneToRedRight method main.

public static void main(String... args) {
    TrajectoryGenerator.Config defaultTrajectoryConfig = new TrajectoryGenerator.Config();
    // In/s
    defaultTrajectoryConfig.max_vel = 23.832;
    // In/s^2
    defaultTrajectoryConfig.max_acc = 100;
    // In/s^3
    defaultTrajectoryConfig.max_jerk = 100;
    // seconds, change of time in each update
    defaultTrajectoryConfig.dt = 0.01;
    TrajectoryGenerator.Config aggressiveTrajectoryConfig = new TrajectoryGenerator.Config();
    aggressiveTrajectoryConfig.max_vel = defaultTrajectoryConfig.max_vel;
    aggressiveTrajectoryConfig.max_acc = 1000;
    aggressiveTrajectoryConfig.max_jerk = 1000;
    aggressiveTrajectoryConfig.dt = defaultTrajectoryConfig.dt;
    double WHEEL_BASE = 12.625;
    WaypointSequence waypointSequence = new WaypointSequence(10);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(48.5 - 21, 23 - 36.0, Math.toRadians(14)));
    Path pathToRightColumnRedFar = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
    waypointSequence = new WaypointSequence(10);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(12, -3, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(25, 0, Math.toRadians(-10)));
    Path pathFromFarRedRightToGlyphPit = PathGenerator.makePath(waypointSequence, aggressiveTrajectoryConfig, WHEEL_BASE, "");
    Trajectory right = pathToRightColumnRedFar.getRightWheelTrajectory();
    double time = 0;
    time = 250.0 / 1000.0;
    System.out.println(time);
    time = right.getNumSegments() * defaultTrajectoryConfig.dt;
    System.out.println(time);
    time = 200.0 / 1000;
    System.out.println(time);
    time = pathFromFarRedRightToGlyphPit.getRightWheelTrajectory().getNumSegments() * aggressiveTrajectoryConfig.dt;
    System.out.println(time);
    time = 1;
    System.out.println(time);
    time = 3.5;
    System.out.println(time);
    time = 2;
    System.out.println(time);
    time = 3.5;
    System.out.println(time);
}

Example 13 with WaypointSequence

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

the class Test method main.

public static void main(String... args) {
    TrajectoryGenerator.Config defaultTrajectoryConfig = new TrajectoryGenerator.Config();
    // In/s
    defaultTrajectoryConfig.max_vel = 23.832;
    // In/s^2
    defaultTrajectoryConfig.max_acc = 100;
    // In/s^3
    defaultTrajectoryConfig.max_jerk = 100;
    // seconds, change of time in each update
    defaultTrajectoryConfig.dt = 0.01;
    WaypointSequence waypointSequence = new WaypointSequence(5);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(13, 1, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(40, -7, Math.toRadians(-23)));
    Path leftToGP = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, 12.625, "");
    System.out.println(leftToGP.getRightWheelTrajectory().toString());
    System.out.println(Math.toDegrees(leftToGP.getRightWheelTrajectory().getSegment(leftToGP.getRightWheelTrajectory().getNumSegments() - 1).heading));
}

Example 14 with WaypointSequence

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

the class HoldMyPaths method calculateBlueCloseLeftToGlyphPit.

public static void calculateBlueCloseLeftToGlyphPit() {
    if (closeBlueLeftToGlyphPit == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(9, -9, Math.toRadians(-70)));
        closeBlueLeftToGlyphPit = PathGenerator.makePath(waypointSequence, Constants.aggressiveTrajectoryConfig, WHEEL_BASE, "");
    }
}

Example 15 with WaypointSequence

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

the class HoldMyPaths method calculateRedRightToGlyphPit.

public static void calculateRedRightToGlyphPit() {
    if (farRedRightToGlyphPit == null) {
        WaypointSequence RightColumnToGlyphPit = new WaypointSequence(5);
        RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(13, -2, 0));
        RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(39, 4, Math.toRadians(4)));
        farRedRightToGlyphPit = PathGenerator.makePath(RightColumnToGlyphPit, defaultTrajectoryConfig, WHEEL_BASE, "");
    }
}