I am looking for a way to implement some sort of controller that takes the field-relative pose of the robot into account to a) compensate for the robot being off the path and b) ensure that it ends up in the right spot facing the right way. I was looking at Pure Pursuit, but from reading into it the algorithm does not take into account headings. RAMSETE, however, does take into account the current and desired headings.
How is the RAMSETE controller supposed to work? Say I had the robot at the state (x,y,theta, linear velocity, angular velocity) of (0,0,0,0,0) and gave it a goal state of (1,5,0,2,0). What does the robot look like if it follows the controller properly and gets to (1,5)? Facing the same direction as the initial state and moving at 2 units/s forward?
Are there other strategies in the realm of pure pursuit, RAMSETE, etc. that could also be applied? It seems RAMSETE is all the rage right now. This is for a personal project of mine, so I am not looking for FRC-specific code, but more of a general understanding of the techniques to implement myself. Any help would be appreciated.