コード例 #1
0
ファイル: car.c プロジェクト: Chasingwhat/Self-Balancing-Car
/*******获取角度的模拟值*****/
void Get_AD_angle()
{
       u16 Gyro_Angle,J_Angle;
       Gyro_Angle=ad_ave(ADC1,SE4a,ADC_12bit,70); //50均值滤波;之前用的是10次滤波。现在改为了100次的滤波(摄像头的问题好像是出现在这里)
       J_Angle=ad_ave(ADC1,SE5a,ADC_12bit,70); //均值滤波50;
       Gyro_Angle_Num =Gyro_Angle0 - Gyro_Angle;//陀螺仪角速度
       J_Angle_Num=J_Angle0-J_Angle;//角度
       Gyro_numcha=Gyro_Angle_Num;
}
コード例 #2
0
ファイル: isr.c プロジェクト: Magicwangs/IAR
void PIT0_IRQHandler(void)
{
  DisableInterrupts; 

    TimeCount ++ ;
    
   I1_Vol_present_value=ad_ave(ADC0, AD9,ADC_16bit,10);
   I1_Vol_present_value=I1_Vol_present_value/65535*3.3;
   
   U1_present_value=ad_ave(ADC0, AD10,ADC_16bit,10);
   U1_present_value=U1_present_value/65535*3.3*16;
   
   U2_present_value=ad_ave(ADC0, AD12,ADC_16bit,10);
   U2_present_value=U2_present_value/65535*3.3*16;

   sum+=I1_Vol_present_value;
   
   PID_control();

  if(TimeCount==3000)
  {
    TimeCount = 0 ;
    sum/=3000;
    I1_Vol_present_value=sum;
    Refresh_Present();
    sum=0;
    if(U1_present_value>=24)
    {
      gpio_init(PORTC,3,GPO,0);
      gpio_init(PORTC,4,GPO,0);
      start_flag=0;
      ftm_flag=0;
    }
    
  }

  EnableInterrupts;
  PIT_Flag_Clear(PIT0);       //清中断标志位
}
コード例 #3
0
ファイル: main.c プロジェクト: avalonLZ/freescale10
 void main()
{  
 DisableInterrupts;
      
   volatile u8 i;
   u32 bianliang;  
 
  flash_init();
  gpio_init (PORTA, 24, GPI_UP_PF,1);///////拨码
  gpio_init (PORTA, 25, GPI_UP_PF,1);
  gpio_init (PORTA, 6, GPI_UP_PF,1);
  gpio_init (PORTA, 10, GPI_UP_PF,1);///////按键
  gpio_init (PORTA, 12, GPI_UP_PF,1);
  gpio_init (PORTA, 8, GPI_UP_PF,1);
  
  if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==0)
   {    
     
      LCD_Init();
   //flash_erase_sector(255);
 //  flash_write(255,0x004,0x03e8);/////////////////////////////需要给定初值时
     while(1&&tx==0)
     {
       if(gpio_get(PORTA,10)==0)
       {
         delayms(100);
         if(gpio_get(PORTA,10)==0)/////////确定按下加计数
         { 
           bianliang=flash_read(255,0x004,u32);
           delayms(50);
           bianliang+=5;
           flash_erase_sector(255);
           delayms(50);
           flash_write(255,0x004, (u32)bianliang);
           delayms(50);
           LCD_CLS();
           LCD_PrintValueI(8, 4, bianliang);
         } 
       }

     if(gpio_get(PORTA,12)==0)
     {
        delayms(100);
         if(gpio_get(PORTA,12)==0)
         {
        
             bianliang=flash_read(255,0x004,u32);
             delayms(50);
             bianliang-=5;
             flash_erase_sector(255);
             delayms(50);
             flash_write(255,0x004,(u32)bianliang); 
             delayms(50);
             LCD_CLS();//清屏
             LCD_PrintValueI(8, 4,bianliang);
         }
      }
      
     if(gpio_get(PORTA,8)==0)
     {
        delayms(100);
        if(gpio_get(PORTA,8)==0)
        {          
            tx=1;   
        }
        else
          tx=0;
     }

  
     }
   }
  /////////////////////////////////////////////////////////////////////////////////////////单道设置档
  if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==0)//////////////右转正值
  {
    LCD_Init();
    while(1)
   {
    if(gpio_get(PORTA,10)==0)
       {
         delayms(100);
         if(gpio_get(PORTA,10)==0)/////////单道右转
         {
           delayms(50);
           flash_erase_sector(254);
           delayms(50);
           flash_write(254,0x004, (u32)6);
           delayms(50);
           LCD_Char( 10,3,'R');
           LCD_Char(26 ,3,'+') ;
           LCD_Char(32 ,3,'6') ;
         }
       }
    if(gpio_get(PORTA,12)==0)
       {
         delayms(100);
         if(gpio_get(PORTA,12)==0)/////////单道左转
         {
           delayms(50);
           flash_erase_sector(254);
           delayms(50);
           flash_write(254,0x004, (u32)(-6));
           delayms(50);
           LCD_CLS();
           LCD_Char( 10,3,'L');
           LCD_Char(26 ,3,'-') ;
           LCD_Char(32 ,3,'6') ;
         }
       }  
    if(gpio_get(PORTA,8)==0)
       {
         delayms(100);
         if(gpio_get(PORTA,8)==0)/////////单道直道
         {
           delayms(50);
           flash_erase_sector(254);
           delayms(50);
           flash_write(254,0x004, (u32)0);
           delayms(50);
           LCD_CLS();
           LCD_Char(32 ,3,'0');
         }
       }  
   }
  }
/////////////////////////////////////////////////////////////////////////////////////////////////////////////速度档位  
  
  if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==1)/////////////////////
   {    
       yz=10,yz1=6,yz2=3;///定义搜线阈值
      buxianL=25,buxianR=27;//左右补线值
      dandao=(int)flash_read(254,0x004,u32);
      zhijiao=8;//直角延时场数
      quanheiMAX=60;//全黑最大值
      quanbaiMIN=80;//全白最小值
      dandaoBDD=9.455;////////////////////////////////////////////////////////////
      dandaoHDD=9.455;////////////////////////////////////////////////////////////
      zhijiaoBDpwmmax=500.0;
      zhijiaoBS_Kp=10.0;
      zhijiaoBD_Kp=378.25;
      zhijiaoHDpwmmax=300.0;
      zhijiaoHS_Kp=42.0;//50轻微振荡
      zhijiaoHD_Kp=305.25;//299.25

      zkp=2200;//清华PD:2053   卡尔曼PD:2200
      zkd=47;//44/////////////50 
      
      S_Kpstart=10.0;//  1.3
      S_Kistart=33.91;//11.8
      S_Kpend=42.0;///////////////29.0
      S_Kiend=32.01;//11.8
      Sqiwang =1400;//  1.3
      Sjishu =7;//11.8
        
      D_Kp =305.25;   //299.25
      D_Kd =300.0;//
      D_Kdd=9.455;//12.05
      zhangaiDmax=300.0;
      direction_pwmmax=300.0;
   }
  
  if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==1&&gpio_get(PORTA,6)==1)/////////////////////
  {
        yz=10,yz1=6,yz2=3;///定义搜线阈值
      buxianL=25,buxianR=27;//左右补线值
      dandao=(int)flash_read(254,0x004,u32);
      zhijiao=10;//直角延时场数
      quanheiMAX=60;//全黑最大值
      quanbaiMIN=80;//全白最小值
      dandaoBDD=9.455;////////////////////////////////////////////////////////////
      dandaoHDD=9.455;////////////////////////////////////////////////////////////
      zhijiaoBDpwmmax=500.0;
      zhijiaoBS_Kp=10.0;
      zhijiaoBD_Kp=378.25;
      zhijiaoHDpwmmax=300.0;
      zhijiaoHS_Kp=42.0;//50轻微振荡
      zhijiaoHD_Kp=305.25;//299.25

      zkp=2200;//清华PD:2053   卡尔曼PD:2200
      zkd=47;//44/////////////50 
      
      S_Kpstart=10.0;//  1.3
      S_Kistart=33.91;//11.8
      S_Kpend=42.0;///////////////29.0
      S_Kiend=32.01;//11.8
      Sqiwang =1400;//  1.3
      Sjishu =7;//11.8
        
      D_Kp =305.25;   //299.25
      D_Kd =300.0;//
      D_Kdd=9.455;//12.05
      zhangaiDmax=300.0;
      direction_pwmmax=300.0;
  }
  
  
  
   if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==1)/////////////////////
   {     
       yz=10,yz1=6,yz2=3;///定义搜线阈值
      buxianL=25,buxianR=27;//左右补线值
      dandao=(int)flash_read(254,0x004,u32);
      zhijiao=12;//直角延时场数
      quanheiMAX=60;//全黑最大值
      quanbaiMIN=80;//全白最小值
      dandaoBDD=9.455;////////////////////////////////////////////////////////////
      dandaoHDD=9.455;////////////////////////////////////////////////////////////
      zhijiaoBDpwmmax=500.0;
      zhijiaoBS_Kp=10.0;
      zhijiaoBD_Kp=378.25;
      zhijiaoHDpwmmax=300.0;
      zhijiaoHS_Kp=42.0;//50轻微振荡
      zhijiaoHD_Kp=305.25;//299.25

      zkp=2200;//清华PD:2053   卡尔曼PD:2200
      zkd=47;//44/////////////50 
      
      S_Kpstart=10.0;//  1.3
      S_Kistart=33.91;//11.8
      S_Kpend=42.0;///////////////29.0
      S_Kiend=32.01;//11.8
      Sqiwang =1400;//  1.3
      Sjishu =7;//11.8
        
      D_Kp =305.25;   //299.25
      D_Kd =300.0;//
      D_Kdd=9.455;//12.05
      zhangaiDmax=300.0;
      direction_pwmmax=300.0;
   }
  
  if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==1)//////////////////////最低速     1400
  {
       yz=10,yz1=6,yz2=3;///定义搜线阈值
      buxianL=25,buxianR=27;//左右补线值
      dandao=(int)flash_read(254,0x004,u32);
      zhijiao=14;//直角延时场数
      quanheiMAX=60;//全黑最大值
      quanbaiMIN=80;//全白最小值
      dandaoBDD=9.455;////////////////////////////////////////////////////////////
      dandaoHDD=9.455;////////////////////////////////////////////////////////////
      zhijiaoBDpwmmax=500.0;
      zhijiaoBS_Kp=10.0;
      zhijiaoBD_Kp=378.25;
      zhijiaoHDpwmmax=300.0;
      zhijiaoHS_Kp=42.0;//50轻微振荡
      zhijiaoHD_Kp=305.25;//299.25

      zkp=2200;//清华PD:2053   卡尔曼PD:2200
      zkd=47;//44/////////////50 
      
      S_Kpstart=10.0;//  1.3
      S_Kistart=33.91;//11.8
      S_Kpend=42.0;///////////////29.0
      S_Kiend=32.01;//11.8
      Sqiwang =1400;//  1.3
      Sjishu =7;//11.8
        
      D_Kp =305.25;   //299.25
      D_Kd =300.0;//
      D_Kdd=9.455;//12.05
      zhangaiDmax=300.0;
      direction_pwmmax=300.0; 
  }
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////特殊速度档
      if(gpio_get(PORTA,24)==0&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==0)
  {
      yz=10,yz1=6,yz2=3;///定义搜线阈值
      buxianL=25,buxianR=27;//左右补线值
      dandao=(int)flash_read(254,0x004,u32);
      zhijiao=14;//直角延时场数
      quanheiMAX=60;//全黑最大值
      quanbaiMIN=80;//全白最小值
      dandaoBDD=9.455;////////////////////////////////////////////////////////////
      dandaoHDD=9.455;////////////////////////////////////////////////////////////
      
      zhijiaoBDpwmmax=500.0;
      zhijiaoBS_Kp=10.0;
      zhijiaoBD_Kp=378.25;
      zhijiaoHDpwmmax=300.0;
      zhijiaoHS_Kp=42.0;//50轻微振荡
      zhijiaoHD_Kp=305.25;//299.25

      zkp=2400;//清华PD:2053   卡尔曼PD:2200
      zkd=47;//44/////////////50 
      
      S_Kpstart=10.0;//  1.3
      S_Kistart=31.91;//11.8
      S_Kpend=42.0;///////////////29.0
      S_Kiend=30.01;//11.8
      Sqiwang =1400;//  1.3
      Sjishu =7;//11.8
        
      D_Kp =305.25;   //299.25
      D_Kd =300.0;//
      D_Kdd=9.455;//12.05
      zhangaiDmax=300.0;
      direction_pwmmax=300.0;
  }
  
    if(gpio_get(PORTA,24)==1&&gpio_get(PORTA,25)==0&&gpio_get(PORTA,6)==0)//////////////////////////有值太大
  {
   
      yz=10,yz1=6,yz2=3;///定义搜线阈值
      buxianL=25,buxianR=27;//左右补线值
      dandao=(int)flash_read(254,0x004,u32);
      zhijiao=16;//直角延时场数
      quanheiMAX=60;//全黑最大值
      quanbaiMIN=80;//全白最小值
      dandaoBDD=9.455;////////////////////////////////////////////////////////////
      dandaoHDD=9.455;////////////////////////////////////////////////////////////
      
      zhijiaoBDpwmmax=500.0;
      zhijiaoBS_Kp=10.0;
      zhijiaoBD_Kp=378.25;
      zhijiaoHDpwmmax=300.0;
      zhijiaoHS_Kp=42.0;//50轻微振荡
      zhijiaoHD_Kp=305.25;//299.25

      zkp=2400;//清华PD:2053   卡尔曼PD:2200
      zkd=47;//44/////////////50 
      
      S_Kpstart=10.0;//  1.3
      S_Kistart=31.91;//11.8
      S_Kpend=42.0;///////////////29.0
      S_Kiend=30.01;//11.8
      Sqiwang =1400;//  1.3
      Sjishu =7;//11.8
        
      D_Kp =305.25;   //299.25
      D_Kd =300.0;//
      D_Kdd=9.455;//12.05
      zhangaiDmax=300.0;
      direction_pwmmax=300.0;
  }

   /**/
  
     //串口初始化
 asm("nop");
  asm("nop");
asm("nop");
  asm("nop");
asm("nop");
  asm("nop");


asm("nop");
  asm("nop");

  if(tx==1)
     uart_init(UART5,115200);
  else
  uart_init(UART5,9600); 
 asm("nop");
  asm("nop");

   //CCD及其各引脚初始化
   CCD1_init() ;
   CCD2_init() ;
asm("nop");
  asm("nop");



   //PWM输出初始化
    FTM_PWM_init(FTM0, CH0, 10000, 0);  //R后退        
    FTM_PWM_init(FTM0, CH1, 10000, 0);   //R前进         
    FTM_PWM_init(FTM0, CH2, 10000, 0);   //L后退        
    FTM_PWM_init(FTM0, CH3, 10000, 0); //L前进
   //拨码初始化
   // gpio_init (PORTA, 7, GPI_UP_PF,1);
    
asm("nop");
asm("nop");

    gpio_init (PORTE, 1, GPI,0);
    gpio_init (PORTD, 15, GPI,0); 
    gpio_init (PORTD, 14, GPI,0);
    gpio_init (PORTD, 12,GPI,0);
    
asm("nop");
asm("nop");
asm("nop");
  asm("nop");
    //正交解码初始化
    FTM1_QUAD_Iint();
    FTM2_QUAD_Iint();
asm("nop");
asm("nop");    
asm("nop");
asm("nop");
    //各种中断初始化
 pit_init_ms(PIT0, 1);
   asm("nop");
asm("nop");
asm("nop");
asm("nop");

    // 陀螺仪及加速度计初始化
    adc_init(ADC0, AD12);      //////  PTB2    ENC      
    adc_init(ADC0, AD12);      ///PTB3      Z
    adc_init(ADC0, AD17);       ///PTE24     DIR_ENC

    /////////////////////////////////////////////////陀螺仪开机自检
  
        delayms(1000);
        gpio_init (PORTA, 17, GPO, 0);
         MMA7361level=flash_read(255,0x004,u32);
        rectifyT = ad_ave(ADC0,SE12,ADC_12bit,10);
    gyro_DIR=ad_ave(ADC0,SE17,ADC_12bit,10);
   // gyro_DIR=gyro_DIR/20;
      /////////////////////////////////////////////////        
//  gpio_init (PORTC, 0, GPI,0);不知道要用否
        delayms(1500); 
        EnableInterrupts;
        
    while(1)
     
    {  
       if((gpio_get(PORTD,12)==0)&&(gpio_get(PORTD,14)==0))//||(gpio_get(PORTE,1)==1)
         {b=1;d=0;}
    if(fs>=1)
     {
          DisableInterrupts;
         fs=0;js=0;
        ImageCapture1(Pixel1);
        ImageCapture2(Pixel2);
       EnableInterrupts;
       Analyze_CCD_Data();      
    }
     
    if(tx==1)
    {
   sendDataToCCDViewer(0,Pixel1);//////////////////////////////////////
        sendDataToCCDViewer(1,Pixel2); 
        sendDataToScope();
    }
  
    else
    {
OutData[0]=Min_Data1;//L_Max_Data_Place3
OutData[1]=Max_Data1;//R_Max_Data_Place1
OutData[2]=L_Max_Data_Place1; //L_Max_Data_Place1
OutData[3]=R_Max_Data_Place1;//
OutPut_Data();
    }//
//sb[0]=FTM1_CNT; 
//sb[1]=MMA7361; 
// tongxun();
    }     
    }
コード例 #4
0
ファイル: main.c プロジェクト: HKUST-Robotics/USTSmartcar
void main()
{   

  uart_init(UART3, 115200); // For our flashed bluetooth
  //uart_init(UART3, 9600); // For our non-flashed bluetooth
  gpio_init(PORTE,6,GPI,0); // SW2 goes into gyro calibration mode
  
  
  printf("\nWelcome to the SmartCar 2013 Sensor team developement system\n");
  while(1){
    
   printf("==============================================================\n");
   printf("Please select mode:\n---------------------------\n");
   printf("1:Accelerometer&gyro\n");
   printf("2:LinearCCD\n");
   printf("3:Flash Memory\n");
   printf("4:Encoder testing\n");
   printf("6:Motor control test\n");
   printf("7:SystemLoop Test\n");
   
   delayms(300);
   
   if(gpio_get(PORTE,6)==0){
     
     adc_init(ADC1,AD5b);
     adc_init(ADC1,AD7b);
        
     printf("We are now in gyro calibration mode, Please keep car stationery and wait till light dies");
     gpio_init(PORTE,24,GPO,0);
     gpio_init(PORTE,25,GPO,0);
     gpio_init(PORTE,26,GPO,0);
     gpio_init(PORTE,27,GPO,0);
     
     delayms(3000);
     
     turn_gyro_offset=ad_ave(ADC1,AD5b,ADC_12bit,1000);
     balance_gyro_offset=ad_ave(ADC1,AD7b,ADC_12bit,1000);
     
     store_u32_to_flashmem1(turn_gyro_offset);
     store_u32_to_flashmem2(balance_gyro_offset);
     
     gpio_turn(PORTE,24);
     gpio_turn(PORTE,25);
     gpio_turn(PORTE,26);
     gpio_turn(PORTE,27);
     
     while(1){}
     
   }
   
   g_char_mode = '7';                 // Hard code mode = system loop
   //g_char_mode = uart_getchar(UART3);
 
     switch (g_char_mode){
      case '0':
        //VR analog input
        adc_init(ADC0,AD14);
        
        while(1){
          delayms(500);
          printf("\n%d",ad_once(ADC0,AD14,ADC_16bit));//vr value
        }
          
      break;
       
      case '1':
        uart_sendStr(UART3,"The mode now is 1: Accelerometer and Gyroscope Test");
        
        //accl_init();
        adc_init(ADC1,AD6b);
        adc_init(ADC1,AD7b);
        adc_init(ADC0,AD14);
        adc_init(ADC1,AD4b);
        
        
        balance_centerpoint_set=ad_ave(ADC0,AD14,ADC_12bit,10);

        printf("\nEverything Initialized alright\n");
        
        while(1)
        { 
          //printf("\n\f====================================");
          //control_tilt=(ad_ave(ADC1,AD6b,ADC_12bit,8)-3300)+(balance_centerpoint_set/10);
          //printf("\nMain gyro%d",control_tilt);//theta
          //printf("\n%d",ad_once(ADC1,AD7b,ADC_12bit)-1940);//omega
          printf("\n%d",ad_ave(ADC1,AD6b,ADC_12bit,8)-940);
          delayms(50);
          
        }
     break;
     
     case '2':
        uart_sendStr(UART3,"The mode now is 2: Linear CCD");
        ccd_interrupts_init();
        printf("\nEverything Initialized alright\n");
       
        while(1)
        { 
            //ccd_sampling(1); // Longer SI CCD Sampling
        }
     break;
     
     case '3':
        uart_sendStr(UART3,"The mode now is 3:Flash Memory\n");
        Flash_init();
        printf("Flash Memory init ok\n");
        printf("writing the u32 : 3125 to memory 1\n");
        store_u32_to_flashmem1(3125);
        printf("writing the u32: 1019 to memory 2\n");
        store_u32_to_flashmem2(1019);
        
        printf("Now reading from memory\n");
        printf("Memory 1:%d\n",get_u32_from_flashmem1());
        
        printf("Memory 2:%d\n",get_u32_from_flashmem2());
        
        
       
        while(1)
        { 
            //stops loop to see results
        }
     break;
        
      case '4':
        //temporary case for debuggine encoder libraries, move to encoder.h later
        uart_sendStr(UART3,"The mode now is 4: encoder test");
             
        DisableInterrupts;
        exti_init(PORTA,6,rising_up);    //inits left encoder interrupt capture
        exti_init(PORTA,7,rising_up);    //inits right encoder interrupt capture
             
        pit_init_ms(PIT1,500);                 //periodic interrupt every 500ms
     
        EnableInterrupts;
        printf("\nEverything Initialized alright\n");
     
        while(1){
        
        }
          
      break;

      case '6':
        uart_sendStr(UART3,"The mode now is 6: Motor Control test");
        //inits
        motor_init();
        
        printf("\nEverything Initialized alright\n");
        delayms(1000);
        
        while(1)
        { 
          //printf("\n\fInput 0-9 Motor Speed, Currently:%d",motor_test);
          //motor_test=100*(uart_getchar(UART3)-48);
          
          FTM_PWM_Duty(FTM1,CH0,2000);
          FTM_PWM_Duty(FTM1,CH1,2000);//left
          
          printf("\n\f Input direction : 0 or 1");
          motor_test = uart_getchar(UART3)-48;
         
          
          
          if (motor_test){
            gpio_set(PORTD,9,1);
            gpio_set(PORTD,7,1);//this is DIR
          }else{
            gpio_set(PORTD,9,0);
            gpio_set(PORTD,1,0);//this is DIR
          }
          
        }  
      break;

      case '7':
        printf("\n The Mode is now 7: SystemLoop Test");
        
        adc_init(ADC1,AD6b);
        adc_init(ADC1,AD7b);
        adc_init(ADC0,AD14);
        adc_init(ADC1,AD5b);
        
        balance_centerpoint_set=ad_ave(ADC0,AD14,ADC_12bit,10);
        
        motor_init();
        
        gpio_set(PORTD,9,0); //dir
        gpio_set(PORTD,7,0); //dir
        
        FTM_PWM_Duty(FTM1, CH0, 3000); //initital speed
        FTM_PWM_Duty(FTM1, CH1, 3000); //initital speed               
        
        ccd_interrupts_init();
        pit_init_ms(PIT3,1);
        
        printf("\nEverything inited alright");
        
        
        while(1){
          //system loop runs 
        }
        
     break;
      
     default :
     printf("\n\fYou entered:%c, Please enter a number from 1-7 to select a mode\n\f",g_char_mode);
        
    }
   }
}
コード例 #5
0
void main()
{
    OLED_Init();           //初始化oled
    qd=1;
    ftm_pwm_init(FTM0,FTM_CH3,10000,0);
    ftm_pwm_init(FTM0,FTM_CH4,10000,0);
    ftm_pwm_init(FTM2,FTM_CH0,10000,0);
    ftm_pwm_init(FTM2,FTM_CH1,10000,0);
    adc_init (ADC1_SE10);
    adc_init (ADC1_SE11);
    adc_init (ADC1_SE12);
    adc_init (ADC1_SE13);                               //按键初始化
    gpio_init (PTA13, GPI,HIGH);//拨码开关初始化
    gpio_init (PTA19, GPI,HIGH);
    gpio_init (PTA24, GPI,HIGH);
    gpio_init (PTA25, GPI,HIGH);
    gpio_init (PTA26, GPI,HIGH);
    gpio_init (PTA27, GPI,HIGH);
    gpio_init (PTA28, GPI,HIGH);
    gpio_init (PTA29, GPI,HIGH);

    led_init (LED0);
    mpu6050_init();

    lptmr_delay_ms(1000);

    gyro_zero=ad_ave(100);

    gyro_zero1=ad_ave1(100);

    mpu6050_read();

    accel_accel=(accel_x-accel_zero)/16384.0;
    if(accel_accel>1)   accel_accel=1;
    if(accel_accel<-1)  accel_accel=-1;
    angle_fuse=180/pi*(asin(accel_accel));

    accel_accel1=(accel_y-accel_zero1)/16384.0;
    if(accel_accel1>1)   accel_accel1=1;
    if(accel_accel1<-1)  accel_accel1=-1;
    angle_fuse1=180/3.1415926*(asin(accel_accel1));

    pit_init_ms(PIT0, 5);                                //初始化PIT0,定时时间为: 5ms
    set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler);      //设置PIT0的中断服务函数为 PIT0_IRQHandler
    enable_irq (PIT0_IRQn);                                 //使能PIT0中断

    uart_init(UART3, 115200);
    while(aa<200);   //初始化 1秒


    DIP_switch();
    while(1)
    {


        //display[0]=angle_fuse;
        //display[1]=angle_fuse1;
        display[0]=angle_fuse3;



        oledplay();


        if(flag==1)
            mode1();
        else if (flag==2)
            mode2();
        else if (flag==3)
            mode3();
        else if (flag==4)
            mode4();



        vcan_sendware((unsigned char *)display, 20);

    }
}