Problems with Pathfinder Motion Profiling

Hello, our team has been using the Pathfinder library in our robot code for autonomous motion profiling. We have tested it and are facing multiple issues. We have a standard West Coast Drive and TalonSRX and VictorSPX motor controllers. Here are some things about our robot which could possibly create a problem:

  1. Does the path have to start at (0,0,0)? We are using the path that is shown in the Pathfinder Github README to test following, which does not start at (0,0,0).

  2. Is there some way to accurately visualize the path over a field? This would help us ensure that the robot is following the correct path.

  3. We are currently using tank drive with PercentOutput on the two master motors and Follower control mode on the slaves. Masters are TalonSRX and Salves are Victor SPX.

  4. In order to move forward, the left side requires positive percent output and the right side requires negative percent output. Do we need to configure it such that to move forward, the percent output is positive on both sides?

  5. Currently, when moving forward, the encoder values for one side increases positively while the other side decreases (going negative). Is there a specific way we need to configure the sensor phases?

  6. When we are setting our left and right motor to the outputs calculated, should we add or subtract turn and for which side?

  7. We specified max velocity, acceleration, etc in meters. Are we right to assume then that the coordinate system is in meters as well?

Here is some of our code:

@Override
    public void robotInit() {
        //Auto Dashboard Initialization
        _chooser.addDefault("Default Auto", Constants.kDefaultAuto);
        SmartDashboard.putData("Auto choices", _chooser);
        _gameData = DriverStation.getInstance().getGameSpecificMessage();
        SmartDashboard.putString("Game Data", _gameData);

        //Controller Initialization
        _elevatorIntakeJoystick = new Joystick(Constants.kElevatorIntakeJoystickId);
        _xboxDriveController = new XboxController(Constants.kDriveControllerId);
        _armJoystick = new Joystick(Constants.kArmJoystickId);

        //Pneumatics Initialization
        _compressor = new Compressor(Constants.kCompressorId);
        _compressor.setClosedLoopControl(true);
        _shifter = new DoubleSolenoid(Constants.kShifterDoubleSolenoidId, Constants.kShifterPositive, Constants.kShifterNegative);
        _elevatorShifter = new DoubleSolenoid(Constants.kElevatorShifterDoubleSolenoidId, Constants.kElevatorShifterPositive, Constants.kElevatorShifterNegative);
        _intakeShifter = new DoubleSolenoid(Constants.kIntakeShifterDoubleSolenoidId, Constants.kIntakeShifterPositive, Constants.kIntakeShifterNegative);

        //Drive Initialization
        _leftMasterDrive = new WPI_VictorSPX(Constants.kLeftMasterDriveCanId);
        _leftSlaveDrive = new WPI_VictorSPX(Constants.kLeftSlaveDriveCanId);
        _rightMasterDrive = new WPI_VictorSPX(Constants.kRightMasterDriveCanId);
        _rightSlaveDrive = new WPI_VictorSPX(Constants.kRightSlaveDriveCanId);

        _leftMasterDrive.setSafetyEnabled(Constants.kDriveMotorsSafetyEnabled);
        _rightMasterDrive.setSafetyEnabled(Constants.kDriveMotorsSafetyEnabled);
        _leftSlaveDrive.setSafetyEnabled(Constants.kDriveMotorsSafetyEnabled);
        _rightSlaveDrive.setSafetyEnabled(Constants.kDriveMotorsSafetyEnabled);
        
        _leftMasterDrive.configSelectedFeedbackSensor(com.ctre.phoenix.motorcontrol.FeedbackDevice.QuadEncoder, Constants.kDrivePidIdx, Constants.kDriveEncoderTimeout); // Device, pidIdx, timeout
        _leftMasterDrive.setSensorPhase(true);
        _rightMasterDrive.configSelectedFeedbackSensor(com.ctre.phoenix.motorcontrol.FeedbackDevice.QuadEncoder, Constants.kDrivePidIdx, Constants.kDriveEncoderTimeout);
        _rightMasterDrive.setSensorPhase(false);
        _rightMasterDrive.setInverted(false);
        _leftMasterDrive.setInverted(false);

        _leftMasterDrive.setNeutralMode(Constants.kDriveNeutralMode);
        _leftSlaveDrive.setNeutralMode(Constants.kDriveNeutralMode);
        _rightMasterDrive.setNeutralMode(Constants.kDriveNeutralMode);
        _rightSlaveDrive.setNeutralMode(Constants.kDriveNeutralMode);

        _leftMasterDrive.set(Constants.kDriveControlMode, 0.0);
        _leftSlaveDrive.set(Constants.kSlaveDriveControlMode, Constants.kLeftMasterDriveCanId);
        _rightMasterDrive.set(Constants.kDriveControlMode, 0.0);
        _rightSlaveDrive.set(Constants.kSlaveDriveControlMode, Constants.kRightMasterDriveCanId);

        _leftSlaveDrive.follow(_leftMasterDrive);
        _rightSlaveDrive.follow(_rightMasterDrive);

        _drive = new DifferentialDrive(_rightMasterDrive, _leftMasterDrive);        

        _drive.setSafetyEnabled(Constants.kDriveSafetyEnabled);	

	//NavX Initialization
        try {
            _ahrs = new AHRS(SPI.Port.kMXP);
            _isNavX = true;
        } catch (RuntimeException ex) {
             DriverStation.reportError("Error Connecting to navX" + ex.getMessage(), true);
             _isNavX = false;
        }
    }

@Override
    public void autonomousInit() {
        _autoSelected = _chooser.getSelected();
        // autoSelected = SmartDashboard.getString("Auto Selector",
        // defaultAuto);
        System.out.println("Auto selected: " + _autoSelected);
       
        setDriveHighGear(false);
        
        switch (_autoSelected) {
        case Constants.kDefaultAuto:
            if (_gameData.charAt(0) == 'L') {
                Waypoint] points = new Waypoint] {
                    new Waypoint(0, 0, 0),
                    new Waypoint(1, 1, Pathfinder.d2r(45))    
                };
                 
                _currentAutoTrajectory = this.setMotionProfile(points);
            } else if (_gameData.charAt(0) == 'R') {
              
            } else {
                System.out.println("Game data not found.");
            }
            break;
        default:
            // Put default auto code here
            break;
        }
    }

    @Override
    public void autonomousPeriodic() {
        smartDashboardUpdate();

        switch (_autoSelected) {
            case Constants.kDefaultAuto:
                if (_gameData.charAt(0) == 'L') {
                    this.runMotionProfile(_currentAutoTrajectory);
                } else if (_gameData.charAt(0) == 'R') {
                    
                } else {
                    // Game data not found
                }
                break;
            default:
                break;
        }
    }
    
    public Trajectory setMotionProfile(Waypoint] waypoints) {
        _rightMasterDrive.setSelectedSensorPosition(0, 0, 0);
        _leftMasterDrive.setSelectedSensorPosition(0, 0, 0);
        
        _ahrs.zeroYaw();        
        
        SmartDashboard.putBoolean("Generated Profile", false);

        Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.005, Constants.kDriveMaxVelocity, 3.00, 120.0);
        
        Trajectory autonomousTrajectory = Pathfinder.generate(waypoints, config);
        
        SmartDashboard.putBoolean("Generated Profile", true);

        return autonomousTrajectory;
    }

    public void runMotionProfile(Trajectory trajectory) {
        TankModifier modifier = new TankModifier(trajectory);
      
        modifier.modify(Constants.kWheelbaseWidth);

        EncoderFollower leftFollower = new EncoderFollower(modifier.getLeftTrajectory());
        EncoderFollower rightFollower = new EncoderFollower(modifier.getRightTrajectory());

        leftFollower.configureEncoder(_leftMasterDrive.getSelectedSensorPosition(Constants.kDrivePidIdx), 256, Constants.kWheelDiameter);
        rightFollower.configureEncoder(_rightMasterDrive.getSelectedSensorPosition(Constants.kDrivePidIdx), 256, Constants.kWheelDiameter);

        leftFollower.configurePIDVA(Constants.kDriveP, Constants.kDriveI, Constants.kDriveD, 1 / Constants.kDriveMaxVelocity, 0.8);
        rightFollower.configurePIDVA(Constants.kDriveP, Constants.kDriveI, Constants.kDriveD, 1 / Constants.kDriveMaxVelocity, 0.8);
//        right.configurePIDVA(0.01, 0.00003, 0.1, 1 / max_velocity, 0);

        double outputLeft = leftFollower.calculate(_leftMasterDrive.getSelectedSensorPosition(Constants.kDrivePidIdx));
        double outputRight = rightFollower.calculate(_rightMasterDrive.getSelectedSensorPosition(Constants.kDrivePidIdx));

        double gyro_heading = _ahrs.getAngle();    // Assuming the gyro is giving a value in degrees
        double desired_heading = Pathfinder.r2d(leftFollower.getHeading());  // Should also be in degrees

        double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
        double turn = 0.8 * (-1.0/80.0) * angleDifference;

        SmartDashboard.putNumber("Right Profile", (outputRight + turn));
        SmartDashboard.putNumber("Left Profile", (outputLeft - turn));
        SmartDashboard.putNumber("Output Left", outputLeft);
        SmartDashboard.putNumber("Output Right", outputRight);
        SmartDashboard.putNumber("Turn", turn);
        SmartDashboard.putNumber("Desired Heading", leftFollower.getHeading());
        SmartDashboard.putNumber("Right Drive", -(outputRight + turn));
        SmartDashboard.putNumber("Left Drive", -(outputLeft - turn));
        
        _drive.tankDrive(outputLeft - turn, -outputRight - turn);
    }

Thanks,
Team 1414 (IHOT Robotics)

In order to avoid confusion about the motor direction, use setInverted() on the TalonSRXs and the Victors SPXs (you need to do this for both master and follower controllers). Invert the motors so that when you set a positive voltage on each side, the robot drives forward. Then change the sensor direction so that positive motor output corresponds to positive sensor output. Use setSensorPhase() if your encoders are connected to the TalonSRXs, or setReverseDirection() if you’re using the WPI library Encoder class.

You can visualize paths with this GUI here. The GUI can also generate CSV paths which can be read by pathfinder which allows for faster load times as the rio will not have to generate the paths each time on init. As for units, any units can be used as long as it is consistent throughout (velocity, acceleration, jerk, wheel diameter, etc).

  1. Paths normally start at <0,0,0>, but in Pathfinder all waypoints are relative to the first. You can have the first waypoint at anything you want, but usually we allocate <0,0,0> as the robots starting position.

  2. You can export a CSV and view it in Microsoft Excel, Tableau, or use Vannaka’s generator as mentioned above.

  3. That should be fine

  4. Set your right side motors to be inverted, this will also correctly invert the reported encoder position (use SetInverted or whatever CTRE’s method is)

  5. See #4

  6. It depends on if your gyro reads CCW-positive or CW-positive. You will have to experiment. If it’s turning the wrong way, flip it.

  7. Yes, the base unit for pathfinder is defined by the waypoints, all other units are derived from that.

Thanks,
~Jaci

Thank you for your help Jaci! It seems like we are getting closer to getting it fully functional. At the moment, it seems to follow the path in the beginning, but then just starts turning/oscillating without finishing the path. We suspect it may be the P-gain for turn, but we aren’t sure. We tried swapping the + and - of turn from right to left, but oddly enough, it didn’t seem to dramatically change the path the robot followed. Also, we have an elevator and arm that need to move during auto. Is there a way to get the current progress along the path so we can change the target set points of our arm and elevator during auto (also drop the cube).

Thanks, Justin

If it’s oscillating randomly, that usually suggests that your encoders are not working properly and the gyro loop is taking over. Make sure you’re also tuning your PDVA loop one term at a time, otherwise it’s hard to strike a nice balance (set kV and kA manually, then P, then D)

As for the elevator arm and such, you can use the position term of the center (or one of the other) path segments that will increase towards path completion.

Hi Jaci, I’m not using PathFinder but I was wondering if you had a general process for tuning a PDVA controller. In the past with basic PD turns, I have increased kP until steady-state error/oscillations and then added a kD to stop the oscillations. Is it similar for the kP and kD constants in the PDVA? Also, when setting kV and kA manually, is there an equation to follow or is it just trial and error with plugging in values?

Thanks, Rohit

We’ve just started working with this tool, ourselves.


It will help you to get, at the very least, some good PDVA parameters that you can start with.

Also, if you’d like to know more about it, read up on this thread.

Thank you. I’ll make sure to look at it.

Hello! My team has been having similar issues as stated above, however we are using Spark MAX’s, NEO’s and robotpy. I have a few questions about the arguments requested by pathfinder. These include: pf.generate() variable ‘dt’ and ‘max_jerk’. Is this library only functional with TimedRobots?
Thanks!

Pathfinder doesn’t have a requirement for a specific framework. However, it does have a requirement to be called at a repeatable rate (that is specified by dt).