# Omnidirectional control

## Balancing the robot

For any 3 wheeled robot with wheels mounted at $\theta_a,\theta_b,\theta_c$ with velocities $v_1,v_2,v_3$ then

where $v_x$ and $v_y$ are the x and y components of the robot’s velocity (relative to the robot), $R$ is the radius of the circle made by the wheels and $\omega$ is the robot’s angular velocity. This is implemented in my omnidrive library which is used by the PiBot and also the ballbot. By treating the problem of balancing the ballbot as a 2D inverted pendulum (like two segways), I can pass the roll and pitch of the robot into a PID control system that outputs the required velocity of the robot in each axes ($v_x$ and $v_y$).

Currently I’ve succeeded in getting the robot to balance on carpet but not on tiles. When I run it on tiles, the slightest error in pitch/roll causes it to keep accelerating horizontally and eventually fall over. Why? Because the robot is thinking that it’s pitch/roll is at 0 when it actually isn’t. Interestingly, I just happened to read about this phenomenon on Wired this morning.

## Getting the location of the robot

As the robot seemed to be drifting away from it’s origin, I decided that trying to get it to stay at the origin would help it from accelerating away and falling over. But then came the first obstacle challenge: getting it to know where it was.

Intuitively,

This brings back the linear system we saw above, except this time our inputs are the wheel velocities, $v_1,v_2,v_3$, calculated from u\sing quadrature encoder data, and our outputs are $v_x$ and $v_y$.

If we let

then

After some algebra, we get

and

But we’re not done yet! Remember that the velocities we have are actually relative to the robot. In order to get the correct components of velocity relative to the floor, we must know the angle of the robot. $\alpha = \int_{0}^{T}\omega .dt$

Then we must rotate the velocity vector $\begin{pmatrix}v_x\\ v_y\end{pmatrix}$ by $-\alpha$ to obtain the real velocity vector $\begin{pmatrix}v_x'\\ v_y'\end{pmatrix}$ which is relative to the floor.

After that, we simply integrate velocity (numerically) to obtain the position of the robot. All this has been translated into code here.

So there we go! Year 11 maths C (and first year uni linear algebra) in use!