Pathfinder Help

Technical Python
1

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.

2

Make sure your gyro and jacis angle correlate correctly, we had the issue where the Navx did 0-360 and jacis did 0-infinite (or something like that I’m not the programmer) depending on the side it turned so we made a logic statement that ensures they are in agreement on which angle they are each at.

3

As you said, it’s very similar to Java, I would browse the various threads on ChiefDelphi about pathfinder and take that advice into account.

In my limited experience playing with pathfinder, I’ve found that the computed turn value really affects turn performance more than anything else, and that the value in the example doesn’t work that well for us (the ‘turn = 0.8 * (-1.0 / 80.0) * angleDifference’ part). Changing those constants to be higher or lower will affect your turning.

Additionally, you should ensure that the max velocity that you pass to generate is actually a sane value for your robot. Figure out what the velocity is for your real robot, and then in simulation make sure that the speed value you pass to your drivetrain function is that same value.

Final note: you won’t be able to run pf.generate on the roborio. See the note in the documentation about how to store the path to a file: http://robotpy-pathfinder.readthedocs.io/en/stable/usage.html#generating-a-trajectory

4