Add active yaw angle control to swerve

[roterrex]

At this stage,

1. the robot drives relative to the field
2. there is no code controlling yaw angle.

As a result if you drive forwards and one wheel drags slightly, the robot will yaw that way while driving a dead straight path.

A PID or other controller is needed to hold the robot to its current angle.

suggestions,

1. human steering can be changed to control a target angle that the PID holds the robot to. This would provide a consistent angle as desired, but may reduce driver feel if the rate that angle changes outpaces the rate the robot is turning.
2. The PID can disable whenever control is applied, re-engaging at to hold the new current angle when released. This would be less noticable to a driver when everything works, but could lead to sudden jerks either direction if the controller disables while actively maintaining direction.
3. A controller may calculate a bias that would be added to steering at all times. When uncontrolled the bias would update, when given a steering command the bias would be locked. This may provide the best fealing when working correctly, but could result is a slight drift being created rather then removed (primarily when steering commands end and the bias is outdated). (Note: calculation would need to account for momentum maintaining the rotation)

A mix of 2. and 3. may provide the best results in time but initially starting with 2. may be easiest.