I need two custom axis, one for wheel speed [0, 1] and one for stering block angle [-1, 1] which equals [-180°; 180°]
I use input from
Left Y (+forwards/-backwards)
Left X (-left/+right)
Right X (-turn left/+ turn right)
α (Direction the wheel needs to face in order for the machine to turn right. This variable is different for each wheel. It is the direction for the Right X vector)
Code:
α for each wheel
North wheel 1/2 pi
NW wheel 1/4 pi NE wheel 3/4 pi
West wheel: 0 East wheel pi
SW wheel: -1/4 pi SE wheel: -3/4 pi
South wheel: -1/2 pi
Here's the maths:
First I convert polar vector {Right X, α} into a carthesian vector {Xr, Yr}.
Code:
Xr=Right X*cos α
Yr=Right X*sin α
Then I can add that vector to the left stick's vector {Left X, Left Y} to make the resulting direction vector {X, Y}.
For the wheel speed {Xs, Ys}, I multiply the two vectors with 0.7 and 0.5 respectively before adding them so they are weighted torwards speed and Xs and Ys stay in the range of [0,1]
Code:
Xs=0.7*Left X+0.3*Xr
Ys=0.7*Left Y+0.3*Yr
Now I convert into polar coordinates again. For the steering block angle I use atan2. To comply with the [-1,1] range I divide by pi.
Code:
steering block angle = atan2(X, Y)/pi
For the wheel speed I use pythagoras.
Code:
wheel speed = sqrt(Xs²+Ys²)
So basically, the two formulas are
Code:
wheel speed = sqrt((0.7*Left X+0.3*Right X*cos α)²+(0.7*Left Y+0.3*Right X*sin α)²)
steering block angle = atan2(Left X+Right X*cos α, Left Y+Right X*sin α)/pi
with α being specific to each wheel
here's what I got:
init code
Code:
from math import *
LeftX = AdvancedControls.GetAxis("Left X")
LeftY = AdvancedControls.GetAxis("Left Y")
RightX = AdvancedControls.GetAxis("Right X")
update code for the wheels
Code:
sqrt(
pow((
0.7*LeftX.OutputValue+0.3*RightX.OutputValue*cos(alpha)
),2)
+
pow((
0.7*LeftY.OutputValue+0.3*RightX.OutputValue*sin(alpha)
),2)
)
update code for the steering blocks
Code:
atan2(
LeftX.OutputValue+RightX.OutputValue*cos(alpha),
LeftY.OutputValue+RightX.OutputValue*sin(alpha)
)/pi
Edit: I didn't get the sin and cos functions to work. So in the code above I replaced them with the numeric values.
Code:
Wheel cos(α) sin(α)
N 1 0
NE 0.707 -0.707
E 0 -1
SE -0.707 -0.707
S -1 0
SW -0.707 0.707
W 0 1
NW 0.707 0.707