Hello, I have been working on a command code for shooting for a while and I came across the problem of moving the bot to a specific Pose2D for aiming or aligning. My plan is to write a function “MoveToPose2D(Pose2D _Pos)” but I have little idea how it should be in practice. Any suggestion is welcome!
Given you mention Pose2d, I’m assuming you’re referring to moving the robot as a whole to a specific point on the field. If so, path following is likely what you’re looking for. Here’s a page in the WPILib docs that should help get you started: Trajectory Tutorial Overview — FIRST Robotics Competition documentation
Thanks, I’ll check it out!
Also look at popular trajectory software packages, such as PathPlanner. They would simplify the process for you. For instance, PathPlanner can make a robot drive automatically from Pose2d-1 to Pose2d-2. You would need to have a routine to determine your current pose, though, if you need to be at a specific place on a field. That can be accomplished with cameras and apriltags.
This requires PathPlanner’s auto builder and a constraints object set up and it’s in C++ which is probably not what you want. But this probably around what you are looking for: AutoBuilder::pathfindToPose(target_pose, DEFAULT_CONSTRAINTS, 1_mps, 0.0_m), where target_pose is of type frc::Pose2d