Added switch code and secondary servo codes