We are trying to do a Centering/tuning command using pathfindtoPose and we have a problem where the robot gets to the point but didnt rotate quick enough, is there any way to solve this problem? we would also like to use a trapezoid profile on the command. Is there any way to do that using pathfindtoPose()?
I would not recommend using pathFindToPose() alone for your auto alignment command. pathFindToPose() only gets you roughly to the desired position. If you want your auto-alignment command to get around obstacles, I recommend having a command sequence with two steps:
- The robot drives and approaches the target roughly using
pathFindToPose(). - The robot performs a “precise alignment” using a Holonomic Drive Controller with a small amount of tolerance to ensure accuracy. This is how we did it: DriveToPoseCommand.java