Pathfinder Half Distance Bug

pathfinding

#1

We are currently testing out pathfinder for our auto this year. We have a command that, when run, generates a path from its current position to 1m in front of it, and then executes that path:

    public class DriveToTarget extends Command {
    // NetworkTable tapeTable;

      double maxVel = 2.8;
  
    Waypoint[] points;
    Trajectory.Config config;
    Trajectory trajectory;
    TankModifier modifier;

    EncoderFollower left;
    EncoderFollower right;

    double leftPower;
    double rightPower;

    int execCount = 0;

    long lastExecTime = 0;

    public DriveToTarget() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(Robot.driveTrain);
    }

    // Called just before this Command runs the first time
    @Override
    protected void initialize() {
        /*
        tapeTable = Robot.networkTableInstance.getTable("ReflectiveTapeContours");
        deltaYaw = (float) tapeTable.getEntry("estAngle").getDouble(0);
        startYaw = Robot.sensors.getYaw();
        currentYaw = Robot.sensors.getYaw();
        */

        long startTime = System.currentTimeMillis();

        System.out.println("Starting tank path generation.");
        
        points = new Waypoint[] {
        new Waypoint(0.0, 0.0, Pathfinder.d2r(90)),
        new Waypoint(0.0, 1.0, Pathfinder.d2r(90))
        };

        config = new Trajectory.Config(
        Trajectory.FitMethod.HERMITE_CUBIC,
        Trajectory.Config.SAMPLES_LOW,
        0.02, maxVel, 3.05, 50.0
        );

        trajectory = Pathfinder.generate(points, config);
        modifier = new TankModifier(trajectory).modify(0.5);

        left = new EncoderFollower(modifier.getLeftTrajectory());
        right = new EncoderFollower(modifier.getRightTrajectory());

        left.configureEncoder(Robot.driveTrain.left1.getSelectedSensorPosition(), 4000, 0.1524); // 0.1524
        right.configureEncoder(Robot.driveTrain.right1.getSelectedSensorPosition(), 4000, 0.1524); // 0.1524

        System.out.println(Robot.driveTrain.left1.getSelectedSensorPosition());
        System.out.println(Robot.driveTrain.right1.getSelectedSensorPosition());

        left.configurePIDVA(0.0, 0.0, 0.0, 1 / maxVel, 0);
        right.configurePIDVA(0.0, 0.0, 0.0, 1 / maxVel, 0);

        long endTime = System.currentTimeMillis();

        System.out.println("Created tank path in ms:");
        System.out.println((endTime - startTime));

        execCount = 0;
    }

    // Called repeatedly when this Command is scheduled to run
    @Override
    protected void execute() {
        leftPower = left.calculate(Robot.driveTrain.left1.getSelectedSensorPosition());
        rightPower = right.calculate(Robot.driveTrain.right1.getSelectedSensorPosition());

        // System.out.println(execCount++);
        // System.out.println(leftPower);

        // TODO: Account for yaw

        // System.out.println((System.nanoTime() - lastExecTime) / 1000000);
        // lastExecTime = System.nanoTime();

        Robot.driveTrain.setPower(leftPower, rightPower);
    }

    // Make this return true when this Command no longer needs to run execute()
    @Override
    protected boolean isFinished() {
        if (left.isFinished() || right.isFinished()) {
        System.out.println("Done with path.");
        System.out.println(Robot.driveTrain.left1.getSelectedSensorPosition());
        System.out.println(Robot.driveTrain.right1.getSelectedSensorPosition());
        return true;
        } else {
        return false;
        }
    }

    // Called once after isFinished returns true
    @Override
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    @Override
    protected void interrupted() {
    }
    }

The very first time that our command is run after restarting our code or rebooting the roborio (regardless of how the command is triggered, i.e. auto or from a button press) it only drives half of the distance (~0.5m), but takes the same amount of time to do it. After that, every time the command runs it does exactly what is expected (~1m forward).

The values leftPower and rightPower, generated in the execute() method and passed to drivetrain.setPower(), are identical whether the robot drives the full distance or half.

Our setPower method in the drivetrain is receiving the same values in both the full and half distance travels, but the robot moves different distances:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.can.TalonSRX;

import edu.wpi.first.wpilibj.command.Subsystem;
import frc.robot.commands.ArcadeDrive;

/**
 * Add your docs here.
 */
public class DriveTrain extends Subsystem {
  // Put methods for controlling this subsystem
  // here. Call these from Commands.
  public TalonSRX left1;
  public TalonSRX left2;
  public TalonSRX right1;
  public TalonSRX right2; 
 
  public DriveTrain() {
    left1 = new TalonSRX(1);
    left2 = new TalonSRX(2);
    right1 = new TalonSRX(3);
    right2 = new TalonSRX(4); 
    right1.setInverted(true);
    right2.setInverted(true);
  }

  private double deadBand(double power) {
    if(Math.abs(power) < .1) {
      power = 0;
    }
    return power;
  }

  public void setPower(double leftPower, double rightPower) {
    System.out.println(leftPower);
    
    leftPower = deadBand(leftPower);
    rightPower = deadBand(rightPower);
    left1.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, leftPower);
    left2.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, leftPower);
    right1.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, rightPower);
    right2.set(com.ctre.phoenix.motorcontrol.ControlMode.PercentOutput, rightPower);
  }

  @Override
  public void initDefaultCommand() {
    // Set the default command for a subsystem here.
    this.setDefaultCommand(new ArcadeDrive());
  }
}

The manual arcade drive command runs the exact same setPower method (in the drivetrain subsystem), but it always works as expected.

(Turning off the ArcadeDrive default command has no effect on the half distance bug.)

For reference, this is the documentation that details most of the code involved in this command: https://github.com/JacisNonsense/Pathfinder/wiki/Pathfinder-for-FRC---Java#using-the-library


#2

When you say that it drives half the distance, is that physical distance, or the encoders report it drove half the distance?

If you’re always generating the same path, it would be better to do that in the constructor instead of initialize so that you don’t spend the time generating the path each time you run it.


#3

That’s physical distance.

As for path generation: This is just a test. Eventually, the command will generate a unique path each time it is run. That’s what we’re building towards, so that’s how the code is written (even though it’s technically sub-optimal for now).