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

Example 21 with Path

use of com.team254.lib_2014.trajectory.Path 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 22 with Path

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

the class PointTurn method calculate.

public static Path calculate(TrajectoryGenerator.Config config, TrajectoryGenerator.Strategy strategy, double start_velocity, double start_heading, double goal_velocity, double goal_heading) {
    Trajectory ref = TrajectoryGenerator.generate(config, strategy, start_velocity, start_heading, 1e-10, goal_velocity, goal_heading);
    Trajectory leftProfile = ref;
    Trajectory rightProfile = ref.copy();
    // Calculate the radius of the arc
    double radius = Math.abs(Math.abs(1e-10) / (goal_heading * Math.PI / 180.0));
    double width = 12.625;
    // Find the difference between the left and right motors
    double faster = (radius + (width / 2.0)) / radius;
    double slower = (radius - (width / 2.0)) / radius;
    // Determine which way to curve
    if (goal_heading > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    return new Path("Turn", new Trajectory.Pair(leftProfile, rightProfile));
}

Example 23 with Path

use of com.team254.lib_2014.trajectory.Path 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 24 with Path

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

the class AppendPaths method appendSpecial.

public static Path appendSpecial(Path original, Path to_append) {
    Trajectory left = original.getLeftWheelTrajectory();
    Trajectory after = to_append.getLeftWheelTrajectory();
    for (int i = 0; i < after.getNumSegments() - 1; i++) {
        double pos = after.getSegment(i).pos + left.getSegment(left.getNumSegments() - 1).pos;
        double vel = after.getSegment(i).vel + left.getSegment(left.getNumSegments() - 1).vel;
        double acc = after.getSegment(i).acc + left.getSegment(left.getNumSegments() - 1).acc;
        double heading = after.getSegment(i).heading + left.getSegment(left.getNumSegments() - 1).heading;
        double x = after.getSegment(i).x + left.getSegment(left.getNumSegments() - 1).x;
        double y = after.getSegment(i).y + left.getSegment(left.getNumSegments() - 1).y;
        Trajectory.Segment update = new Trajectory.Segment(pos, vel, acc, heading, after.getSegment(i).jerk, after.getSegment(i).dt, x, y);
        after.setSegment(i, update);
    }
    return new Path("", new Trajectory.Pair(after, original.getRightWheelTrajectory()));
}