I am trying to formulate an equation to calculate left and right motor speeds of a robot. The robot is four wheeled drive with independent motors connected to tank-like best on each side. see this https://www.sparkfun.com/products/10336 So, the robot can be controlled using left and right speeds. Lets assume the left and right speeds, VL and VR are in range [-1, 1].
The joystick gives (x, y) position. Lets assume x and y are in range [-1, 1]. y value controls forward/backward movement and x value controls the turn.
Here is the desired behaviour (boundary conditions).(On left is the (x, y) joystick position, on right is the velocity (VL, VR))
Stop position
- (0, 0) --> (0, 0)
Go Forward
- (0, 1) --> (1, 1)
Turn right (clockwise)
- (1, 0) --> (1, -1)
Go Forward while turning right
- (1, 1) --> (1, 0)
Similar behaviour in other three quadrants as well (for moving back, turning left etc)
I want to have a smooth function of (x, y) which return (VL, VR) satisfying this conditions and have smooth transitions. (Hopefully it should closely resemble driving a car in a game)
How should I go about deriving such a function? Multivariate interpolation?
One idea is to use the similarity with polar coordinates in the transformation you want. The idea relies on the fact that you need to split the transformation in four cases, one for each of the quadrants of the joystick position. I will do the first quadrant ($x \ge 0, y \ge 0$), and then it's easy to apply the same concept to the other quadrants.
The first quadrant is the home of the coordinates $(0,0), (0,1), (1,0)$ and $(1,1)$, and they have the corresponding motor velocities $(0,0), (1,1), (1,-1)$ and $(1,0)$ respectively. Denote the joystick position $(x,y)$ and the motor velocities $(\alpha,\beta)$. See the below picture of the first quadrant, where the desired $(\alpha,\beta)$ velocities are written in the $(x,y)$ coordinate system.
Now, if the $(x,y)$ was actually a circular area instead of a square, $\alpha$ would be the radius of $(x,y)$ and $\beta$ would be related to the angle of the $(x,y)$ vector relative to the x-axis. Let's assume a circle for now and correct for it later. As noted, $\alpha$ corresponds to the radius:
$$\alpha = \sqrt{x^2+y^2}$$
We want that the angle $\varphi = 0$ should give $\beta = -1$ and $\varphi = \frac{\pi}2$ should result in $\beta = 1$. Hence, $$\beta = \sin\left(2\varphi - \frac{\pi}2\right)$$
But $\varphi = \arctan\frac{y}{x}$, so the expression simplifies to
$$\beta = \frac{y^2-x^2}{y^2+x^2}$$
Now we have a way to transform $(x,y)$ to $(\alpha,\beta)$, but we need to scale $\alpha$ to account for the fact that it is a square area that we are transforming, and not a circular one. For example, with the current formulas, $(1,1) \rightarrow (\sqrt2,0)$, where $\alpha$ needs to be scaled by $\frac1{\sqrt2}$.
Generally, the point $A$ in the below picture needs to be moved to $B$.
The scaling factor for $\alpha$ can then be expressed as follows
$$\begin{cases} s = \cos\varphi & 0 \le \varphi < \frac{\pi}4 \\ s = \cos\left(\frac{\pi}2-\varphi\right) & \frac{\pi}4 \le \varphi \le \frac{\pi}2 \\ \end{cases}$$
This simplifies to
$$\begin{cases} s = \frac{x}{\sqrt{x^2+y^2}} & 0 \le \varphi < \frac{\pi}4 \\ s = \frac{y}{\sqrt{x^2+y^2}} & \frac{\pi}4 \le \varphi \le \frac{\pi}2 \\ \end{cases}$$
Now, the final simplified transformation becomes:
$$\begin{cases} \alpha = x\;, & 0 \le \arctan\frac{y}{x} < \frac{\pi}4 \\ \alpha = y\;, & \frac{\pi}4 \le \arctan\frac{y}{x} \le \frac{\pi}2 \\ \end{cases}\\ \beta = \frac{y^2-x^2}{y^2+x^2}$$
I realize now that this was an awful lot of writing to get to very simple expressions for $\alpha$, but having written all of this, I'll let i stand.
Now, the same reasoning can be made in the other three quadrants, where it's just a matter of identifying which of the $\alpha$ or $\beta$ that should take the "radius" and "angle" part, and to decide on the sign.