c语言怎么编程机器人,移动机器人（电子球）编程（c语言）

我正在编程我的差分驱动移动机器人(电子球)移动到特定方向的特定坐标。机器人到达坐标没有问题，但是当它到达坐标时，它不能定位在特定的方向上，并在现场保持“旋转”寻找方向，有没有人有过这方面的经验？我被困在这个问题很长时间，真的希望有人知道为什么。代码的相关部分粘贴在下面。移动机器人(电子球)编程(c语言)

static void step() {

if (wb_robot_step(TIME_STEP)==-1) {

wb_robot_cleanup();

exit(EXIT_SUCCESS);

}

}

.

.

.

.

.

static void set_speed(int l, int r)

{

speed[LEFT] = l;

speed[RIGHT] = r;

if (pspeed[LEFT] != speed[LEFT] || pspeed[RIGHT] != speed[RIGHT]) {

wb_differential_wheels_set_speed(speed[LEFT], speed[RIGHT]);

}

}

.

.

.

.

static void goto_position1(float x, float y, float theta)

{

if (VERBOSE > 0) printf("Going to (%f, %f, %f)\n",x,y,theta);

// Se