Add cosine power control to wheels

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