瑞萨智能车软件报告.docx
《瑞萨智能车软件报告.docx》由会员分享，可在线阅读，更多相关《瑞萨智能车软件报告.docx（46页珍藏版）》请在冰豆网上搜索。
瑞萨智能车软件报告
华中科技大学
2010瑞萨超级MCU改装四驱车
测试程序
一．总体规划
1．程序流程
根据比赛规定和模型车的特征，简要程序流程如下：
2.简要说明
本方案基于组委会公布的文档，旨在通过在组委会文档上做出一定的修改以加快模型车的速度，提高稳定性。
因此在组委会公布的方案上加上一块驱动板把模型车改为四驱的来提高动力，在组委会公布的方案上加上编码器以提高调速的准确性，稳定性，快速性。
3.资源分配
电机的控制周期上最好一致，经过测试电机周期为1ms是比较合适的，但是
在组委会已经确定的方案中，由于硬件的限制，接口上提供的是ITU3来确定后面两个驱动轮和舵机的周期。
所以本方案采取的是用ITU0提供1ms的计时中断，用ITU3的同步复位PWM方式，采取16ms的周期提供后轮电机的驱动和舵机控制。
由于和两个前轮和编码器的加入。
定时端口不够用了，所以我们采取的是用ITU0复用和ITU1一起提供周期为1ms的PWM来驱动前面两个轮子，采取ITU2来进行编码器计数。
引脚1
编码器的电源VCC
引脚10
编码器的地GND
引脚8
编码器的的信号输入端
引脚4
接电机驱动板的7脚，输出左前轮的方向
引脚5
接电机驱动板的8脚，输出左前轮的PWM
引脚6
接电机驱动板的6脚，输出右前轮的方向
引脚7
接电机驱动板的5脚，输出右前轮的PWM
扩展的驱动板B的接口如上图所示。
二．加入部分的程序说明
1.前轮PWM设置
采取的是用ITU0复用和ITU1一起提供周期为1ms的PWM来驱动前面两个轮子，基本设置如下。
定时器周期设置：
#defineTIMER_CYCLE3071/*Timercycle:
1ms*/
/*Whenusingcy/8,*/
/*cy/8=325.5[ns]*/
/*ThusTIMER_CYCLE=*/
/*1[ms]/325.5[ns]*/
/*=3072*/
定时器初始化：
/*ITU0andITU1PWMmodesforfrontleftandrightmotors*/
ITU_MDR=0x03;//ITU0,ITU1PWMMode
ITU0_TCR=0x43;//TCNT0isClearedbyGRBCompareMatch,φ/8
ITU1_TCR=0x43;//TCNT1isClearedbyGRBCompareMatch,φ/8
ITU0_IER=0x01;//interputenable
ITU0_GRB=TIMER_CYCLE;//ITU0duty
ITU1_GRB=TIMER_CYCLE;//ITU1duty
ITU1_GRA=0;
ITU0_GRA=0;
/*ITU3andITU4resetsynchronizedPWMmodesforleftandrightmotorsandservo*/
ITU3_TCR=0x23;
ITU_FCR=0x3e;
ITU3_GRA=PWM_CYCLE;/*Frequencysetting*/
ITU3_GRB=ITU3_BRB=0;/*LeftmotorPWMsetting*/
ITU4_GRA=ITU4_BRA=0;/*RightmotorPWMsetting*/
ITU4_GRB=ITU4_BRB=SERVO_CENTER;/*ServoPWMsetting*/
ITU_TOER=0x38;
速度设置：
/************************************************************************/
/*Speedcontrol*/
/*Arguments:
leftmotor:
-100to100,rightmotor:
-100to100*/
/*0:
stopped,100:
forward100%,-100:
reverse100%*/
/************************************************************************/
voidspeed（intaccele_gl,intaccele_gr,intaccele_fl,intaccele_fr）
{
/*SetPWM*/
unsignedlongspeed_max1;
unsignedlongspeed_max2;
speed_max1=（PWM_CYCLE-1）;//GearPWMperiod
speed_max2=（TIMER_CYCLE-1）;//FrontPWMperiod
/*GearLeftmotor*/
if（accele_gl>=0）{
PBDR|=0x04;
ITU3_BRB=speed_max1*accele_gl/100;
}else{
PBDR&=0xfb;
accele_gl=-accele_gl;
ITU3_BRB=speed_max1*accele_gl/100;
}
/*GearRightmotor*/
if（accele_gr>=0）{
PBDR&=0xf7;
ITU4_BRA=speed_max1*accele_gr/100;
}else{
PBDR|=0x08;
accele_gr=-accele_gr;
ITU4_BRA=speed_max1*accele_gr/100;
}
/*Frontleftmotor*/
if（accele_fl>=0）{
PADR&=0xdf;
ITU1_GRA=TIMER_CYCLE-1-speed_max2*accele_fl/100;
}else{
PADR|=0x20;
accele_fl=-accele_fl;
ITU1_GRA=TIMER_CYCLE-1-speed_max2*accele_fl/100;
}
/*FrontRightmotor*/
if（accele_fr>=0）{
PADR&=0xf7;
ITU0_GRA=TIMER_CYCLE-1-speed_max2*accele_fr/100;
}else{
PADR|=0x08;
accele_fr=-accele_fr;
ITU0_GRA=TIMER_CYCLE-1-speed_max2*accele_fr/100;
}
}
2.编码器设置
测速测距初始化设置
/*ITU2Encoderpulseinput*/
ITU2_TCR=0x0c;//EncoderPulseinputfromTCLKA
/*======================================*/
/*Symboldefinition*/
/*======================================*/
#defineENCODER_GIRTH110
#defineERR_GAIN（ENCODER_GIRTH/（（SPEED_SAMPLE_TIME+1）*2））
此处的ERR_GAIN乘以当前时间内的脉冲计数就可以算成单位是cm/s的速度
具体推算略
/*Encoder*/
unsignedcharucSpeedTimer=0;//Speedsampletimecounter
unsignedintuEncoderBuff=0;//Pulsecounterbuffer
unsignedlonglEncoderLine=0;//Crankturndistancerecord
unsignedlonggl_lEncoderTotal=0;//Totalencoderpulses（distance）
unsignedintgl_uEncoder=0;//Currentpulse（speed）
intgl_curSpeed=0;
intgl_motorPWM=0;
/************************************************************************/
/*FunctionsetCarSpeed*/
/*UsageSpeedloop*/
/*Argumentspeed:
setproperspeed*/
/*ReturnValueNo*/
/************************************************************************/
intsetCarSpeed（intsetSpeed）
{
staticintspeedErr[3]={0,0,0};
intspeedErrD=0;
staticintmotorPWM=0;
intspeed_P=0;
intspeed_D=0;
intspeed_I=0;
speedErr[2]=speedErr[1];
speedErr[1]=speedErr[0];
speedErr[0]=setSpeed-gl_curSpeed;
speedErrD=speedErr[0]-speedErr[1];
speed_P=2;
speed_I=5;
motorPWM=motorPWM+（int）（speedErr[0]/speed_I+speedErrD/speed_P）;
if（motorPWM>100）{
motorPWM=100;
}
if（motorPWM<0）{
motorPWM=0;
}
returnmotorPWM;
}
所有涉及到PID控制程序的P，I，D相关参数与车的具体性能参数有关，必须自己进行相关的调试来使车到达一个快速，准确，稳定的状态。
这里主要是介绍一种思想，理解思想后进行相关的参数设定才能到达好的效果。
下面涉及到具体的参数调节都是根据特定车模调试的，不再做说明。
3.调速过程采取增量式PI调速控制。
下面对增量PI进行简单的说明，考虑到K-1次时采样时有
第K次采样时有
两次相减存在有
如果只采取PI控制，则有
=0，故该公式可以简化成
可以看到与上面的PI调速程序是一样的，在这个过程中比例调节器对于偏差e变化是即时反应的，偏差一旦产生，调节器立即产生控制作用使被控量朝着偏差减小的方向变化，控制作用的强弱取决于比例系数Kp的大小。
积分环节只要偏差e不为零，它将通过累计作用影响控制量u，并减小偏差，直至偏差为零，控制作用不在变化，使系统达到稳态。
由上面的公式可以看出比例系数和积分系数的大小影响到调速的过程，具体数值要根据自己的小车行进调节。
4．差速表的引入
另外由于是四轮驱动，由于四个轮子在实际运动过程当中各自的PWM值是不一样的，可以通过加入差速表来控制四个轮子的PWM。
constchargl_cSpeedDifference[3][60]={
{100,98,97,95,93,92,90,88,87,85,83,82,80,78,77,75,73,72,70,69,67,65,64,62,60,59,57,56,54,52,
51,49,48,46,45,43,41,40,38,37,35,34,32,31,29,28,26,25,23,22,21,19,18,16,15,13,12,11,9,8},
{100,98,97,95,94,92,91,89,88,86,85,83,82,81,79,78,77,76,74,73,72,71,70,69,68,67,66,65,64,63,
62,61,60,59,58,57,56,56,55,54,53,53,52,51,50,50,49,49,48,47,47,46,46,45,45,44,44,43,43,42},
{100,100,100,100,100,100,99,99,99,99,99,98,98,98,97,97,97,96,96,95,95,94,94,93,93,92,92,91,90,90,
89,89,88,87,86,86,85,84,84,83,82,81,81,80,79,78,77,76,76,75,74,73,72,71,70,69,68,67,67,66}};
这样就可以通过差速表来调节速度，差速表的输出函数如下：
/************************************************************************/
/*FunctionSpeedOutput*/
/*UsageOutputspeed*/
/*ArgumentNo*/
/*ReturnValueNo*/
/************************************************************************/
voidspeedOutput（intsetSpeed,intservoAngle）
{
charcInsideRDiff=0;//Speeddifferentialproportionofinsiderearwheel
charcInsideFDiff=0;//Speeddifferentialproportionofinsidefrontwheel
charcOutsideRDiff=0;//Speeddifferentialproportionofoutsiderearwheel
intmotorPWM=0;
motorPWM=setCarSpeed（setSpeed）;
if（servoAngle>=0）
{
cInsideRDiff=gl_cSpeedDifference[0][servoAngle];
cInsideFDiff=gl_cSpeedDifference[1][servoAngle];
cOutsideRDiff=gl_cSpeedDifference[2][servoAngle];
if（motorPWM>0）
{
speed（motorPWM*cInsideRDiff/100,motorPWM*cOutsideRDiff/100,motorPWM,motorPWM*cInsideFDiff/100）;
}
else
{
speed（motorPWM,motorPWM,motorPWM,motorPWM）;
}
}
else
{
cInsideRDiff=gl_cSpeedDifference[0][-servoAngle];
cInsideFDiff=gl_cSpeedDifference[1][-servoAngle];
cOutsideRDiff=gl_cSpeedDifference[2][-servoAngle];
if（motorPWM>0）
{
speed（motorPWM*cOutsideRDiff/100,motorPWM*cInsideRDiff/100,motorPWM*cInsideFDiff/100,motorPWM）;
}
else
{
speed（motorPWM,motorPWM,motorPWM,motorPWM）;
}
}
gl_motorPWM=motorPWM;
}
5.关于传感器（舵机）去抖
如果采取的是Kit所给出的程序，在小车寻迹的过程当中，一般会发现传感器不停的抖动，分析原因如下：
比如当小车本来只有中间一个传感器可以检测到白线，突然有一个左转弯，此时由于转弯不大，有中间和中间靠左的传感器检测到白线，于是舵机向左转向，舵机转向完成后又只有中间一个传感器检测到白线，此时舵机应该恢复中间的位置，当舵机恢复中间位置后由于此时仍然是左转弯，于是有中间和中间靠左的传感器检测到白线，于是舵机向左转向，舵机转向完成后又只有中间一个传感器检测到白线，此时舵机应该恢复中间的位置……….
上面的过程就是一个抖动的过程，舵机不停的打角，肯定会影响速度的。
因此提供下面一种方式减小抖动,将数字传感器赋权值转化成模拟信号进行处理。
intgl_sensorErr=0;
intgl_angle=0;
//对传感器的每一位赋权值
intgl_sensorDistance[7]={65,45,25,0,-25,-45,-65};
/************************************************************************/
/*GetSensorValue*/
/*Returnvalues:
7bitsSensorValue*/
/************************************************************************/
intgetSensorErr（unsignedcharsensorValue）
{
unsignedchari=0;
unsignedcharsensor=0;
intsensorErr=0;
intsensorCnt=0;
staticintsensorErrLast=0;
sensor=sensorValue;
for（i=0;i<7;i++）
{
if（sensor&（1<{
sensorErr=sensorErr+gl_sensorDistance[i];
sensorCnt++;
}
}
if（sensorCnt==0）
{
sensorErr=sensorErrLast;
}
else
{
sensorErr=sensorErr/sensorCnt;
}
if（（（sensorErr-sensorErrLast）>40）||（（sensorErr-sensorErrLast）<-40））
{
sensorErr=sensorErrLast;
}
sensorErrLast=sensorErr;
returnsensorErr;
}
上面的程序处理过程还有一个滤波的过程，这个滤波方式是防止速度过快后由于传感器打角不及时而检测两边的白线影响偏差的计算。
滤波算法需要在以后的调试中继续完善，这里只是提供了一种比较简单的方式进行滤波。
//通过传感器的差值给角度
voidgetCarAngle（void）
{
staticintsensorErrLast=0;
staticintsensorErrNow=0;
staticintangle=0;
intsensorErrD=0;
sensorErrLast=sensorErrNow;
sensorErrNow=gl_sensorErr;
sensorErrD=sensorErrNow-sensorErrLast;
angle=angle+（sensorErrNow/25）+sensorErrD/5;
if（angle>30）
{
angle=30;
}
elseif（angle<-30）
{
angle=-30;
}
gl_angle=angle;
}
这里可以说明增量式PI控制的思想，基于偏差变化的补偿，外加一定积分作用来消除静差。
周期控制是为了满足PID控制的基本条件。
如果在两次计算sensorErr的时候传感器状态发生了变化，那么舵机需要打一个角度，初步估算，假设刚第一次是中间的加上右边一个传感器检测到白线（即从左到右的第四位和第五位），突然变成中间偏右的一个和第二个（从左到右的第三位和第四位）检测到白线，此时gl_sensorErr=35,sensorErrD=25,舵机将要右转6度，但是在下一次测传感器信号时，舵机的转向还没有完成，此时相对于刚才的角度，由于时间很短传感器信号没有变化，此时gl_sensorErr=35,sensorErrD=0，还需要在右转1读。
所以，必须加上一个时间周期，等舵机转向完成，如果加入时间周期舵机转向完成，此时再测传感器信号，此时gl_sensorErr=0,sensorErrD=0。
舵机不需要转向了。
这种算法其实可以看做值一种设定输入为加权值0的增量式的PI控制，假设输入加权值为0，其中P参数是1/5，I参数是1/25。
6.关于标志位误检
在智能车检测标志位的过程当中我们发现经常会把直角弯误检成换道标志位，基于组委会给出的检测，我们在检测到到右换道标志位以后不是直接跳到51的右换道标志位，而是考虑到50状态对换道标志位进行确认，如果在100mm的距离累检测到最左边的传感器检测到白线我们认为这种情况不是换道而是直接弯标志位。
并且在这个过程中也过滤掉了第二条确认的标志检测为。
case50:
升级会员 