beginning of new algorithm
compiles, still lots of error
todo: when skeleton get crazy, offset signs
feierabend
ready to test
updated rate into 50hz
Merge branch 'new_algorithm'