worked on servo code, this thing is giving me headaches. anyway implementing the new command system partialy