Pathplanner path length

[loganbachman]

If I have an auto selected with the autoChooser in the RobotContainer, is there any way the code can return the time the path takes to complete? I know it’s possible to hard code this in but any way to get it directly from the auto?

[Fletch1373]

If you have a PathPlannerTrajectory object, you can call the getTotalTimeSeconds() method to get the time.

[loganbachman]

Trying to implement this, I have the autoChooser which is initialized to the AutoBuilder.buildAutoChooser(), How would I get the auto and set the PathPlannerTrajectory to be the auto selected?

[Fletch1373]

You would need a PathPlannerTrajectory object, not a Command object (which is what I believe buildAutoChooser() configures the chooser to return. Without seeing your code, we could only speculate on what changes you would need to make.

[loganbachman]

Here is the RobotContainter, I have the PathPlannerTrajectory object, I just don’t know how to get it to change to be the auto selected. Currently, for testing, it is just being sent to the dashboard.

package frc.robot;

import frc.robot.commands.Drives.Drive;
import frc.robot.commands.Drives.TurnToAngle;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import frc.robot.Constants.ModuleConstants;

import java.util.Optional;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.path.PathPlannerTrajectory;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.subsystems.*;


/**
 * This class is where the bulk of the robot should be declared.  Since Command-based is a
 * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
 * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
 * (including subsystems, commands, and button mappings) should be declared here.
 */
public class RobotContainer {
  private PathPlannerTrajectory trajectory;
  private static RobotContainer m_robotContainer = new RobotContainer();
  //#SUBSYSTEMS
  public final ShuffleboardSub m_shuffle = new ShuffleboardSub();
  public final NavX m_gyro = new NavX();
  public final DriveTrain m_driveTrain = new DriveTrain();

  // Joysticks
  private final XboxController driverController = new XboxController(0);
  private final XboxController manipController = new XboxController(1);    
  
  // A chooser for autonomous commands
  private SendableChooser<Command> autoChooser = new SendableChooser<>();
  
  /**
  * The container for the robot.  Contains subsystems, OI devices, and commands.
  */
  private RobotContainer() {

    SmartDashboard.putNumber("Trajectory Time: ", trajectory.getTotalTimeSeconds());
    
    // Build an auto chooser. This will use Commands.none() as the default option.
    autoChooser = AutoBuilder.buildAutoChooser();
    
    Shuffleboard.getTab("Autonomous").add(autoChooser);
    
    //SmartDashboard subsystems
    SmartDashboard.putData(m_shuffle);
    SmartDashboard.putData(m_driveTrain);

    // SmartDashboard Values
    SmartDashboard.putNumber("DrivingP", ModuleConstants.kDrivingP);
    SmartDashboard.putNumber("DrivingI", ModuleConstants.kDrivingI);
    SmartDashboard.putNumber("DrivingD", ModuleConstants.kDrivingD);
    
    SmartDashboard.putNumber("TurningP", ModuleConstants.kTurningP);
    SmartDashboard.putNumber("TurningI", ModuleConstants.kTurningI);
    SmartDashboard.putNumber("TurningD", ModuleConstants.kTurningD);

    SmartDashboard.putData("Drive", new Drive( m_driveTrain ));
    
    

    //REGISTERCOMMANDS
    //examples of using NamedCommands to run commands on event markers
    //How 3467 did this was making a function in the subsystem that returned a command
    NamedCommands.registerCommand("turnToZero", m_driveTrain.subTurnToAngle(0));

    // Configure the button bindings
    configureButtonBindings();
    
    //#DEFAULTS
    // Configure default commands
    m_driveTrain.setDefaultCommand(new Drive( m_driveTrain ));

    SmartDashboard.putData("Auto Mode", autoChooser);
  }

  public static RobotContainer getInstance() {
    return m_robotContainer;
  }

  /**
   * Use this method to define your button->command mappings.  Buttons can be created by
   * instantiating a {@link GenericHID} or one of its subclasses ({@link
   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {

    //This is how 190 is calling buttons which looks much cleaner
    //Initializes the controller like this
    // // Controller
    // private final CommandXboxController controller = new CommandXboxController(0);
    //Then calls commands like this
    // driverController.x().onTrue(m_driveTrain.subTurnToAngle(0));

    //#DRIVER
    //Turn to 90,180,270,0
    final JoystickButton xboxButtonA = new JoystickButton(driverController, XboxController.Button.kA.value);        
    xboxButtonA.whileTrue(new TurnToAngle(0, m_driveTrain ).withInterruptBehavior(InterruptionBehavior.kCancelSelf));

    final JoystickButton xboxButtonX = new JoystickButton(driverController, XboxController.Button.kX.value);        
    xboxButtonX.whileTrue(new TurnToAngle(270, m_driveTrain ).withInterruptBehavior(InterruptionBehavior.kCancelSelf));

    final JoystickButton xboxButtonY = new JoystickButton(driverController, XboxController.Button.kY.value);        
    xboxButtonY.whileTrue(new TurnToAngle(90, m_driveTrain ).withInterruptBehavior(InterruptionBehavior.kCancelSelf));

    final JoystickButton xboxButtonB = new JoystickButton(driverController, XboxController.Button.kB.value);        
    xboxButtonB.whileTrue(new TurnToAngle(180, m_driveTrain ).withInterruptBehavior(InterruptionBehavior.kCancelSelf));

    //#MANIP
    //manipulator controls example
    // final JoystickButton manipXboxButtonLBumper = new JoystickButton(manipController, XboxController.Button.kLeftBumper.value);        
    // manipXboxButtonLBumper.onTrue(new LEDYellow(m_lED ).withInterruptBehavior(InterruptionBehavior.kCancelSelf));
  }

public XboxController getdriverController() {
      return driverController;
}
public XboxController getmanipController() {
      return manipController;
}

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
  */
  public Command getAutonomousCommand() {
    return autoChooser.getSelected();
  }
}