cleaning up some and adding code to calcuate speed
speed from encoder works. Whoooooohooooo
Merge branch 'master' of https://github.com/trevhoot/tetris_arm