Move to Pose

[Portnord]

🚀 Summary:

Add capability to command movement to a specific field location.

📜 Tasks:

• [x] Create swerve drive command to move to a specific pose, requires odometry and gyro.
• [x] Add button mappings to engage automated motion (e.g. shooting position, pickup location, amplifier)
• [x] Add robot.py logic to select destination pose and command drivebase.

[Portnord]

Correction to make: The field constants aren't correctly computing shooting distances. All distances in inches need to be added together before multiplying by the conversion to meters.

[Portnord]

Correction: The x and y inputs in the driveteam file should not be inverted for just red alliance inputs. They should invert only for red alliance input when we are in field oriented mode. Should probably be done in robot or swerve drive instead.

[Portnord]

Correction: Should force field oriented TPV when commanding move to pose

[Portnord]

Correction: Move to pose for pickups crashes, Rotation2D constructors need parentheses.

[Portnord]

Still testing, accidentally closed.