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

Example 1 with Segment

use of com.team254.lib_2014.trajectory.Trajectory.Segment in project K-9 by TheGreenMachine.

the class PathSegment method getClosestPoint.

/**
 * Gets the point on the segment closest to the robot
 *
 * @param position the current position of the robot
 * @return the point on the segment closest to the robot
 */
public Translation2d getClosestPoint(Translation2d position) {
    if (isLine) {
        Translation2d delta = new Translation2d(start, end);
        double u = ((position.x() - start.x()) * delta.x() + (position.y() - start.y()) * delta.y()) / (delta.x() * delta.x() + delta.y() * delta.y());
        if (u >= 0 && u <= 1)
            return new Translation2d(start.x() + u * delta.x(), start.y() + u * delta.y());
        return (u < 0) ? start : end;
    } else {
        Translation2d deltaPosition = new Translation2d(center, position);
        deltaPosition = deltaPosition.scale(deltaStart.norm() / deltaPosition.norm());
        if (Translation2d.cross(deltaPosition, deltaStart) * Translation2d.cross(deltaPosition, deltaEnd) < 0) {
            return center.translateBy(deltaPosition);
        } else {
            Translation2d startDist = new Translation2d(position, start);
            Translation2d endDist = new Translation2d(position, end);
            return (endDist.norm() < startDist.norm()) ? end : start;
        }
    }
}

Example 2 with Segment

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

the class TextFileDeserializer method deserialize.

public Path deserialize(String serialized) {
    StringTokenizer tokenizer = new StringTokenizer(serialized, "\n");
    System.out.println("Parsing path string...");
    System.out.println("String has " + serialized.length() + " chars");
    System.out.println("Found " + tokenizer.countTokens() + " tokens");
    String name = tokenizer.nextToken();
    int num_elements = Integer.parseInt(tokenizer.nextToken());
    Trajectory left = new Trajectory(num_elements);
    for (int i = 0; i < num_elements; ++i) {
        Trajectory.Segment segment = new Trajectory.Segment();
        StringTokenizer line_tokenizer = new StringTokenizer(tokenizer.nextToken(), " ");
        segment.pos = Double.parseDouble(line_tokenizer.nextToken());
        segment.vel = Double.parseDouble(line_tokenizer.nextToken());
        segment.acc = Double.parseDouble(line_tokenizer.nextToken());
        segment.jerk = Double.parseDouble(line_tokenizer.nextToken());
        segment.heading = Double.parseDouble(line_tokenizer.nextToken());
        segment.dt = Double.parseDouble(line_tokenizer.nextToken());
        segment.x = Double.parseDouble(line_tokenizer.nextToken());
        segment.y = Double.parseDouble(line_tokenizer.nextToken());
        left.setSegment(i, segment);
    }
    Trajectory right = new Trajectory(num_elements);
    for (int i = 0; i < num_elements; ++i) {
        Trajectory.Segment segment = new Trajectory.Segment();
        StringTokenizer line_tokenizer = new StringTokenizer(tokenizer.nextToken(), " ");
        segment.pos = Double.parseDouble(line_tokenizer.nextToken());
        segment.vel = Double.parseDouble(line_tokenizer.nextToken());
        segment.acc = Double.parseDouble(line_tokenizer.nextToken());
        segment.jerk = Double.parseDouble(line_tokenizer.nextToken());
        segment.heading = Double.parseDouble(line_tokenizer.nextToken());
        segment.dt = Double.parseDouble(line_tokenizer.nextToken());
        segment.x = Double.parseDouble(line_tokenizer.nextToken());
        segment.y = Double.parseDouble(line_tokenizer.nextToken());
        right.setSegment(i, segment);
    }
    System.out.println("...finished parsing path from string.");
    return new Path(name, new Trajectory.Pair(left, right));
}

Example 3 with Segment

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

the class Path method canFlip.

public boolean canFlip(int segmentNum) {
    Segment a = go_left_pair_.right.getSegment(segmentNum);
    Segment b = go_left_pair_.left.getSegment(segmentNum);
    return (a.pos == b.pos) && (a.vel == b.vel);
}

Example 4 with Segment

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

the class Path method getEndHeading.

public double getEndHeading() {
    int numSegments = getLeftWheelTrajectory().getNumSegments();
    Segment lastSegment = getLeftWheelTrajectory().getSegment(numSegments - 1);
    return lastSegment.heading;
}

Example 5 with Segment

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

the class TextFileSerializer method serializeTrajectory.

private String serializeTrajectory(Trajectory trajectory) {
    String content = "";
    for (int i = 0; i < trajectory.getNumSegments(); ++i) {
        Segment segment = trajectory.getSegment(i);
        content += String.format("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", segment.pos, segment.vel, segment.acc, segment.jerk, segment.heading, segment.dt, segment.x, segment.y);
    }
    return content;
}