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

Example 1 with TrajectoryFollower

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

the class TestingSplinesSingleTrajectory method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    telemetry.addData("Started", "");
    telemetry.update();
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    // In/s
    config.max_vel = 2.0 * 12;
    // In/s^2
    config.max_acc = 0.6 * 12;
    // In/s^3
    config.max_jerk = 0.4 * 12;
    // seconds
    config.dt = 0.01;
    Trajectory trajectory = TrajectoryGenerator.generate(config, TrajectoryGenerator.SCurvesStrategy, 0, 0, 10, 0, 0);
    telemetry.addData("Trajectory Generation Finished", "");
    telemetry.update();
    double gearing = Constants.DRIVE_GEAR_REDUCTION;
    // Volts
    double Vmax = 12;
    // lbs
    double mass = 48.0 / 9.8;
    double numberOfMotors = 2;
    // lb-in
    double stallTorque = 1.531179;
    double velocityMax = (5400 * Math.PI * Constants.WHEEL_DIAMETER_INCHES) / (gearing);
    double kv = Vmax / velocityMax;
    double accelerationMax = (2 * numberOfMotors * stallTorque * gearing) / (4.0 * mass);
    double ka = Vmax / accelerationMax;
    FollowerConfig followerConfig = new FollowerConfig(0.1, 0, kv, ka, Math.PI / 1000);
    TrajectoryFollower follower = new TrajectoryFollower("Follower");
    follower.configure(followerConfig.get()[0], followerConfig.get()[1], followerConfig.get()[2], followerConfig.get()[3], followerConfig.get()[4]);
    follower.setTrajectory(trajectory);
    Drive mDrive = new Drive();
    mDrive.init(this, true);
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    waitForStart();
    ElapsedTime time = new ElapsedTime();
    sleep(10);
    while (!follower.isFinishedTrajectory() && opModeIsActive()) {
        if (time.seconds() > config.dt) {
            double distance = (mDrive.getRightDistanceInches() + mDrive.getLeftDistanceInches()) / 2;
            double forwardSpeed = follower.calculate(distance);
            telemetry.addData("Distance", distance);
            telemetry.addData("Turning Error", mDrive.getGyroAngleDegrees() - follower.getHeading());
            telemetry.addData("Forward", forwardSpeed);
            telemetry.update();
            mDrive.setLeftRightPower(forwardSpeed, forwardSpeed);
            time.reset();
        }
    }
    mDrive.setLeftRightPower(0, 0);
}

Example 2 with TrajectoryFollower

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

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