Draft of driver with servo code