Add Vector Scaling

[ssejrog]

Expected Behavior

The drive PID should act "normally" regardless of the speed the robot is going

Actual Behavior

When driving with PID at low speeds, the heading PID is too powerful and the robot oscillates.

Steps to Reproduce

void autonomous() {
  chassis.pid_targets_reset(); // Resets PID targets to 0
  chassis.drive_imu_reset(); // Reset gyro position to 0
  chassis.drive_sensor_reset(); // Reset drive sensors to 0
  chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold.  This helps autonomous consistency.

  // This is fine
  chassis.pid_drive_set(24_in, 110); 
  chassis.pid_wait();

  // This wiggles
  chassis.pid_drive_set(24_in, 30);
  chassis.pid_wait();
}