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

Example 6 with Path

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

the class Arc method calculate.

public static Path calculate(TrajectoryGenerator.Config config, TrajectoryGenerator.Strategy strategy, double start_velocity, double start_heading, double distance, double goal_velocity, double goal_heading) {
    // Generate trajectory based to the inputs
    Trajectory reference = TrajectoryGenerator.generate(config, strategy, // start velocity
    start_velocity, // start heading
    start_heading, // goal position
    Math.abs(distance), // goal velocity
    goal_velocity, // goal angle
    goal_heading);
    Trajectory leftProfile = reference;
    Trajectory rightProfile = reference.copy();
    // Calculate the radius of the arc
    double radius = Math.copySign(Math.abs(Math.abs(distance) / (goal_heading - start_heading * Math.PI / 180.0)), distance);
    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;
    System.out.println(faster);
    System.out.println(slower);
    // Determine which way to curve
    if (goal_heading - start_heading > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else if (goal_heading - start_heading < 0) {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    Path ref = new Path("Arc", new Trajectory.Pair(leftProfile, rightProfile));
    if (distance < 0)
        InvertY.calculate(ref);
    return ref;
}

Example 7 with Path

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

the class TestingSplinesDoubleTrajectory method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    // Used to log the speed and position to be able to plot the data in Excel
    PhoneLogger logger = new PhoneLogger("SpeedPosTimeData.csv");
    // Generate a simple path for testing
    Path path = Line.calculate(defaultTrajectoryConfig, TrajectoryGenerator.SCurvesStrategy, 0, 0, 15, 0);
    // Init our drivetrain
    Drive mDrive = new Drive();
    mDrive.init(this, true);
    // Use speed control to make the output of the motor linear
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    /*
        * Used to make it easier to save constants
        * P term, D term, V term, A term, P term for gyro compensation
        * We are not using I because the feed-forward + feed-back control
        * will be enough to control the system
        */
    FollowerConfig followerConfig = new FollowerConfig(Constants.p.getDouble(), Constants.d.getDouble(), Constants.v.getDouble(), Constants.a.getDouble(), Constants.headingP.getDouble());
    // New Drive Controller
    TrajectoryDriveController controller = new TrajectoryDriveController(mDrive, followerConfig);
    // Tell the controller what profiles to follow
    // (the two 1's are in case we want to mirror the path)
    controller.loadPath(path, 1, 1);
    // Lets go
    telemetry.setAutoClear(false);
    telemetry.addData("Ready to Start", "");
    telemetry.update();
    waitForStart();
    telemetry.setAutoClear(true);
    telemetry.clearAll();
    // Used to calculate the change in time from the last reading
    ElapsedTime dtTimer = new ElapsedTime();
    ElapsedTime timeToComplete = new ElapsedTime();
    double totalTime = 0;
    boolean first = true;
    // Used to calculate speed
    double lastPosition = 0;
    while (opModeIsActive()) {
        // data
        telemetry.addData("Time To Complete", totalTime);
        telemetry.addData("Left Distance", mDrive.getLeftDistanceInches());
        telemetry.addData("Right Distance", mDrive.getRightDistanceInches());
        telemetry.addData("Left Speed", controller.wantedLeftSpeed);
        telemetry.addData("Right Speed", controller.wantedRightSpeed);
        telemetry.addData("Speed output", controller.getGoal());
        telemetry.addData("Gyro", mDrive.getGyroAngleRadians());
        telemetry.addData("dt", dtTimer.seconds());
        telemetry.addData("Number of Segments", controller.getNumSegments());
        telemetry.addData("Current Pos", controller.getFollowerCurrentSegment());
        telemetry.update();
        // Make sure we only update when we told the controller/trajectory we would
        if (dtTimer.seconds() > defaultTrajectoryConfig.dt && !controller.isOnTarget()) {
            // Update our controller
            controller.update();
            // Used to log data so we can plot it
            double currentTime = timeToComplete.milliseconds();
            double currentLeftPos = mDrive.getLeftDistanceInches();
            double left_speed = (currentLeftPos - lastPosition) / dtTimer.milliseconds();
            logger.write(String.valueOf(currentTime) + "," + String.valueOf(path.getLeftWheelTrajectory().getSegment(controller.getFollowerCurrentSegment()).pos) + "," + String.valueOf(currentLeftPos));
            idle();
            dtTimer.reset();
        } else if (controller.isOnTarget()) {
            mDrive.setLeftRightPower(0, 0);
            if (first) {
                // Track how long it took to complete the path
                totalTime = timeToComplete.seconds();
                first = false;
            }
        }
    }
    // Close the logger
    logger.close();
    // Be able to move the robot after we run it
    mDrive.setRunMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}

Example 8 with Path

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

the class PathFollowing method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    waitForStart("Path", true);
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    config.dt = .01;
    config.max_acc = 10.0;
    config.max_jerk = 60.0;
    config.max_vel = 15.0;
    final double kWheelbaseWidth = 12.625;
    // Path name must be a valid Java class name.
    final String path_name = "RedCloseCryptobox";
    // Description of this auto mode path.
    // Remember that this is for the GO LEFT CASE!
    WaypointSequence p = new WaypointSequence(10);
    p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    p.addWaypoint(new WaypointSequence.Waypoint(7.0, 0, 0));
    Path path = PathGenerator.makePath(p, config, kWheelbaseWidth, path_name);
    // Robot.getDrive().loadProfile(path.getLeftWheelTrajectory(),
    // path.getRightWheelTrajectory(), 0, 0);
    // 
    // while (opModeIsActive() && !Robot.getDrive().onTarget()){
    // Robot.getDrive().update(1, 1);
    // }
    Robot.getDrive().setLeftRightPower(0, 0);
}

Example 9 with Path

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

the class Drive method driveTrajectory.

public void driveTrajectory(double leftDistance, double rightDistance, long timeout) {
    int lastLeft = getLeftCurrentPosition();
    int lastRight = getRightCurrentPosition();
    Trajectory left;
    Trajectory right;
    left = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(leftDistance), 0, 0);
    right = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(rightDistance), 0, 0);
    if (leftDistance < 0) {
        left.scale(-1);
    }
    if (rightDistance < 0) {
        right.scale(-1);
    }
    new FollowPath(linearOpMode, this, new Path("", new Trajectory.Pair(left, right)), timeout, 1, 1);
    setRunMode(DcMotor.RunMode.RUN_TO_POSITION);
    this.setLeftRightTarget((int) ((leftDistance * COUNTS_PER_INCH) + lastLeft), (int) ((rightDistance * COUNTS_PER_INCH) + lastRight));
    setLeftRightPower(0.2, 0.2);
    while (linearOpMode.opModeIsActive() && (leftMotor.isBusy() || rightMotor.isBusy())) Thread.yield();
    setLeftRightPower(0, 0);
    setRunMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}

Example 10 with Path

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

the class TestingFIles method main.

public static void main(String... args) {
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    // In/s
    config.max_vel = 23.832;
    // In/s^2
    config.max_acc = 11.9;
    // In/s^3
    config.max_jerk = 10;
    config.dt = 0.01;
    // WaypointSequence waypointSequence = new WaypointSequence(10);
    // waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    // waypointSequence.addWaypoint(
    // new WaypointSequence.Waypoint(4, 0, 0));
    // waypointSequence.addWaypoint(new WaypointSequence.Waypoint(42.25-30, 34.5-26, Math.toRadians(5)));
    // WaypointSequence blueClose = new WaypointSequence(5);
    // blueClose.addWaypoint(new WaypointSequence.Waypoint(0, 4, 0));
    // blueClose.addWaypoint(new WaypointSequence.Waypoint(, 0, 0));
    // blueClose.addWaypoint(new WaypointSequence.Waypoint(5, 36, Math.toRadians(-10)));
    // 
    // Path path = PathGenerator.makePath(blueClose, config, 12.625, "");
    // System.out.println(path.getLeftWheelTrajectory().toStringEuclidean());
    WaypointSequence waypointSequence = new WaypointSequence(10);
    double xOffset = 120;
    double yOffset = 96;
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(96 - yOffset, 120 - xOffset, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(126 - yOffset, 115 - xOffset, Math.toRadians(-15)));
    Path pathToRightColumnRedFar = PathGenerator.makePath(waypointSequence, config, 12.625, "");
    System.out.println(pathToRightColumnRedFar.getLeftWheelTrajectory().toStringEuclidean());
}