Basic controller implemented and working (tested). Solved stupid bugs on GaitTable implementation.