3.1寻迹算法
采用PID(PD)控制算法，如果某时刻检测到黑线偏左，就要向左转弯；如果检测到黑线偏右，就要向右转。偏得越多，就要向黑线方向打越大的转角。这就是比例控制（P）。
遗憾的是，因为小车有惯性。假设黑线偏左，说明小车偏右了，需要左传舵，等到小车回到中心的时候，停止转舵，可是小车的惯性会使车身继续左转，直到冲过黑线，黑线又偏右。然后控制过程反复，车身是在左右摇摆中向前行走的。这种摇摆叫做“超调”，超调越大，控制越不稳定，容易出轨。
为了克服惯性，我们除了位置信息之外，还需要知道轨迹的变化趋势。我们可以用黑线位置的微分值来提前得到变化趋势。用本次位置减去前次位置求出差值，就大致知道偏移量的变化趋势。将该差值和比例相加后一起作为控制量，即可实现提前控制。这就叫做比例微分控制（PD控制）
/*PID(PD)控制算法*/
int PID_Control(signed char Position)
{
  int Temp_P,Temp_D,Temp_PID,Temp_I,k;  //声明三个变量，用于存放P、I、D三分量的运算结果(I没用上)
if(Position==-128) return (No_black);            //错误处理(值得改进的地方)
else
{
    Temp_I=Position;
    for(k=0;k<5;k++)Temp_I+=Last_Position[k];
    Temp_I*=I_coefficient;
    Temp_P=P_coefficient*Position;      //计算比例分量(P)=比例系数*本次位置差
    Temp_D=D_coefficient*(Position-Last_Position[5]);  //计算微分分量(D)=微分系数*(本次位
置差-前3次的位置差)
                                //注意由于采样比较快，用本次位置-前3次位置才有足够大的控制量
    Last_Position[6]=Last_Position[5];
    Last_Position[4]=Last_Position[3]; 
    Last_Position[3]=Last_Position[2]; 
    Last_Position[2]=Last_Position[1]; 
    Last_Position[1]=Last_Position[0];     
Last_Position[0]=Position;        /*保存前5次的位置，以备用。
    Temp_PID=Temp_P+Temp_D+Temp_I;    //P分量和D分量相加，得到控制量。
    if(Temp_PID>5000) Temp_PID=5000;    //防止控制量溢出
    if(Temp_PID<-5000) Temp_PID=-5000;  //控制量-5000~5000作为左右满舵
    Temp_PID=Temp_PID*1/5;      //-1000~+1000是左右满舵的输出，因此需要除以0.5
                              /*单片机浮点运算非常慢，所以用乘2除5两次定点运算来替代定点数要先乘后除，才能保证精度，同时要防止溢出，用起来比较麻烦，但CPU和内存开销小。*/
    return (Temp_PID);
}
}
3.2 寻光，避障算法
题目要求小车到达C点之后，在光源引导下避开障碍物进入车库，这就要求小车同时完成寻光和避障的功能。如果只进行寻光，小车会撞上障碍物，如果只进行避障，小车也许会离光源越来越远。理论上避障的优先级是要高于寻光的，因为一旦接触上障碍便宣告失败。所以一旦检测到障碍物，小车会立刻执行避障动作，一旦传感器环路没有检测到障碍小车就向光源靠拢，这样能够保证小车在成功避障的条件下逐渐逼近光源，直到走出障碍区之后就能直奔光源而去。这种算法小车执行寻光避障整体采用了状态机的切换，
void FSM()
{  switch (Status)
  {
case  Light_Status :  Find_Light(); break;    //没有障碍物就进入寻光模式
  case  Barrier_Status : Avoid_Obstacle(); break;  //检测到障碍进入避障状态
  default:  break;
  }
}
具体的寻光采用了149内部的ADC12模块，通过AD读出的光敏电阻的值进行处理。根据传感器环路的结构，以小车前后方向作为Y轴，左右方向作为X轴，五个光敏电阻分别位于0度，45度，90度，135度，180度的位置。先对采回的各个光强进行归一化的处理，然后根据矢
量合成的原则，计算出小车应该行进的方向，其中X为两个轮的速度差，Y为共同速度。代码如下：
//矢量合成，X,Y两个轴，
  x=0;
  y=0;
  //1号光敏电阻，无Y轴，X为负
  x=x-Intensity*Light[0];
  //2号光敏电阻，X为-0.707，Y为0.707,45度
  x=x-(Intensity *Light[1]*707)/1000;
  y=y+(Intensity *Light[1]*707)/1000;
  //3号光敏电阻，X为0，Y为正
  y=y+Intensity * Light[2];
  //4号光敏电阻，X,Y都为+0.707，45度
  x=x+(Intensity*Light[3]*707)/1000;
  y=y+(Intensity*Light[3]*707)/1000;
  //5号光敏电阻，Y为0，X为正
  x=x+Intensity*Light[4];
  y=y;
由于小车采用左右两轮的差动控制，在小车测试时，，若X和Y的差距太小，会导致趋光性并不明显，若Y的值太小又会使小车前进速度很慢。所以要仔细调制X和Y参数归一到占空比CCRx的系数。或许采用舵机控制后轮或者利用汽车结构的前轮转向小车系统能够更高效的解决寻光灵敏度的问题。
在寻光小车设计中，还要考虑实际环境对小车的影响。如何滤除环境光的影响时设计的一个难点，引导小车的光源时200W的灯泡，在实际测试时，由于白天和晚上环境光的不同，小车的实际AD采样值也有差异，会造成小车运行的不稳定。
不过即使环境光再强烈，在题目要求的场地里也不及灯泡的亮度，该问题解决方法是先大概感知周围环境的光强，再根据预设的值调整光强系数，自适应调整电机转速，这样就能保证只要是200W的灯泡作为引导光源，无论周围环境光的强弱，小车都能稳定地、以大概相同的速度寻光进入车库。
这个问题要是深究下去还有很多值得研究的地方：比如同时有若干个强光源，小车现在的算法是朝着这些光源合成的几何中心进行，如果要实现朝着光最强的那个光源行进该采用什么样的算法。
避障算法采用了检测发射红外LED，读一体化接收头的数据判断障碍物的位置。这里采用38Khz敏感的接收头。为了使小车的检测距离适中，还需调整通过红外LED的电流为5mA左右。
采用了TA0来发生红外线，不干扰CPU的运行：
void TimerAInit()
{  //设置数组，对应频率分别为38,41,44,48,54,60,67
  TACTL |= TASSEL_2 + TACLR + MC_1 ;  //TIMER_A时钟源选为SMCLK,清TAR
  TACCR0 = 44;          //产生约38KHZ的PWM输出,SMCLK=4M,105个时钟周期
  TACCTL0 |= OUTMOD_4;  //翻转模式产生占空比为50％的PWM
}
 
避障底层程序：
void Measure_Distance()
{
  unsigned char flag_1=0,flag_2=0,flag_3=0,flag_4=0,flag_0=0;
  unsigned char i,Frequency[5] = {29,36,44,47,51}; //66,54,44,42,38
  for (i=0;i<5;i++)
  {  TACCR0 = Frequency[i];
  // 1
        if(flag_0==0)
        {
          Delay(34);
    PIRE1_H;  // 打开1号传感器
    Delay(34);    // 延迟，等待数据稳定
汽车检测线
        if(IRE1_IN==0) // 如果接收到的信号为低电平，表明前方有障碍物
          {flag_0 = 1;BarrierData[0]=5-i;}
        else BarrierData[0]=0;
        PIRE1_L;
        }
//2
        if(flag_1==0)
        {
          Delay(34);
    PIRE2_H;  // 打开2号传感器
    Delay(34);    // 延迟，等待数据稳定
        if(IRE1_IN==0) // 如果接收到的信号为低电平，表明前方有障碍物
          {flag_1 = 1;BarrierData[1]=5-i;}
        else BarrierData[1]=0;
        PIRE2_L;
        }
        // 3
    if(flag_2==0)
        {
        PIRE3_H ;  // 打开3号传感器   
    Delay(34); // 延迟，等待数据稳定
if(IRE3_IN==0) // 如果接收到的信号为低电平，表明右前方有障碍物
          {flag_2 = 1;BarrierData[2]=5-i;}
        else BarrierData[2]=0;
        PIRE3_L;  // 关闭3号传感器
        }
        // 4
        if(flag_3==0)