I think you could benefit greatly from
Ether's swerve kinematics and programming documents. Look for the
excel spreadsheet. All you need to give is a Joystick Y(up/down), X(left/right strafe), Z (Rotation) and Gyro Angle and it visualizes the angle and speed of 4 swerve wheels.
There is also a
paper on how to write these algorithms into the programming language of your choice.
Hope this helps.