Go to Post Sometime it makes me think what would I do without FIRST... - Arefin Bari [more]
Go Back   Chief Delphi > Technical > Programming > Python
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 03-16-2018, 04:50 PM
adamthekiwi adamthekiwi is offline
Registered User
FRC #6517
Join Date: Jan 2018
Location: Tennessee
Posts: 6
adamthekiwi is an unknown quantity at this point
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?


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

    # Pathfinder constants
    MAX_VELOCITY = 11  # ft/s

    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()

        info, trajectory = pf.generate(points, pf.FIT_HERMITE_CUBIC, pf.SAMPLES_HIGH,

        # 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)
            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)
            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):

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

        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
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):

    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):

if __name__ == "__main__":
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.
Reply With Quote
  #2   Spotlight this post!  
Unread 03-16-2018, 09:30 PM
Brian M's Avatar
Brian M Brian M is offline
Design Lead
FRC #1360 (Orbit Robotics)
Team Role: CAD
Join Date: Feb 2017
Rookie Year: 2012
Location: Ontario, Canada
Posts: 307
Brian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond reputeBrian M has a reputation beyond repute
Re: Pathfinder Help

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.
2016 - Rah Cha Cha Ruckus Winners (3015 & 4039)
2017 - Industrial Design (Durham), Entrepreneurship (McMaster), Quality Award (Ontario DCMPS)
2018 - Georgian District Winner (1305 & 6864), York District Winner (1325 & 6140), York District Chairmans Award, Georgian Innovation in Control

Reply With Quote
  #3   Spotlight this post!  
Unread 03-16-2018, 10:09 PM
virtuald's Avatar
virtuald virtuald is offline
RobotPy Guy
AKA: Dustin Spicuzza
FRC #6367 (), FRC #7240 ()
Team Role: Mentor
Join Date: Dec 2008
Rookie Year: 2003
Location: Boston, MA
Posts: 1,382
virtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant futurevirtuald has a brilliant future
Re: Pathfinder Help

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.readthedoc...g-a-trajectory
Maintainer of RobotPy (Python for FRC) & WPILib Contributor
Creator of pyfrc (Robot Simulator + utilities for Python), pynetworktables/pynetworktables2js (NetworkTables for Python & Javascript), and lots more...

Team #1418 (remote mentor): Newton Quarterfinalists, 2016 Chesapeake District Champion, 2x Innovation in Control award, 2x district event winner
Team #1418: 2015 DC Regional Innovation In Control Award, #2 seed; 2014 VA Industrial Design Award; 2014 Finalists in DC & VA
Team #2423: 2012 & 2013 Boston Regional Innovation in Control Award

Resources: FIRSTWiki (relaunched!) | My Software Stuff
Reply With Quote

Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump

All times are GMT -5. The time now is 01:45 AM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2018, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi