-Corrected slope calculation bug (negative index);
-Fixed acessing buffer values before being filled;
-Tested controlled walking pattern (straight line, wide and tight
turns).