1. #define speeds 50      //宏定义speeds
    2. byte GLevel[22]={0};     //灰度储存
    3. short Condition = 0;     //定义前进时为Condition = 0;
    4. //---------------------------------------------------目前没有改变Condition的语句，调试时只能改变这里的值----------------------//
    5. //-1~4依次为停止-1，前进0，后退1，左2，右3，原地旋转4
    6. const short GrayPin[22] = {40, 41, 42, 43, 44, 45,46, 47, 48, 49, 50, 51, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39}; 
    7. //40-45为前接口，46-51为后接口，30-34为左接口，35-39为右接口。;
    9. //---------------------------------灰度部分------------------------------//
    10. void Gray_Init()   //灰度初始化
    11. { for (short i = 0; i <22; i++)
    12.   { pinMode(GrayPin[i], INPUT);
    13.   }
    14. }
    16. void GrayGet_value(byte value[])
    17. { int j = 0;
    18.   for (j = 0; j < 22; j++)
    19.     value[j] = digitalRead(GrayPin[j]);
    20. }
    23. //--------------------------------电机部分------------------------------//
    24. //23左前，45右前，67左后，89右后
    26. void _M_Init() {   //电机初始化
    27.   for (int i = 2; i <= 9; i++)
    28.   {
    29.     pinMode(i, OUTPUT);
    30.     analogWrite(i, 0);
    31.   }
    32. }
    34. void _M_Front(int pin1, int pin2, int _speed) {
    35.   analogWrite(pin1, _speed);
    36.   analogWrite(pin2, 0);
    37. }
    38. void _M_Back(int pin1, int pin2, int _speed) {
    39.   analogWrite(pin2, _speed);
    40.   analogWrite(pin1, 0);
    41. }
    42. /*void _M_Stop() {
    43.   for (int i = 2; i <= 9; i++)
    44.     analogWrite(i, 255);
    45.   Condition = -1;
    46. }*/
    47. void _M_Stop(int pin1,int pin2) {
    48.   analogWrite(pin1,255);
    49.   analogWrite(pin2,255);
    51. }
    53. //-----------------------------前进---------电机全部正转-----------------------//
    54. void _M_GoFront(int _speed) {   //直行时四个轮子全部正转
    55.   _M_Front(7, 6, _speed + 0);//左前
    56.   _M_Front(4, 5, _speed +0 );//右前
    57.   _M_Front(8, 9, _speed + 0);//左后 
    58.    _M_Front(2, 3, _speed + 0);//右后
    59. }
    60. //-----------------------------后退---------电机全部反转------------------------//
    61. void _M_GoBack(int _speed) {
    62.   _M_Back(7, 6, _speed + 0);
    63.   _M_Back(4, 5, _speed + 0);
    64.   _M_Back(8, 9, _speed + 0);
    65.   _M_Back(2, 3, _speed + 0);
    66. }
    67. //--------------------------------------左横行-------------------------------//
    68. void _M_GoLeft(int _speed){
    69.     _M_Back(7, 6, _speed +0 );
    70.     _M_Front(4, 5,_speed +0);
    71.     _M_Front(8, 9,_speed +0);
    72.     _M_Back(2, 3,_speed +0);
    73. }
    74. //--------------------------------------右横行-------------------------------//
    75. void _M_GoRight(int _speed){
    76.     _M_Front(7, 6,  _speed +0);
    77.     _M_Back(4, 5, _speed +0);
    78.     _M_Back(8, 9, _speed +0);
    79.     _M_Front(2, 3, _speed +0);
    80. }
    81. //--------------------------------------右前走------------------------------//
    82. void _MGo_youqian(int _speed){
    83.     _M_Front(7, 6,  _speed +0);
    84.     _M_Stop(4,5);
    85.     _M_Front(8, 9, _speed +0);
    86.     _M_Stop(2,3);
    87. }
    88. //---------------------------------------左前走-----------------------------//
    89. void _MGo_zuoqian(int _speed){
    90.     _M_Stop(7,6);
    91.     _M_Front(4, 5, _speed +0);
    92.     _M_Stop(8,9);
    93.     _M_Front(2, 3, _speed +0);
    94. }
    95. //---------------------------------------左后走----------------------------//
    96. void _MGo_zuohou(int _speed){
    97.    _M_Back(7, 6, _speed +0);
    98.     _M_Stop(4,5);
    99.     _M_Back(8, 9, 255);
    100.     _M_Stop(2,3);
    101. }
    102. //--------------------------------------右后走----------------------------//
    103. void _MGo_youhou(int _speed){
    104.     _M_Stop(7,6);
    105.     _M_Back(4, 5,_speed +0 );
    106.     _M_Back(8, 9,_speed +0);
    107.     _M_Stop(2,3);
    108. }
    109. void setup() {
    110.   Gray_Init();
    112. }
    114. void loop() 
    115. {
    116.   GrayGet_value(GLevel);  
    117.   //0 1 2 3 4 5 
    118.   switch(Condition)
    119. {
    120.   case 0:
    121.   //前进
    122.   {
    123.   unsigned a;
    124.   a=(GLevel[0]<< 5)|(GLevel[1] << 4) | (GLevel[2] << 3) | (GLevel[3] << 2) | (GLevel[4]<<1) | (GLevel[5]);
    126.   switch( a ){
    127.   case 0x0C://直行不偏转 //001100
    128.   _M_GoFront(speeds);      
    129.    break;
    130.  //--------------------------------前---小偏--------------//
    131.   case 0x08://右小偏 //001000
    132.   _MGo_zuoqian(speeds) ;
    133.   break;
    134.   case 0x04://左小偏 //000100
    135.    _MGo_youqian(speeds);
    136.   break;
    138.  //------------------------------前-----中偏---------------// 
    139.   case 0x18://右中偏 //011000
    140.   _MGo_zuoqian(speeds) ;
    141.   break;
    142.   case 0x06://左中偏 //000110
    143.    _MGo_youqian(speeds);
    144.   break;
    146.   //----------------------------前-------大偏---------------//
    147.   case 0x30://右大偏 //110000
    148.   _MGo_zuoqian(speeds) ;
    149.   break;
    150.   case 0x03://左大偏 //000011
    151.    _MGo_youqian(speeds);
    152.   break;
    153.   default:
    154.   _M_GoFront(speeds);      
    155.    break;
    156.   }
    157.   }
    158.  case 2:
    159.  //车左横行时
    160.  {
    161.   unsigned b=0;
    162.   b=(GLevel[16]<< 4)|(GLevel[15] << 3) | (GLevel[14] << 2) | (GLevel[13] << 1) | (GLevel[12]);
    163.   switch(b){
    164.     case 0x04://直行//00100
    165.     _M_GoLeft(speeds);
    166.     break;
    168.     //-------------------------------左---小偏--------------//
    169.     case 0x06://小左后偏//调用左前走函数//00110
    170.     _MGo_zuoqian(speeds+0);
    171.     break;
    172.     case 0x0C://小左前偏//调用左后走函数01100
    173.     _MGo_zuohou(speeds+0);
    174.     break;
    176.     //------------------------------左---中偏----------------//
    177.     case 0x02://中左后偏//调用左前走函数00010
    178.     _MGo_zuoqian(speeds-1);
    179.     break;
    180.     case 0x08://中左前偏//调用左后走函数01000
    181.     _MGo_zuohou(speeds-1);
    182.     break;
    184.     //------------------------------左---大偏----------------//
    185.     case 0x03://大左后偏//调用左前走函数00011
    186.     _MGo_zuoqian(speeds-2);
    187.     break;
    188.     case 0x18://大左前偏//调用左后走函数11000 
    189.     _MGo_zuohou(speeds-2);
    190.     break;
    191.     default:
    192.     _M_GoLeft(speeds);
    193.     break;}
    194.       }
    195.   case 3:
    196.  //车右横行时
    197.  {
    198.   unsigned c=0;
    199.   c=(GLevel[17]<< 4)|(GLevel[18] << 3) | (GLevel[19] << 2) | (GLevel[20] << 1) | (GLevel[21]) ;
    200.   switch(c){
    201.     case 0x04://直行//00100
    202.     _M_GoRight(speeds);
    203.     break;
    205.     //-------------------------------左---小偏--------------//
    206.     case 0x06://小右前偏//00110
    207.     _MGo_youhou(speeds+0);
    208.     break;
    209.     case 0x0C://小右后偏01100
    210.     _MGo_youqian(speeds+0);
    211.     break;
    213.     //------------------------------左---中偏----------------//
    214.     case 0x02://中右前偏00010
    215.     _MGo_youhou(speeds-1);
    216.     break;
    217.     case 0x08://中右后偏01000
    218.     _MGo_youqian(speeds-1);
    219.     break;
    221.     //------------------------------左---大偏----------------//
    222.     case 0x03://大右前偏00011
    223.     _MGo_youhou(speeds-2);
    224.     break;
    225.     case 0x18://大右后偏11000 
    226.     _MGo_youqian(speeds-2);
    227.     break;
    228.     default:
    229.     _M_GoRight(speeds);
    230.     break;}
    231.     }
    232.   }
    233.   delay(2000); 
    234. }

    在最后delay(2000)的原因：
    Mega2560性能不够，时间太短无法调整轮子的转动方向和速度；
    void loop(){
    switch（）{
    case 0：
    ···
    case 1：
    ···
    ···
    }
    delay（2000）；
    }
    //原理：等待2000ms再进行下一次判断~