Added diagrams and servo code