A phase compensation algorithm to solve modes switching problem for bioinspired undulations of robotic fish models