Examples with Trajectory me.wobblyyyy.pathfinder2.trajectory.Trajectory used on opensource projects

Example 61 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestableRobot method testTestTrajectory.

@Test
public void testTestTrajectory() {
    Assertions.assertThrows(RuntimeException.class, () -> {
        Trajectory trajectory = new EmptyTrajectory();
        PointXYZ target = PointXYZ.ZERO;
        double maxTimeMs = 0;
        testTrajectory(trajectory, target, maxTimeMs);
    });
}

Example 62 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory 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 63 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory 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 64 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.

the class TrajectoryGraph method main.

public static void main(String... args) {
    TrajectoryGenerator.Config defaultTrajectoryConfig = new TrajectoryGenerator.Config();
    // In/s
    defaultTrajectoryConfig.max_vel = 23.832;
    // In/s^2
    defaultTrajectoryConfig.max_acc = 40;
    // In/s^3
    defaultTrajectoryConfig.max_jerk = 40;
    // seconds, change of time in each update
    defaultTrajectoryConfig.dt = 0.01;
    Trajectory trajectory = TrajectoryGenerator.generate(defaultTrajectoryConfig, TrajectoryGenerator.TrapezoidalStrategy, 0, 0, 15, 0, 0);
    System.out.println(trajectory.toString());
}

Example 65 with Trajectory

use of me.wobblyyyy.pathfinder2.trajectory.Trajectory in project Relic_Main by TeamOverdrive.

the class TestingTrajectory 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;
    // Config for the based on what we want the robot to do, not
    // based on the characteristics of the robot.
    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;
    TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.TrapezoidalStrategy;
    Trajectory reference = TrajectoryGenerator.generate(config, strategy, // start velocity
    0.0, 0, Math.abs(distance), // goal velocity
    0.0, 0);
    Trajectory leftProfile = reference;
    // Copy
    Trajectory rightProfile = reference.copy();
    double radius = Math.abs(Math.abs(distance) / (angle * Math.PI / 180.0));
    double width = 12.625;
    double faster = (radius + (width / 2.0)) / radius;
    double slower = (radius - (width / 2.0)) / radius;
    if (angle > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    double scaleFactor = 1.0 / 30.0;
    leftProfile.scale(scaleFactor);
    rightProfile.scale(scaleFactor);
    double max = 0;
    for (int i = 0; i < leftProfile.getNumSegments(); i++) {
        if (leftProfile.getSegment(i).vel > max)
            max = leftProfile.getSegment(i).vel;
    }
    System.out.println(leftProfile.getNumSegments());
    System.out.println(max);
// System.out.println(leftProfile.toStringProfile());
// System.out.println(rightProfile.toStringProfile());
}