机器人车旋转特定角度的方法

本方法结和IMU和里程计底盘实现。imu采集的heading范围需要在0-360之间变化，如果是0-180，-180-0最好转换为0-360方便计算。

如下程序为仿真，flg的值决定了小车左右转弯的方向。

arg1 是初始heading，arg2是要旋转的角度建议范围取值-180----+180 ，正负号决定方向。

计算处旋转方向后停止。

gcc -o main  main.c 

main.c 
#include <time.h>
#include <sys/time.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <ctype.h>

#include <pthread.h>
#include <sys/types.h>
#include <sys/wait.h>
int main(int* arc,int*arg[])
{
    // Make sure there's only one rotate thread running at a time.
    // TODO: proper thread synchronization would be better here

    if(arc < 2){
      printf("usage:app startdegree rotatedegree \n");
      exit(1);
    }

    int startHeading = atoi(arg[1]);
    int degrees = atoi(arg[2]);

    int targetHeading = startHeading + degrees;
    if (targetHeading < 0){
    targetHeading += 360;
    if(targetHeading<180)
     degrees += 360;
    }
    if (targetHeading > 359){
       targetHeading -=360;

    }

    char flg =0;
    int heading = startHeading;
    if (degrees < 0)
    {
       flg =1;
    }
    else if (degrees > 0)
    {
       flg =0;
    }

    char  done = 0;
//    double turn = 0.0;
    do
    {
     if(flg){
         heading --;
         if(heading <= 0)
             heading =360;
     }
     else {
         heading ++;
         if(heading >= 360)
             heading = 0;
     }
    // Backup method - use the magnetometer to see what direction we're facing.  Stop turning when we reach the target heading.
    int currentHeading = (int)heading;//headingFilter.GetValue();
    printf("Rotating: currentHeading = %d   targetHeading = %d\n", currentHeading, targetHeading);
    if ((currentHeading >= targetHeading) && (degrees > 0) && (startHeading < targetHeading))
    {
        done = 1;
    }
    if ((currentHeading <= targetHeading) && (degrees < 0) && (startHeading > targetHeading))
    {
        done = 1;
    }
    if (currentHeading < startHeading && degrees > 0)
        startHeading = currentHeading;
    if (currentHeading > startHeading && degrees < 0)
        startHeading = currentHeading;
//#endif
    usleep(100000);
    }
    while (!done);

    return 0;
}

起初纠结 一个问题，如果车辆0度时，要想往左旋转330度，也就是往左旋转30度，是否可以选择最优的转动方向，考虑到实际环境中，如果车辆左侧恰好有障碍物阻碍转弯，这个时候继续往左转显然不明智。所以就没必要纠结了。