Added code for two more servos

There are four servos and two motors now.