智能小车pid算法

//矢量合成，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;
Temp_PID=Temp_PID*1/5;
//-1000~+1000 是左右满舵的输出，因此需要除以
0.5
/*单片机浮点运算非常慢，所以用乘 2 除 5 两次定点
运算来替代定点数要先乘后除，才能保证精度，同时要防止溢出，用起来比较麻烦，但 CPU
和内存开销小。*/
return (Temp_PID);
//开启串口 0 收发模块
U0TCTL |= SSEL0;
//选择 ACLK 作为串口波特率时钟源
U0BR1 = 0;
//
U0BR0 = 13;
//分频系数整数部分=13
U0MCTL = 0X6B;
//分频系数小数部分调制=5/8.（2400bps）
P3SEL |= BIT4 + BIT5;
U0CTL &=~ SWRST;
光的强弱，小车都能稳定地、以大概相同的速度寻光进入车库。
这个问题要是深究下去还有很多值得研究的地方：比如同时有若干个强光
源，小车现在的算法是朝着这些光源合成的几何中心进行，如果要实现朝着光最
强的那个光源行进该采用什么样的算法。
避障算法采用了检测发射红外 LED，读一体化接收头的数据判断障碍物的
位置。这里采用 38Khz 敏感的接收头。为了使小车的检测距离适中，还需调整
{flag_2 = 1;BarrierData[2]=5-i;} else BarrierData[2]=0; PIRE3_L; // 关闭 3 号传感器 } // 4 if(flag_3==0) { PIRE4_H ; // 打开 4 号传感器 Delay(34); // 延迟，等待数据稳定
if(IRE4_IN==0) // 如果接收到的信号为低电平，表明右前方有障碍物 {flag_3 = 1;BarrierData[3]=5-i;} else BarrierData[3]=0; PIRE4_L; // 关闭 4 号传感器 } // 5 if(flag_4==0) { Delay(34); PIRE5_H ; // 打开 5 号传感器 Delay(34); // 延迟，等待数据稳定
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;
//计算比例分量(P)=比例系数*本次位置差
Temp_D=D_coefficient*(Position-Last_Position[5]); //计算微分分量(D)=微分系数
*(本次位置差-前 3 次的位置差)
//注意由于采样比较快，用本次位置-前 3 次位置
才有足够大的控制量
Last_Position[6]=Last_Position[5];
//启动串口
IE1 |= URXIE0;
}
从串口读一个字节数据 char UART0_GetChar(unsigned char *Chr) {
if(UART_InpLen == 0) return (0); _DINT(); UART_InpLen--; *Chr = RX_BUFF[RX_IndexR]; if (++RX_IndexR >= RXBUF_SIZE) { RX_IndexR = 0; } _EINT(); return (1);
影响时设计的一个难点，引导小车的光源时 200W 的灯泡，在实际测试时，由于
白天和晚上环境光的不同，小车的实际 AD 采样值也有差异，会造成小车运行的
不稳定。
不过即使环境光再强烈，在题目要求的场地里也不及灯泡的亮度，该问题
解决方法是先大概感知周围环境的光强，再根据预设的值调整光强系数，自适应
调整电机转速，这样就能保证只要是 200W 的灯泡作为引导光源，无论周围环境
冲过黑线，黑线又偏右。然后控制过程反复，车身是在左右摇摆中向前行走的。
这种摇摆叫做“超调”，超调越大，控制越不稳定，容易出轨。
为了克服惯性，我们除了位置信息之外，还需要知道轨迹的变化趋势。我们
可以用黑线位置的微分值来提前得到变化趋势。用本次位置减去前次位置求出差
值，就大致知道偏移量的变化趋势。将该差值和比例相加后一起作为控制量，即
} }
3.2 寻光，避障算法
题目要求小车到达 C 点之后，在光源引导下避开障碍物进入车库，这就要 求小车同时完成寻光和避障的功能。如果只进行寻光，小车会撞上障碍物，如果 只进行避障，小车也许会离光源越来越远。理论上避障的优先级是要高于寻光的， 因为一旦接触上障碍便宣告失败。所以一旦检测到障碍物，小车会立刻执行避障 动作，一旦传感器环路没有检测到障碍小车就向光源靠拢，这样能够保证小车在 成功避障的条件下逐渐逼近光源，直到走出障碍区之后就能直奔光源而去。这种 算法小车执行寻光避障整体采用了状态机的切换，
if(IRE5_IN==0) // 如果接收到的信号为低电平，表明右前方有障碍物 {flag_4 = 1;BarrierData[4]=5-i;} else BarrierData[4]=0; PIRE5_L; // 关闭 5 号传感器 }
if(flag_0||flag_1||flag_2||flag_3||flag_4) {flag=1;} else flag=0;
}
3.3 串口通信
由于小车采用双核结构，所以就涉及到了两个单片机的通信问题。在整个进
行的过程中，由于两个单片机分工明确，需要通信的地方大概有如下几个地方：
1. 启动时，149 给 425 发送信号，开始计时；
2. 检测到金属时，425 发给 149 计数；
3. 检测到 C 点停车；
4. 车身入库后 149 发给 425 停车，停止计时。
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;
Tห้องสมุดไป่ตู้mp_P=P_coefficient*Position;
由此可见，通信量并不是很大，而且通信距离不过十几个厘米，所以采用两
个单片机的串口直接相连的方式。全部收发均放在中断中执行，以下是串口
通信的部分程序：
149 串口初始化
UART_Init149()
{
U0CTL |= CHAR;
//异步通讯模式，8 位数据，无校验，1 位停止位
ME1 |= UTXE0 + URXE0;
//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) // 如果接收到的信号为低电平，表明右前方有障碍物
3.1 寻迹算法
采用 PID(PD)控制算法，如果某时刻检测到黑线偏左，就要向左转弯；如果
检测到黑线偏右，就要向右转。偏得越多，就要向黑线方向打越大的转角。这就
是比例控制（P）。
遗憾的是，因为小车有惯性。假设黑线偏左，说明小车偏右了，需要左传舵，
等到小车回到中心的时候，停止转舵，可是小车的惯性会使车身继续左转，直到
通过红外 LED 的电流为 5mA 左右。
采用了 TA0 来发生红外线，不干扰 CPU 的运行：
void TimerAInit()
{ //设置数组，对应频率分别为 38,41,44,48,54,60,67
TACTL |= TASSEL_2 + TACLR + MC_1 ; //TIMER_A 时钟源选为 SMCLK,清 TAR
/*保存前 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 作为左右满舵
可实现提前控制。这就叫做比例微分控制（PD 控制）