Wpi trajectory support

[gnawinganimal]

Support for WPILib trajectories. Might change later, but this is good for now ™

[gnawinganimal]

So to use this you would need to make a path follower in your drivetrain.

class Driveytrain : Subsystem("Driveytrain") {
    val follower = follower {
        pose { odometry.getPoseMeters }
        outputVelocity { l, r -> setTargetVelocity(l.m, r.m) }
        trackWidth = 0.0
    }
}

Then you can run a path in a command like this:

doTask {
    val drivetrain = use<Driveytrain>()
    action {
        drivetrain.follower.followPath {
            maxVel = 0.0
            maxAcc = 0.0
            p(0.0, 0.0) ang 0.0
        }
    }
}

[gnawinganimal]

This is wrong I'm going to change it

[mckelveygreg]

Is this PR still relevant @Jamdan2 ?