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

Example 6 with Segment

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

the class Path method verifySpeeds.

/**
 * Ensures that all speeds in the path are attainable and robot can slow down in time
 */
public void verifySpeeds() {
    double maxStartSpeed = 0.0;
    double[] startSpeeds = new double[segments.size() + 1];
    startSpeeds[segments.size()] = 0.0;
    for (int i = segments.size() - 1; i >= 0; i--) {
        PathSegment segment = segments.get(i);
        maxStartSpeed += Math.sqrt(maxStartSpeed * maxStartSpeed + 2 * Constants.kPathFollowingMaxAccel * segment.getLength());
        startSpeeds[i] = segment.getStartState().vel();
        // System.out.println(maxStartSpeed + ", " + startSpeeds[i]);
        if (startSpeeds[i] > maxStartSpeed) {
            startSpeeds[i] = maxStartSpeed;
        // System.out.println("Segment starting speed is too high!");
        }
        maxStartSpeed = startSpeeds[i];
    }
    for (int i = 0; i < segments.size(); i++) {
        PathSegment segment = segments.get(i);
        double endSpeed = startSpeeds[i + 1];
        MotionState startState = (i > 0) ? segments.get(i - 1).getEndState() : new MotionState(0, 0, 0, 0);
        startState = new MotionState(0, 0, startState.vel(), startState.vel());
        segment.createMotionProfiler(startState, endSpeed);
    }
}

Example 7 with Segment

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

the class PathSegment method getRemainingDistance.

/**
 * Gets the remaining distance left on the segment from point <code>point</code>
 *
 * @param position result of <code>getClosestPoint()</code>
 * @return distance remaining
 */
public double getRemainingDistance(Translation2d position) {
    if (isLine) {
        return new Translation2d(end, position).norm();
    } else {
        Translation2d deltaPosition = new Translation2d(center, position);
        double angle = Translation2d.getAngle(deltaEnd, deltaPosition).getRadians();
        double totalAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians();
        return angle / totalAngle * getLength();
    }
}

Example 8 with Segment

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

the class PathSegment method getPointByDistance.

/**
 * Calculates the point on the segment <code>dist</code> distance from the starting point along the segment.
 *
 * @param dist distance from the starting point
 * @return point on the segment <code>dist</code> distance from the starting point
 */
public Translation2d getPointByDistance(double dist) {
    double length = getLength();
    if (!extrapolateLookahead && dist > length) {
        dist = length;
    }
    if (isLine) {
        return start.translateBy(deltaStart.scale(dist / length));
    } else {
        double deltaAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians() * ((Translation2d.cross(deltaStart, deltaEnd) >= 0) ? 1 : -1);
        deltaAngle *= dist / length;
        Translation2d t = deltaStart.rotateBy(Rotation2d.fromRadians(deltaAngle));
        return center.translateBy(t);
    }
}