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

上一篇文章介绍了旋转的特定角度，想尝试在没有任何障碍物的广场中旋转角度的最优方向，用到概率仿真的算法。

得到最优旋转方向。

#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>

#define MIN(a,b) ((a)<(b))?(a):(b)
int main(int* arc,int*arg[])
{

    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 > 359){
       targetHeading -=360;

    }

    char flg =0;
    int heading = startHeading;
    char done =0;
     int stepl =0,stepr=0;
    do
    {

         heading --;
         if(heading < 0)
             heading =359;
          stepl ++;
    // 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))
    {
        done = 1;
    }

    }
    while (!done);
    printf("turn left step :%d \n",stepl );
    heading = startHeading;
    done =0;
  do {
         heading ++;
         if(heading > 360)
             heading = 0;
         stepr ++;
    // 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))
    {
        done = 1;
    }

    }
    while (!done);
    printf("turn right step :%d \n",stepr);
    printf("the best turn way:%d \n",MIN(stepl,stepr));
    return 0;
}

@lid-VirtualBox:~/share/car/rotate$ ./main 300 330

turn left step :30 

turn right step :331 

the best turn way:30