求问大神~~~为何我们的程序不能使舵机转动......

奖奖奖奖~~~

程序如下：
#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#include <math.h>
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
///////////////////////常量定义////////////////////
#define row_num 30                  //图像采集行数
#define col_num 184                 //图像采集列数 196
#define steer_center 2900           //舵机中心值
#define car_center 92               //车模中心值
#define steer_left 2500             //舵机左拐最大值
#define steer_right 3300            //舵机右拐最大值
#define motor_left 30               //马达速度最小值
#define motor_right 200             //马达速度最大值
#define yuzhi 40                    //赛道黑白差值  

#pragma DATA_SEG DEFAULT
//------------------状态定义------------------------
///////////////////////数码管/////////////////////
unsigned char LedCode[]={0xc0,0xf9,0xa4,0xb0,0x99,0x92,0x82,0xf8,0x80,0x90};
unsigned char LedData[]={0,0,0,0};
unsigned char LedNum=0;
///////////////////摄像头图像采集/////////////////
uchar image[row_num][col_num];      //图像存储数组
uchar image_v_num=0;                //已采集到的摄像头的当前行
uchar v_counter=0;                  //已读取的行数
uchar ccd_state=0;
unsigned int c=0;
unsigned int All_VSYN_Count=0;     //摄像头采集的场数
//--------------摄像头要采集的行----------------------------
const int selectRow[]=
{
50,   55,   60,   65,   70,   75,   80,   85,   90,   95,   
100,  105,  110,  115,  120,  125,  130,  135,  140,  145,
150,  155,  160,  165,  170,  175,  180,  185,  190,  195,
//200,  204,  208,  212,  216,  220,  224,  228,  232,  236,
};
//////////////////////黑线提取////////////////////
uchar center_line[row_num];         //中点坐标 ,等于0表示没有找到中心
uchar Lx[row_num];                    //左引导线中心点列号,没有找到的值为col_num
uchar Rx[row_num];                    //右引导线中心点列号,没有找到的值为0
uchar BW_DELTA=40;                //黑白线的差值
uchar isLeftEdge=0,isRightEdge=0; //是否看到左右边界
uchar L_LeftEdge=col_num,L_RightEdge=0; //是否看到左右边界         
uchar Distance[row_num]=              //黑线距离中心的距离
{
80,78,76,74,72,70,67,65,63,60,
58,55,53,50,47,45,42,39,35,33,
};  
//////////////////////舵机控制////////////////////
int steer_out=steer_center;         //舵机控制输出量
int pre_steer_out=0;                //前一次的舵机控制量
float steer_p_near=2.5;                 //近处舵机P值
float steer_p_mid=5;                  //中间舵机P值
float steer_p_far=3;                  //远处舵机P值
int near_piancha;                   //近处黑线与车模中心偏差
int mid_piancha;                    //中间黑线与车模中心偏差
int far_piancha;                    //远处黑线与车模中心偏差
int allpiancha;
float SteerKd=2;
int SteerE0=0;
int SteerE1=0;
uint Count_Num=0;
int motor_out=0;                    //马达控制输出量
#pragma CODE_SEG DEFAULT
///////////////////////初始化/////////////////////
void initGPIO(void){                //通用IO口初始化
  DDRA=0x00;                        //A口输入  用于摄像头的数据采集接摄像头数据线
}
void initPLL80M(void)//--------------锁相环初始化，将总线频率调整到80M -------------------
{                 
  CLKSEL=0X00;         //禁止锁相环，时钟有外部晶振提供，总线频率=外部晶振/2
  PLLCTL_CME=1;     //允许时钟监控     
  PLLCTL_SCME=0;    //外部晶振失效进入自给时钟     
  CRGINT_SCMIE=1;     //允许时钟状态改变时中断
  PLLCTL_PLLON=1;      //打开锁相环
  SYNR=0xc9;
  REFDV=0x81;
  POSTDIV = 0x00;
  _asm(nop);                //BUS CLOCK=40M   (f_bus=f_pll/2)
  _asm(nop);
  while(!(CRGFLG_LOCK==1)); // 等待锁相环初始化完成
  CLKSEL_PLLSEL =1;         // 使用锁相环
}
void PP_init(void)  //摄像头中断初始化    PP4为场中断  PP5为行中断
{
  PIEP_PIEP4 = 1;                     
    PIEP_PIEP5 = 1;       //...............              
    PPSP=0xFF;       //上升沿触发中断
}

void initPWM(void){                 //PWM初始化
  DDRP_DDRP0 = 1;
  DDRP_DDRP2 = 1;
  PWME=0x00;                          //关闭所有PWM通道  每位对应一个端口
  PWMPOL = 0xFF;                      //PWM极性选择，选择一个周期开始时为高电平
  PWMCTL=0xF0;                        //01级联，23级联，45级联，67级联
  PWMPRCLK = 0x11;                    //CLOCK B,A时钟频率  20M  20M
  PWMCLK = 0xFF;                      //选择CLOCK SA SB为PWM时钟
  PWMSCLA=5;                          //Clock SA = Clock A / (2 * PWMSCLA),从CLOCK A 10分频，2M
  PWMSCLB=5;                          //CLOCK SB从CLOCK B 10分频 ,20M/10=2M;
  
  PWMCAE=0;                           //边沿对齐
  
  PWMPER01=200;                       //电机0 PWM频率=2M/200=10KHz,200为计数值，计数完了进入下一个PWM，计数频率为2M
  PWMDTY01=0;                         //电机0的占空比=PWMDTY01/PWMPER01
     
  PWMPER23=200;                       //电机1 PWM频率=2M/200=10KHz
  PWMDTY23=100;                         //电机1的占空比=PWMDTY23/PWMPER23
  
  PWMPER67=40000;                     //舵机PWM频率为2M/40000=50Hz
  PWMDTY67=steer_center;                      //舵机占空比
  
  PWME_PWME1=1;   
  PWME_PWME3=1;                       //电机PWM波开始输出
  PWME_PWME7=1;                       //舵机PWM波开始输出

}
void InitPACNT(void){
  PACTL = 0x40;                     //脉冲累加功能，下降沿计数
  PACNT = 0;      
}
//------Leftline()和Rightline()函数-----提取线但是不处理（可以滤除内部的一个黑点的噪点）
//给出了最简单的找线情况，其他情况可自己发挥                                                   5
void Leftline()
{
  uchar lp1=car_center,lp2=car_center+6;     
  uchar LLeftEdge=col_num;
  uchar CurL=0;                            //指向当前的处理的行
  uchar LastL=0;                           //上一行
  signed char  Blackcnt=0;               //记录出现几个黑点
  for(;CurL<=row_num;CurL++)
  {
      Blackcnt=0;
    if(CurL>0)
    {
      LastL=CurL-1;                                            //Lx[]=col_num表示没有找到左黑线
      while(Lx[LastL]==col_num&&LastL>0) LastL--;            //LastL中记录第一个找到的有效左黑线中心所在行数
      if(Lx[LastL]!=col_num)
      {
            lp1=Lx[LastL]-30;                                //lp1指向上一个可循到 黑线中心列号-15列
          lp2=Lx[LastL]-24;                                //lp2指向上一个可循到 黑线中心列号-13列
        }
        else
        {
            lp1=car_center;
            lp2=car_center+6;
        }
    }
    if(lp2<col_num)                    //提取左黑线边界
    {
        while(lp2<col_num&&!isLeftEdge)                                        //当lp2没有到达列的最大值继续扫描，右扫描模式
        {
            if(fabs((int)(image[CurL][lp1]-image[CurL][lp2]))>BW_DELTA)            //
            {
                while(fabs((int)(image[CurL][lp1]-image[CurL][lp2]))>BW_DELTA&&lp2<col_num)
                {
                  Blackcnt++;
                  lp1++;
                  lp2++;
                  if(lp2>=col_num)break;
                }
                if(Blackcnt>=2)            //找到左边界退出循环，lp1和lp2间隔设为1，只有有连续三点的时候才会识别为黑线   
                {
                  isLeftEdge=1;
                  LLeftEdge=lp1;
                  //Lx[CurL]=lp1;               //记录左线的内边界
                  break;
                }
                else                    //遇到噪点，计数清零
                {
                    Blackcnt=0;
                    Lx[CurL]=col_num;
                }
            }
            else
            {
                lp1++;
                lp2++;
            }
        }
        if(isLeftEdge)
          Lx[CurL]=LLeftEdge;
      else
        Lx[CurL]=col_num;
      isLeftEdge=0;
    }//每一行的扫描结束，继续下一行或退出循环
  }
}//Leftline()函数结束
                                
void Rightline()//右线须自己完成
{
}//Rightline()函数结束

void CenterLine()//寻找道路中点
{
     
    int CurL;
    for(CurL=0;CurL<row_num;CurL++)
    {
        if(Lx[CurL]!=col_num && Rx[CurL]!=0)//当左右引导线都找到时候
        {
            center_line[CurL]=(Lx[CurL]+Rx[CurL])>>1;
        }
        //其他情况自行处理
    }
}
                             
void steer_PD(void)
{               //舵机PD控制
  steer_p_near=3;                   //对P值赋初值
  steer_p_mid=7; //之前是5;
  steer_p_far=3;//近中远三处的黑线偏差值来控制舵机，属于点控制
  near_piancha= car_center-(int)center_line[0] ;   //第0行中心值有错误，暂时用第1行  
  mid_piancha= car_center-(int)center_line[row_num>>2] ;
  far_piancha= car_center-(int)center_line[row_num-1] ;
  allpiancha=steer_p_near*near_piancha+steer_p_mid*mid_piancha+steer_p_far*far_piancha;
  SteerE0=allpiancha;
  //求近中远三处黑线与车模中心的差值
  steer_out=steer_center+allpiancha+SteerKd*(SteerE1-SteerE0);
  SteerE1=SteerE0;  
  if(steer_out>steer_right)steer_out=steer_right;     //舵机输出限幅，否则会烧坏舵机
  if(steer_out<steer_left) steer_out=steer_left;                                 
  pre_steer_out=steer_out;          //记住此次控制量
  PWMDTY67= steer_out;
}

void speed_PID(void){               //速度采用匀速控制，适当修改
  motor_out=40;                    
  if(motor_out>motor_right)motor_out=motor_right;
  if(motor_out<motor_left)motor_out=motor_left;                                  //电机输出限幅
}
void final_control(void){
  Leftline();
  Rightline();
  CenterLine();
  steer_PD();
  speed_PID();
}  
void main(void) {
  /* put your own code here */
  DisableInterrupts;
  initPLL80M();
  initPWM();
  PP_init();
  initGPIO();
  InitPACNT();
  EnableInterrupts;  
  for(;;)
  {
    if(ccd_state==1)
    {
      final_control();
      ccd_state=0;
    }
  } /* wait forever */
  /* please make sure that you never leave this function */
}   
#pragma CODE_SEG __NEAR_SEG NON_BANKED
////////////////////中断服务程序////////////////////（中断号从Includes/MC9S12XS128.h中查询）
interrupt 56 void V_SYNC(void)
{     //行场中断服务子函数
  //--------------------------------
  if (PIFP_PIFP4)                   //场中断
  {
    All_VSYN_Count++;                       //4 VSync
    image_v_num=0;                  ////已采集到的摄像头的当前行数清零
    if (All_VSYN_Count%2 == 1)    // 奇场采集图像
       {
         //ccd_state=0;
         PIEP_PIEP5 = 1;                      // 开启行中断
       }
       else                                     // 偶场做控制
       {
         ccd_state=1;  
         PIEP_PIEP5 = 0;                      //关闭行中断
       }
    v_counter=0;                    //已读取的行数,采集行清零
    c=0;
    PIFP_PIFP4=1;
  }
  else if (PIFP_PIFP5)              //行中断
  {
   //way2-数字摄像头---------------------------------
   image_v_num++;                     //记录出现多少次行中断,即多少行
   if(v_counter<row_num)
   {
     c=0;
     if(image_v_num==selectRow[v_counter])
     {
       //此处可加延时，用晕调整采集的图像的左右对称性
       while (c<col_num)
       {    //17
         image[v_counter][c++]=PORTA;
       }
       v_counter++;
      }//记录实际记录的行
      PIFP_PIFP5=1;
    }
    PIFP_PIFP5=1;
  }
}
#pragma CODE_SEG DEFAULT


请大神帮忙啊~~~~

回首怎奈梦已空

看pwm波形

吢冇紷蒠