Added servo_cntrl core. Not finished yet. The counter in the generate
block can be used once. And the testbench has to be updated and the core
needs more verification