[Jared Russell]

Answer for first part:

The code you have looks reasonable for a "standard" kiwi drive. Let's look at another way to derive the solution.

For each wheel, determine the angle of the vector formed by forward motion on that wheel with respect to:
* the vehicle's forward axis
* the vehicle's lateral axis
* the center of rotation of the vehicle (which is always 0 or 180 degrees for a 3 wheeled vehicle as long as their axes of rotation all intersect at a point as in your attached drawing)

Additionally, determine how far the wheel is from the center of rotation (e.g. the radius of the circle that a given wheel drives around as the vehicle turns).

Let's call the front left wheel W1, the front right W2, and the rear wheel W3. To drive forward in a straight line, take the speed you would like the vehicle to travel at, and for each wheel multiply by the cosine of the angle that wheel forms with the forward axis.

The angle of W1 w.r.t. the forward axis is something like 150 degrees in your drawing (30 degrees, but let's say forward moves W1 in reverse). (Plug in whatever the actual angle is here). W2 will be something like 30 degrees. W3 is -90 degrees.

To go 1 m/s forward (ignoring conversion between linear and radial velocities):
W1 = 1 m/s * cos(150 degrees) = -.866
W2 = 1 m/s * cos(30 degrees) = .866
W3 = 1 m/s * cos(-90 degrees) = 0

Do the same for strafing. The angle w.r.t. strafing to the left (keep in mind the right hand rule) is simply 90 degrees less than the angle w.r.t. the forward axis. So you can actually use the same numbers, but plug in sine instead.

To go 1 m/s to the left:
W1 = 1 m/s * sin(150 degrees) = .5
W2 = 1 m/s * sin(30 degrees) = .5
W3 = 1 m/s * sin(-90 degrees) = -1

For turning, we already know the angle for each wheel is 0 (or 180 if the motor is flipped). Multiply the desired turning velocity of the vehicle (let's use deg/sec here) by the radius to obtain the desired linear veloctity for each wheel. Let's say the radius of W1 and W2 is 0.5 meters, and for W3 it is 0.25 meters (again, use whatever your numbers actually are).

To turn 1 rad/s counter-clockwise
W1 = 1 rad/s * 0.5 meters
W2 = 1 rad/s * 0.5 meters
W3 = 1 rad/s * 0.25 meters

Now how do you combine the commands? What if you want to move at 1 m/s forwards, 1 m/s to the left, and 1 rad/s counter-clockwise simultaneously? This is where holonomic vehicles are really nice. Just add the three equations together!

W1 = (forward velocity) * cos(150 degrees) + (lateral velocity) * sin(150 degrees) + (radial velocity) * 0.5 meters
W2 = (forward velocity) * cos(30 degrees) + (lateral velocity) * sin(30 degrees) + (radial velocity) * 0.5 meters
W3 = (lateral velocity) * sin(-90 degrees) + (radial velocity) * 0.25 meters

The cos and sin components are actually now constants that you can pre-compute (or use the constexpr version of std::sin to compute, but don't worry about that). Just figure out what the right angles and radii for your robot are and go for it.

Note that things get more interesting when you add together all of the commands and come up with a velocity faster than your wheel can spin. If this happens, you need to scale down the velocity of all of the wheels proportionally to maintain your desired direction of travel.