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

Example 46 with Trajectory

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

the class TestingTrajectory method main.

public static void main(String[] args) {
    // Distance of an arc that we want to travel.
    double distance = 10;
    // Angle that we want to be at, at the end of the circle.
    // Has to be non-zero because we, for now, assume that we are traveling in a arc.
    // Based on unit circle
    double angle = -0.00001;
    // Config for the based on what we want the robot to do, not
    // based on the characteristics of the robot.
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    // In/s
    config.max_vel = 2.5 * 12;
    // In/s^2
    config.max_acc = 7.57 * 12;
    // In/s^3
    config.max_jerk = 10 * 12;
    // seconds, change of time in each update
    config.dt = 0.01;
    TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.TrapezoidalStrategy;
    Trajectory reference = TrajectoryGenerator.generate(config, strategy, // start velocity
    0.0, 0, Math.abs(distance), // goal velocity
    0.0, 0);
    Trajectory leftProfile = reference;
    // Copy
    Trajectory rightProfile = reference.copy();
    double radius = Math.abs(Math.abs(distance) / (angle * Math.PI / 180.0));
    double width = 12.625;
    double faster = (radius + (width / 2.0)) / radius;
    double slower = (radius - (width / 2.0)) / radius;
    if (angle > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    double scaleFactor = 1.0 / 30.0;
    leftProfile.scale(scaleFactor);
    rightProfile.scale(scaleFactor);
    double max = 0;
    for (int i = 0; i < leftProfile.getNumSegments(); i++) {
        if (leftProfile.getSegment(i).vel > max)
            max = leftProfile.getSegment(i).vel;
    }
    System.out.println(leftProfile.getNumSegments());
    System.out.println(max);
// System.out.println(leftProfile.toStringProfile());
// System.out.println(rightProfile.toStringProfile());
}

Example 47 with Trajectory

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

the class howlong method main.

public static void main(String[] args) {
    double starttime = System.currentTimeMillis();
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    // In/s
    config.max_vel = 2.0 * 12;
    // In/s^2
    config.max_acc = 0.6 * 12;
    // In/s^3
    config.max_jerk = 0.4 * 12;
    // seconds
    config.dt = 0.001;
    double distance = 20;
    double angle = -90;
    double velocityMax = config.max_vel;
    double Vmax = 12;
    double kv = Vmax / velocityMax;
    System.out.println(kv);
    TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.TrapezoidalStrategy;
    Trajectory reference = TrajectoryGenerator.generate(config, strategy, // start velocity
    0.0, 0, Math.abs(distance), // goal velocity
    0.0, angle);
    System.out.println(reference.toStringEuclidean());
    Trajectory leftProfile = reference;
    // Copy
    Trajectory rightProfile = reference.copy();
    double radius = Math.abs(Math.abs(distance) / (angle * Math.PI / 180.0));
    double width = 12;
    double faster = (radius + (width / 2.0)) / radius;
    double slower = (radius - (width / 2.0)) / radius;
    // System.out.println("faster " + faster);
    if (angle > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    System.out.println((System.currentTimeMillis() - starttime) / 1000);
}

Example 48 with Trajectory

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

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

use of com.team254.lib_2014.trajectory.Trajectory 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()));
}