Pathfinder Help

We are having issues using robotpy-pathfinder. When we run our code in autonomous, our robot jitters back and forth. We’ve attached some of our code below. What are some common pitfalls or mistakes implementing pathfinder?

subsystems.py


class PathFinder():
    WHEEL_DIAMETER = 0.5  # 6 inches
    ENCODER_COUNTS_PER_REV = 4096 # mag encoder

    # Pathfinder constants
    MAX_VELOCITY = 11  # ft/s
    MAX_ACCELERATION = 6

    WHEEL_BASE = (29/12) #29 inches

    def __init__(self, points, period, gyro, left_motor, right_motor):
        self.gyro = gyro
        self.l_motor = left_motor
        self.r_motor = right_motor

        # self.r_motor.invert()
        self.reset()

        info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,
                                       dt=period,
                                       max_velocity=self.MAX_VELOCITY,
                                       max_acceleration=self.MAX_ACCELERATION,
                                       max_jerk=120.0)

        # Wheelbase Width = 2 ft
        modifier = pf.modifiers.TankModifier(trajectory).modify(self.WHEEL_BASE)

        # Do something with the new Trajectories...
        left = modifier.getLeftTrajectory()
        right = modifier.getRightTrajectory()

        leftFollower = pf.followers.EncoderFollower(left)
        leftFollower.configureEncoder(
            self.l_motor.getRawPosition(), 4096, self.WHEEL_DIAMETER)
        leftFollower.configurePIDVA(1.0, 0.0, 0, 1 / self.MAX_VELOCITY, 0)
        leftFollower.segment = 0

        rightFollower = pf.followers.EncoderFollower(right)
        rightFollower.configureEncoder(
            self.r_motor.getRawPosition(), 4096, self.WHEEL_DIAMETER)
        rightFollower.configurePIDVA(1.0, 0.0, 0, 1 / self.MAX_VELOCITY, 0)
        rightFollower.segment = 0

        self.leftFollower = leftFollower
        self.rightFollower = rightFollower

    def reset(self):
        self.l_motor.resetPostion()
        self.r_motor.resetPostion()
        self.gyro.reset()

    def calculate(self):
        lvel = self.leftFollower.calculate(
            self.l_motor.getRawPosition())
        rvel = self.rightFollower.calculate(
            self.l_motor.getRawPosition())

        print("l: {}".format(self.l_motor.getRawPosition()))
        print("r: {}".format(self.r_motor.getRawPosition()))

        gyro_heading = -self.gyro.getAngle()

        desired_heading = pf.r2d(self.leftFollower.getHeading())

        angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
        turn = 0.8 * (-1.0 / 80.0) * angleDifference

        lvel = lvel + turn
        rvel = rvel - turn
        # -1 is forward, so invert both values
        return -lvel, -rvel

robot.py


import wpilib
from subsystems import HiroChassis, HiroLauncher, HiroElevator, HiroPathfinder

class Hiro(wpilib.IterativeRobot, wpilib.TimedRobot):

    def robotInit(self):
        self.buttons = wpilib.Joystick(0)
        self.joystick = wpilib.Joystick(1)

        self.chassis = HiroChassis()
        self.launcher = HiroLauncher()
        self.elevator = HiroElevator()
        self.pathfinder = HiroPathfinder(self.getPeriod())

    def autonomousInit(self):
        self.pathfinder.reset()

    def autonomousPeriodic(self):
        l, r = self.pathfinder.calculate()
        self.chassis.tankDrive(l, r)
        # self.chassis.tankDrive(-0.5, -0.5)

    def teleopPeriodic(self):
        self.chassis.update(self.joystick, self.buttons)
        self.launcher.update(self.joystick, self.buttons)
        self.elevator.update(self.joystick, self.buttons)

    def testPeriodic(self):
        wpilib.LiveWindow.run()


if __name__ == "__main__":
    wpilib.run(Hiro)

I know this is in python, but it should be very similar to Java.
Also, motor.getRawPosition is the same as getQuadraturePosition.
The encoders are directly connected to the TalonSRXs.