Here is a
swerve calculator (Excel spreadsheet) you can use to test your code.
It shows (both graphically and numerically) what each wheel angle and speed should be for any given vehicle-desired-motion vector [FWD,STR,RCW].
Just enter the vehicle forward/reverse command, strafe right/left command, and rotate clockwise/counterclockwise command into cells A6, A7, and A8, respectively.