Fixed seg fault. Now tunning the controller. It's a wild ride :D

<3 Patrick for his indexing mistake info