Added motor code + modified arduino library to allow servos to work