Implement a drive to point with odometry

[ErikCald]

Last year, we used a more basic point to point system to drive the robot with vision feedback. This simpler approach works well with vision than pathplanner and the simplicity makes it more reliable and consistent.

The system we used last year used 3 ProfiledPidControllers for X, Y and Rot and worked well. But it was copy pasted in multiple different commands. It would be good to instead include in the SwerveSubsystem with a driveToPoint(Pose2d pose) method. That way the code is in one location and improving there will improve it for all commands that might want to use it.

I recommend looking at the AlignToTargetVision.java command from last year, and copying over the nessecary driving code.

Only after this system is back up and tested to the same level it was last year, I have a suggested improvement to make. Last year it was relying solely on the PID to do everything. Instead, ask the ProfiledPidController for the desired velocity, and add that to the output of the calculate method of the ProfiledPidController, then pass that to the swerve subsystem. This adds a feedforward alongside the feedback which should be much more accurate and allow this system to work for a wider range of velocities and accelerations.

[ErikCald]

The intent of this issue is to help issue #13, but can also be used for many other cases (like possible doing final alignment if pathplanner is not accurate enough)

[ryanidk]

Completed in old repo under swerveSubsystem branch, move when swerve is merged to this repo