Add cosine power control to wheels

[danielbrownmsm]

Currently they always go at full commanded input, instead they should only output as much drive power as like the cosine of the angle error or something. not a super huge thing because we can make the wheels steer arbitrarily fast, but nice to have and we can reference it for the firmware for the actual robot later