Gyro pid yaw

[Alexyglenn]

Add rotation while leveling using PID controller based on gyro yaw.

Also, do initial config for swervometer.