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

Example 16 with Path

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

the class Drive method driveAction.

public void driveAction(double distance, double heading) {
    double curHeading = this.getGyroAngleDegrees();
    double deltaHeading = heading - curHeading;
    double radius = Math.abs(Math.abs(distance) / (deltaHeading * Math.PI / 180.0));
    TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.SCurvesStrategy;
    Trajectory reference = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, // start velocity
    0.0, curHeading, Math.abs(distance), // goal velocity
    0.0, heading);
    Trajectory leftProfile = reference;
    // Copy
    Trajectory rightProfile = reference.copy();
    double faster = (radius + (WHEEL_BASE / 2.0)) / radius;
    double slower = (radius - (WHEEL_BASE / 2.0)) / radius;
    System.out.println("faster " + faster);
    if (deltaHeading > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    new FollowPath(linearOpMode, this, new Path("", new Trajectory.Pair(leftProfile, rightProfile)), (distance > 0.0 ? 1.0 : -1.0), heading);
}

Example 17 with Path

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

the class NewTestSplines method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    telemetry.addData("P", Constants.p.getDouble());
    telemetry.addData("D", Constants.d.getDouble());
    telemetry.addData("V", Constants.v.getDouble());
    telemetry.addData("A", Constants.a.getDouble());
    telemetry.update();
    // Path driveOffStone = Line.calculate(defaultTrajectoryConfig,
    // SCurvesStrategy,
    // 0.0,
    // 0.00001,
    // 10,
    // 0.00001);
    // Path fullPath = ScalePath.calculate(driveOffStone,
    // 1.0/Constants.scale.getDouble());
    Trajectory ref = TrajectoryGenerator.generate(defaultTrajectoryConfig, TrajectoryGenerator.SCurvesStrategy, 0, 0, 10, 0, 0.000001);
    // Init our drivetrain
    Drive mDrive = new Drive();
    // true = use gyros and reset the encoders
    mDrive.init(this, true);
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    // Lets go
    telemetry.setAutoClear(false);
    telemetry.addData("Ready to Start", "");
    telemetry.update();
    waitForStart();
    telemetry.setAutoClear(true);
    // New Drive Controller
    TrajectoryDriveController controller = new TrajectoryDriveController(mDrive, defaultFollowerConfig);
    controller.loadProfile(ref, ref, 1, 1);
    ElapsedTime dtTimer = new ElapsedTime();
    while (opModeIsActive() && !controller.onTarget()) {
        // data
        telemetry.addData("kV", Constants.v.getDouble());
        telemetry.addData("kA", Constants.a.getDouble());
        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.getGyroAngleDegrees());
        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.update();
            idle();
            dtTimer.reset();
        }
    }
}

Example 18 with Path

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

the class TestCombinePaths method main.

public static void main(String... args) {
    TrajectoryGenerator.Config RedConfig = new TrajectoryGenerator.Config();
    RedConfig.max_vel = 23.23;
    RedConfig.max_acc = 200;
    RedConfig.max_jerk = 10000;
    RedConfig.dt = 0.01;
    double WHEEL_BASE = 12.625;
    WaypointSequence waypointSequence = new WaypointSequence(10);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(6, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(48.0 - 33, 36.0 - 28.5, Math.toRadians(-10)));
    Path pathToRightColumnRedFar = PathGenerator.makePath(waypointSequence, RedConfig, WHEEL_BASE, "");
    pathToRightColumnRedFar.goRight();
    waypointSequence = new WaypointSequence(10);
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    waypointSequence.addWaypoint(new WaypointSequence.Waypoint(10, 5, Math.toRadians(20)));
    Path fromRedRightToBox = PathGenerator.makePath(waypointSequence, RedConfig, WHEEL_BASE, "");
    fromRedRightToBox.getLeftWheelTrajectory().scale(-1);
    fromRedRightToBox.getRightWheelTrajectory().scale(-1);
    Trajectory.Pair base = new Trajectory.Pair(pathToRightColumnRedFar.getLeftWheelTrajectory(), pathToRightColumnRedFar.getRightWheelTrajectory());
    base.left.append(fromRedRightToBox.getLeftWheelTrajectory());
    base.right.append(fromRedRightToBox.getRightWheelTrajectory());
    Path output = new Path("Hello", base);
    System.out.println(output.getLeftWheelTrajectory().toStringEuclidean());
}

Example 19 with Path

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

the class InvertTest 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;
    // Path t = Arc.calculate(config, TrajectoryGenerator.SCurvesStrategy,
    // 0, 0, -5, 0, -Math.PI/2);
    Path curve = Arc.calculateAngleRadius(config, SCurvesStrategy, 0, 90, -12.625 / 2, 0, 0);
    System.out.println(curve.getLeftWheelTrajectory().toString());
    System.out.println(curve.getRightWheelTrajectory().toString());
}

Example 20 with Path

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

the class Blue_Close_Test_Path method main.

public static void main(String[] artgs) {
    TrajectoryGenerator.Config defaultTrajectoryConfig = new TrajectoryGenerator.Config();
    // Found by using "FindPDVA" class
    // In/s
    defaultTrajectoryConfig.max_vel = 23.832;
    // In/s^2
    defaultTrajectoryConfig.max_acc = 100;
    // In/s^3
    defaultTrajectoryConfig.max_jerk = 100;
    // seconds, change of time in each update
    defaultTrajectoryConfig.dt = 0.01;
    WaypointSequence blueClose = new WaypointSequence(5);
    blueClose.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    blueClose.addWaypoint(new WaypointSequence.Waypoint(36 - 8, 0, 0));
    blueClose.addWaypoint(new WaypointSequence.Waypoint(36, -6, -89));
    // Path blueFirstPath = PathGenerator.makePath(blueClose, defaultTrajectoryConfig, 12.625, "");
    double WHEEL_BASE = 12.625;
    WaypointSequence RightColumnToGlyphPit = new WaypointSequence(5);
    RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(13, -2, 0));
    RightColumnToGlyphPit.addWaypoint(new WaypointSequence.Waypoint(39, 4, Math.toRadians(4)));
    Path farRedRightToGlyphPit = PathGenerator.makePath(RightColumnToGlyphPit, defaultTrajectoryConfig, WHEEL_BASE, "");
    // System.out.println(farRedRightToGlyphPit.getRightWheelTrajectory().toStringEuclidean());
    System.out.println(farRedRightToGlyphPit.getLeftWheelTrajectory().toString());
}