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

Example 11 with Path

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

the class RedClose_Paths method calculateRedClose.

public static void calculateRedClose(final FieldConfig field) {
    if (bsToRight == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        // Path from Red Far Stone to Right Column
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 21, 23 - field.getFarRedCenterToWall(), Math.toRadians(14)));
        bsToRight = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
    }
}

Example 12 with Path

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

the class RedFar_Paths method calculateRedFar.

public static void calculateRedFar(final FieldConfig field) {
    if (bsToRight == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        /**
         * Right *
         */
        // Path from Red Far Stone to Right Column
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 20, 29 - field.getFarRedCenterToWall(), Math.toRadians(15)));
        bsToRight = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
        /**
         * Center *
         */
        // Path from Red Fat Stone to Center Column
        waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 18, 29 - field.getFarRedCenterToWall(), -15));
        bsToCenter = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
        /**
         * Left To Glyph Pit*
         */
        waypointSequence = new WaypointSequence(5);
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(33, -10, 0));
        leftToGP = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
        leftToGP.goLeft();
    }
}

Example 13 with Path

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

the class AdaptivePurePursuitController method getDirection.

/**
 * Gives the direction the robot should turn to stay on the path
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return the direction the robot should turn: -1 is left, +1 is right
 */
public static int getDirection(Pose2d pose, Translation2d point) {
    Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point);
    Translation2d robot = pose.getRotation().toTranslation();
    double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x();
    // if robot < pose turn left
    return (cross < 0) ? -1 : 1;
}

Example 14 with Path

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

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

the class BlueFar_Paths method calculateBlueFar.

public static void calculateBlueFar(final FieldConfig field) {
    if (bsToRight == null) {
        WaypointSequence waypointSequence = new WaypointSequence(5);
        // Path from Red Far Stone to Right Column
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
        waypointSequence.addWaypoint(new WaypointSequence.Waypoint(field.getFarRedCryptoboxToFarStone() - 21, 23 - field.getFarRedCenterToWall(), Math.toRadians(14)));
        bsToRight = PathGenerator.makePath(waypointSequence, defaultTrajectoryConfig, WHEEL_BASE, "");
    }
}