modified drive and servo code but everything responds to ros stuff