Pulling out some code from another branch - this PR modifies the ProfiledPointToPointCommand to use relative rotations for the theta as opposed to regular full rotational values. This should ensure we travel the shortest distance to get to a theta as opposed to going the full range to the theta. Ex: a starting theta of 0 and a target theta of 270 will currently wrap us all the way around the target CCW to get to the 270 theta. Now, the 270 theta is converted in to a -90, and we'll travel 90 CW to get to the location.
Pulling out some code from another branch - this PR modifies the
ProfiledPointToPointCommandto use relative rotations for the theta as opposed to regular full rotational values. This should ensure we travel the shortest distance to get to a theta as opposed to going the full range to the theta. Ex: a starting theta of 0 and a target theta of 270 will currently wrap us all the way around the target CCW to get to the 270 theta. Now, the 270 theta is converted in to a -90, and we'll travel 90 CW to get to the location.