1. void PID_test()           //使用于寻迹小车，
    2. //根据灰度传感器返回值，判断小车是否歪斜，进而调整速度使小车摆正，但是四个麦轮控制速度协调太难了
    3. //调整速度的正负号需要再仔细看一下
    4. {int PID_LEVEL_1=7,       //根据转弯幅度设置的三个值
    5.     PID_LEVEL_2=8,
    6.     PID_LEVEL_3=10;
    7. //定义了三个PID的模式
    9. //PID直线前进循迹
    10.   int count=0;            //防止第二次调用时速度过快
    11.   float error=0;
    12.   static float lasterror=0;
    15.   float kp=100;//决定每次调节幅度的大小,数值越大调节幅度越大            
    16.   float ki=0.0f;      
    17.   float kd=10; //可以决定调节速度的快慢，数值越大调节速度越快,当以比例为主的时候. 可以起到修正作用         
    18.   float derivative=0; //导数
    19.   static  float iout=0; 
    20.   float pid=0;
    22.   if(Condition!=-1)
    23.   {
    24.     Call();
    26.     switch(Condition)
    27.     {
    29.       //-----------------------------直行-------------------------------//
    30.       case 0:
    31.       //0 1 2 3 4 5//直行
    32.       int a;
    33.       a=(（Glevel[0]<<5）(GLevel[1] << 4) | (GLevel[2] << 3) | (GLevel[3] << 2) | (GLevel[4]<<1) | (GLevel[5]));
    34.       switch(a)                  //这里的灰度全反了，检测到黑线是1，白色是0
    35.       {
    36.        // case 0x35://左小偏 只有2能检测到 //110111
    37.            error=-PID_LEVEL_2;   
    38.            break; 
    39.         case 0x3B://右小偏 只有3能检测到 //111011
    40.            error=PID_LEVEL_2;   
    41.            break; 
    42.         case 0x33://直行不偏转 //110011
    43.            error=0; 
    44.            break; 
    45.         case 0x39://左中偏 3,4能够检测到 //111001
    46.            error=PID_LEVEL_2+1; 
    47.            break; 
    48.         case 0x27://右中偏 1 2能够检测到 //100111
    49.            error=-PID_LEVEL_2;                 
    50.            break; 
    51.         case 0x3C://左大偏 右侧灰度检测到 4，5能够检测到 //111100
    52.            error=PID_LEVEL_3+1;
    53.            break; 
    54.         case 0x0F://右大偏 0,1 能够检测到  //001111   
    55.            error=-PID_LEVEL_3+1;
    56.            break;  
    57.         default:
    58.         error=0;
    59.         break;
    60.         }//switch(a)
    62.         //------------------------------------后退-----------------------------//
    63.     case 1://后退
    64.     {
    65.       //灰度传感器损坏暂时不写
    66.       }
    68.       //--------------------------------------左横行---------------------------//
    69.     case 2://左横行
    70.     //12~16为左，17~21为右，左右两边小数在前，大数在后
    71.     //12,13,14,15,16
    72.     if(GLevel[14]==1)//当中间的传感器没有检测到黑线的时候
    73.   {
    74.   count=1000;
    75.   LEFT=100;
    76.   RIGHT=100;  
    77.   }
    78.   else if(count>0&&GLevel[14]==0)//当中间的两个传感器检测到黑线的时候,
    79.   {
    80.     count--;
    81.     delay_ms(1);
    82.     LEFT=pwm_left/8;//这里的LEft需要改---------------------------------------------------
    83.     RIGHT=pwm_right/8;//这里的RIGHT需要改--------------------------------------------------
    84.   }
    85.       int a;
    86.       a=((GLevel[12] << 3) | (GLevel[13] << 2) | (GLevel[15] << 1) | (GLevel[16]));         
    88.        switch(a)                     
    89.   { 
    90.    case 0x08:  //1000
    91.    error=PID_LEVEL_2;   
    92.    break; 
    94.     case 0x01:  //0001
    95.    error=-PID_LEVEL_2;   
    96.    break;
    98.   case 0x00:    // 0000 
    99.   error=0; 
    100.   break; 
    102.   case 0x06:    // 0110 
    103.   error=0; 
    104.   break; 
    106.   case 0x09:    // 1001 
    107.   error=0;
    108.   break; 
    110.   case 0x0D:    // 1101 
    111.   error=PID_LEVEL_1+1;
    112.   break; 
    114.   case 0x0C:    // 1100
    115.   error=PID_LEVEL_2+1; 
    116.   break; 
    118.   case 0x0E:    // 1110
    119.   error=PID_LEVEL_3+1;
    120.   break; 
    122.   case 0x0B:    //1011 
    123.   error=-PID_LEVEL_1;
    124.   break; 
    126.   case 0x03:    // 0011 
    127.   error=-PID_LEVEL_2;                 
    128.   break; 
    130.   case 0x07:    // 0111 
    131.   error=-PID_LEVEL_3;
    132.   break;
    134.    default:
    135.    error=0;
    136.    break; 
    137.   }
    138.         case 3://右横行
    139. {
    140.    if(GLevel[19]==1)//当中间的传感器没有检测到黑线的时候
    141.   {
    142.   count=1000;
    143.   LEFT=100;
    144.   RIGHT=100;  
    145.   }
    147.   if(count>0&&GLevel[19]==0)//当中间的两个传感器检测到黑线的时候,
    148.   {
    149.     count--;
    150.     delay_ms(1);
    151.     LEFT=pwm_left/8;//这里的LEft需要改---------------------------------------------------
    152.     RIGHT=pwm_right/8;//这里的RIGHT需要改--------------------------------------------------
    153.   }
    154.     //12~16为左，17~21为右，左右两边小数在前，大数在后
    155.     //17,18,19,20,21
    156.     int a;
    157.     a=((GLevel[17] << 3) | (GLevel[18] << 2) | (GLevel[19] << 1) | (GLevel[20]));         
    158.        switch(a)                     
    159.   { 
    160.    case 0x08:  //1000
    161.    error=PID_LEVEL_2;   
    162.    break; 
    164.     case 0x01:  //0001
    165.    error=-PID_LEVEL_2;   
    166.    break;
    168.   case 0x00:    // 0000 
    169.   error=0; 
    170.   break; 
    172.   case 0x06:    // 0110 
    173.   error=0; 
    174.   break; 
    176.   case 0x09:    // 1001 
    177.   error=0;
    178.   break; 
    180.   case 0x0D:    // 1101 
    181.   error=PID_LEVEL_1+1;
    182.   break; 
    184.   case 0x0C:    // 1100
    185.   error=PID_LEVEL_2+1; 
    186.   break; 
    188.   case 0x0E:    // 1110
    189.   error=PID_LEVEL_3+1;
    190.   break; 
    192.   case 0x0B:    //1011 
    193.   error=-PID_LEVEL_1;
    194.   break; 
    196.   case 0x03:    // 0011 
    197.   error=-PID_LEVEL_2;                 
    198.   break; 
    200.   case 0x07:    // 0111 
    201.   error=-PID_LEVEL_3;
    202.   break;
    204.    default:
    205.    error=0;
    206.    break; 
    207.   }
    208.   }
    211.      }//switch(Condition)
    213.     }
    214.   //float LEFT=pwm_left;
    215.   //float RIGHT=pwm_right;  
    216.   lasterror=error;
    219.   //示例：如果直行左中偏（车在黑线的左侧），error就是正值，左侧加正值，右侧减正值
    220.   iout += ki *error;
    221.   derivative=error-lasterror; 
    222.   pid=kp*error+kd*derivative+ki*iout;
    223.   pid=pid/10;
    225.  // Chassis_Control((LEFT+pid),(RIGHT-pid));    
    226. }