added mapping function to get correct input to hump function regardless of direction. Changed Hump dynamixel examble to have more controls for testing