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

Example 6 with Trajectory

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

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

the class Drive method leftMotorTurn.

public void leftMotorTurn(double degrees) {
    double leftDistance = (WHEEL_BASE * 2 * PI * degrees) / -360;
    leftDistance += this.getLeftDistanceInches();
    double sign = Math.abs(leftDistance) / leftDistance;
    Trajectory left;
    left = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(leftDistance), 0, 0);
    TrajectoryFollower leftFollower = new TrajectoryFollower("");
    leftFollower.configure(Constants.singleMotorP / 2, 0, 0, Constants.singleMotorV / 2, Constants.singleMotorA / 2);
    leftFollower.setTrajectory(left);
    setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
    ElapsedTime dtTimer = new ElapsedTime();
    while (linearOpMode.opModeIsActive() && !linearOpMode.isStopRequested() && !leftFollower.isFinishedTrajectory()) {
        if (dtTimer.seconds() > defaultTrajectoryConfig.dt) {
            // Update our controller
            setLeftRightPower(sign * leftFollower.calculate(sign * getLeftDistanceInches()), 0);
            Thread.yield();
            dtTimer.reset();
        }
    }
}

Example 8 with Trajectory

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

the class TestSplines 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;
    // What kind of trajectory profile do we want?
    TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.SCurvesStrategy;
    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;
    // Generate trajectory based to the inputs
    Trajectory reference = TrajectoryGenerator.generate(config, strategy, // start velocity
    0.0, // start heading
    0.0, // goal position
    Math.abs(distance), // goal velocity
    5, // goal angle
    angle);
    // Copy the trajectories to left and right
    // Right now the robot would go straight, if we purely used this traject.
    Trajectory leftProfile = reference;
    // Copy
    Trajectory rightProfile = reference.copy();
    // Calculate the radius of the arc
    double radius = Math.abs(Math.abs(distance) / (angle * 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 (angle > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    // Used to make sure that all the outputs are in the range of [-1,1]
    // This is not scientific but rather a guess based to testing
    double scaleFactor = 1.0 / 30.0;
    leftProfile.scale(scaleFactor);
    rightProfile.scale(scaleFactor);
    distance = 15;
    angle = -90;
    // Generate trajectory based to the inputs
    reference = TrajectoryGenerator.generate(config, strategy, // start velocity
    config.max_vel + 2, // start heading
    -0.00001, // goal position
    Math.abs(distance), // goal velocity
    0.0, // goal angle
    angle);
    // Copy the trajectories to left and right
    // Right now the robot would go straight, if we purely used this traject.
    Trajectory leftProfile2 = reference;
    // Copy
    Trajectory rightProfile2 = reference.copy();
    // Calculate the radius of the arc
    radius = Math.abs(Math.abs(distance) / (angle * Math.PI / 180.0));
    width = 12.;
    // Find the difference between the left and right motors
    faster = (radius + (width / 2.0)) / radius;
    slower = (radius - (width / 2.0)) / radius;
    // Determine which way to curve
    if (angle > 0) {
        leftProfile2.scale(faster);
        rightProfile2.scale(slower);
    } else {
        leftProfile2.scale(slower);
        rightProfile2.scale(faster);
    }
    // Used to make sure that all the outputs are in the range of [-1,1]
    // This is not scientific but rather a guess based to testing
    scaleFactor = 1.0 / 30.0;
    leftProfile2.scale(scaleFactor);
    rightProfile2.scale(scaleFactor);
    // leftProfile.append(leftProfile2);
    // rightProfile.append(rightProfile2);
    System.out.println(leftProfile.toStringProfile());
    System.out.println(leftProfile.getNumSegments() * config.dt);
}

Example 9 with Trajectory

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

Example 10 with Trajectory

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

the class TestTrajectoryGeneration 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;
    Trajectory test = TrajectoryGenerator.generate(config, TrajectoryGenerator.SCurvesStrategy, 0, 0, 24, 0, 0);
    TrajectoryFollower follower = new TrajectoryFollower("");
    follower.setTrajectory(test);
    follower.configure(0.1, 0.0, 0, 0.1, 0.1);
    // System.out.println(test.getNumSegments()*config.dt);
    for (int i = 0; i < 250; i++) {
        System.out.println(i / 10.0 + "\t" + follower.calculate(i / 10));
    }
}