Kinematics for the NAOH25 and many many code fixes.
Now code is in boolean!

Signed-off-by: eldrad <nikofinas@gmail.com>