Fixes and making demo run two threads one for every servo.