Awsome robot!
You mentioned you used DC motors and in your diagramm I can see you even have an angualr feedback of the motor shaft. Did you use this information in any way?
Also you have motors in the top and some wheels, so can it also drive on 4 wheels?