车模速度控制:

速度控制子程序是每隔一段时间（100ms）调用一次，程序并没有直接更新 g_nLeftMotorSpeedOut ，g_nRightMotorSpeedOut的数值，而是通过调用函数更新上面的数值CalculateMotorSpeedOut()，而此函数是每5ms被调用一次，所以程序是将速度控制的变化量平均到20次进行更新。这样可以降低速度控制对于车模直立控制的影响。

该函数的输入为：g_nLeftMotorSpeedCount ，g_nRightMotorSpeedCount 。

计算得到：g_nLeftMotorSpeedOut ，g_nRightMotorSpeedOut 。

 1 void MotorSpeedAdjustCal(void) {
 2     int nLeftSpeed, nRightSpeed;
 3     int nDeltaValue, nP, nI;
 4     int nSpeed;
 5  
 6     nLeftSpeed = (int)g_nLeftMotorSpeedCount;        //左电机脉冲计数
 7     nRightSpeed = (int)g_nRightMotorSpeedCount;      //右电机脉冲计数
 8     nSpeed = (nLeftSpeed + nRightSpeed) / 2;         //平均脉冲
 9  
10     nDeltaValue = g_nMotorSpeedSet - nSpeed;        //设置值 减平均值
11     nP = mult_r(nDeltaValue, MOTOR_SPEED_P_INT);    // P
12     nI = mult_r(nDeltaValue, MOTOR_SPEED_I_INT);    // I
13  
14     g_nMotorOutSpeedOld = g_nMotorOutSpeedNew; 
15  
16     g_nMotorOutSpeedKeep -= nI;                    //减积分
17     g_nMotorOutSpeedNew = (g_nMotorOutSpeedKeep >> 3) - nP; //缩8 减比例（除8，主要是放置积分系数为小数）
18     if(g_nMotorOutSpeedKeep > MOTOR_OUT_MAX) 
19               g_nMotorOutSpeedKeep = MOTOR_OUT_MAX;
20     if(g_nMotorOutSpeedKeep < MOTOR_OUT_MIN) 
21               g_nMotorOutSpeedKeep = MOTOR_OUT_MIN;
22   
23 }
24 void CalculateMotorOutSpeed(void) {
25     int nValue;
26     nValue = g_nMotorOutSpeedNew - g_nMotorOutSpeedOld;
27     nValue = nValue * (g_nCarMotionCount + 1) / 
28              (CAR_MOTION_PERIOD - 1) + g_nMotorOutSpeedOld; //平均20次
29     g_nLeftMotorOutSpeed = g_nRightMotorOutSpeed = nValue;
30 }

车模直立控制

注意：由于现在还没有加入速度闭环，所以由于加速度传感器零偏的误差，会导致车模在直立的时候会往一个方向加速行驶。

void CarAngleAdjust(void) {
    int nLeft, nRight;
    int nSpeed;
    int nP, nD;
    nP = g_nCarAngle;
    nP = (int)mult_r(nP, CAR_AA_P_INT);
    nD = g_nCarGyroVal >> 5;
    nD = (int)mult_r(nD, CAR_AA_D_INT);
    nSpeed = nD + nP;                  (-1000～1000)
    if(nSpeed > MOTOR_SPEED_SET_MAX)        nSpeed = MOTOR_SPEED_SET_MAX;
    else if(nSpeed < MOTOR_SPEED_SET_MIN)   nSpeed = MOTOR_SPEED_SET_MIN;
 
    nLeft = nSpeed + g_nLeftMotorOutSpeed - g_nMotorLeftRightDiff;
    nRight = nSpeed + g_nRightMotorOutSpeed + g_nMotorLeftRightDiff;
    g_nLeftMotorOut = nLeft << 6;
    g_nRightMotorOut = nRight << 6;
    if(g_nLeftMotorOut > MOTOR_OUT_MAX)
 
g_nLeftMotorOut = MOTOR_OUT_MAX;
    if(g_nLeftMotorOut < MOTOR_OUT_MIN) g_nLeftMotorOut = MOTOR_OUT_MIN;
    if(g_nRightMotorOut > MOTOR_OUT_MAX) g_nRightMotorOut = MOTOR_OUT_MAX;
    if(g_nRightMotorOut < MOTOR_OUT_MIN) g_nRightMotorOut = MOTOR_OUT_MIN;
    MotorSpeedOut();
}

if((L-R)>2)

{

temp_R=temp_R+1;

}else if((R-L)>2)

{

temp_L=temp_L+1;

}//判断两轮子脉冲的差异，进行校正；