/******************** (C) COPYRIGHT 2011 野火嵌入式开发工作室 ********************
 * 文件名       ：main.c

**********************************************************************************/

#include "common.h"
#include "include.h" 
#include "led.h" 
#include "math.h"

u8  nrf_buff[CAMERA_SIZE + MAX_ONCE_TX_NUM];     //预多
u8 *img_bin_buff = (u8 *)(((u8 *)&nrf_buff) + COM_LEN);  //二值化图像的buf指针，由于开头有 COM_LEN 个字节是留给校验，所以需要加 COM_LEN

u8   nrf_buff2[160];                         //预多             一个三十二个的数组
u8   *nrf_u32= (u8 *)(((u8 *)&nrf_buff2) + COM_LEN)  ;   //转化为八位的

u8  nrf_buff3[ MAX_ONCE_TX_NUM];     //预多 2401发送包   发送示波器参数
u8   *nrf_3u32= (u8 *)(((u8 *)&nrf_buff3) + COM_LEN)  ; //指针

u8 img_buf[CAMERA_W*CAMERA_H];					//非压缩的二值化图像（用于识别）
s16 img_mid[CAMERA_H]=
{
PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,
PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,
PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,
PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,
PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL,PPY_NULL
};                 //中线数组 有负数
volatile u8 ppy_qianzhan;                       //前瞻行数
                                //中线数组
extern u16  ppy_receive;       //改参
extern u32  ppy_receive1;       //改参
extern u16  ppy_receive2;       //ppy_jieshou

extern u8  var1,var2;
extern u16 var3,var4;
extern u32 var5,var6;

void blprint(uint8 x,uint8 y,uint8 ax);

//速度部分
s32   Speed_PWM=0;
s32    g_nLeftMotorPulseSigma;
s32   g_nRightMotorPulseSigma;
s32   lpt1;
s32   lpt2;
s32   SPEED_CONTROL_P=0;
s32   SPEED_CONTROL_I=0; 
float g_fSpeedControlIntegral=0;
s32   g_fSpeedControlOutNew =0;
s32   g_fSpeedControlOutOld;
s32   g_fSpeedControlOut;
s8    CAR_SPEED_SETfDelta=0;

s32   Speed_error0;
s32   Speed_error_dt;
float g_fCarSpeed;
s32   Dead_PWM=400;
u8    b;
u16    a=0;
u8    c;
u8    d;
u8    e;

s8    speed=19;


extern u8 zhili_flag;
u8 get_img_flag=0;
u8 count_flage=0;

volatile u16 DUOJI=duoJiMid,DIANJI1=0,DIANJI2=0;   //按键测试用
u16 KP=100;
u16 KI=6;

u16 Factor_A=155;
u16 A_=1;  
u16 Factor_d=100;
u16 duShu=0;
s16 hope_speed=10;   //60侦对应脉冲数 //1M 接受2900个脉冲 10MS 1M/S  48  2M/S 96 3M/S 144 4M/S 192
volatile u32 LPT_INT_count=0;//脉冲计数
u16 ppy_qian=23;
//角度部分
volatile s32   Angle_set;
s32   Angle_get;

s32   Angle_error;
s32   Angle_p=120;
s32   Angle_d=7;
s32   Angle_PWM;
s32   Angle_w_get;
s32   Angle_w_set;

//PWM输出部分
s32 uPWML;
s32 uPWMR;
s32 PWML;
s32 PWMR;
//方向 控制
u8 TSL[128];

s32   left_sum;
s32   right_sum;
s32   Direct_P=50;
s32   Direct_D=6;
s32   Direct_PWM;

s32   TSL_MAX;
s32   TSL_MIN;
s32   oledend=0;
s32   Start=0;

u32 i1=0;/***********************ov7725帧数、工作状态测定*****************************/
u32 i2=0;/***********************ov7725帧数、工作状态测定*****************************/
u32 i3=0;/***********************ov7725帧数、工作状态测定*****************************/
u32 i4=0;/***********************ov7725帧数、工作状态测定*****************************/
u32 i5=0;/***********************ov7725帧数、工作状态测定*****************************/
u32 i6=0;
u32 i7=0;
u32 i8=0;
void ppy_delay(u16 ms)
{
  u16 j,k;
   u32 i;
    i=ms/2;
  for(j=0;j<i;j++)   
  { for(k=0;k<9207;k++)
      {
       gpio_turn (PORTE,0);
       }
  }
     PTE0_OUT=0;   
}
void ppy_stop(void);//停车
 void main(void)
{

  //指针指向NRF_U32第三个字节,前两个预留为命令
    s16 *deg = (s16 *)nrf_u32; 
    u8 i;
    u32 bb=0;
    
    u8 a1,a2,a3, v1,v2,v3;        //
    u8 wave1[4], wave2[4],wave3[4],wave4[4], wave5[4],wave6[4],wave7[4], wave8[4],wave9[4],wave10[4];
    u8 status = 0;	//用于判断接收/发送状态
    crtl_e  com;
    u32 value=0;
    //gpio_init(PORTB,21,GPO,HIGH);
    
    pyLCD_Init();
    LCD_CLS();
    LED_init(); 
  
     NRF_Init();
    //串口初始化
    //uart_init(UART2,19200);
    //AD初始化
    adc_init(ADC1,SE4a);//ADC1_SE4a   -- PTE0
    adc_init(ADC1,SE5a);//ADC1_SE5a   -- PTE1
    adc_init(ADC1,SE6a);//ADC1_SE6a   -- PTE2
    //端口初始化
   /**************CCD****************/
    gpio_init (PORTB,10,GPO,LOW);
    //gpio_init (PORTB,9, GPO,LOW);
    //gpio_init (PORTD,4, GPI,HIGH);
    //gpio_init (PORTD,5, GPI,HIGH);
   // gpio_init (PORTD,6, GPI,HIGH);
    //gpio_init (PORTD,7, GPI,HIGH);
    /**************CCD****************/
    //外部中断初始化
            //set_irq_priority (16, 0);//B口中断优先级
            //set_irq_priority (84, 2);//PIT0中断优先级
           // set_irq_priority (68, 3);//
           // set_irq_priority (88, 4);//B口中断优先级提前
    DisableInterrupts;
    port_init(PTB9,IRQ_FALLING | PULLUP );
    port_init(PTB0,IRQ_FALLING | PULLUP );
    port_init(PTB2,IRQ_FALLING | PULLUP );
    port_init(PTB3,IRQ_FALLING | PULLUP );
    enable_irq(88);
    //pwm初始化
    FTM_PWM_init(FTM0,CH1,10000,0);
    FTM_PWM_init(FTM0,CH0,10000,0);
    FTM_PWM_init(FTM0,CH2,10000,0); 
    FTM_PWM_init(FTM0,CH3,10000,0);
  
    for(i=0;i<3;i++)
     ppy_delay(1000);
    //time_delay_ms(1000);
    Angle_set  =ad_ave(ADC1,SE4a,ADC_12bit,255);
    Angle_w_set=ad_ave(ADC1,SE6a,ADC_12bit,255);
 
    //定时中断初始化
     pit_init_ms(PIT0 ,1);
   // pit_init_ms(PIT1 ,5);
  
     enable_irq(89);
     
 lptmr_counter_init(LPT0_ALT1,PPY_LPT,2,LPT_Falling);
  lptmr_counter_init(LPT0_ALT2,PPY_LPT,2,LPT_Falling);     
     
    Ov7725_Init(img_bin_buff);                    //摄像头初始化  
    EnableInterrupts;
   
     
       while(1)
         
       { 
         
         
         if(zhili_flag==0) 
        { 
          i7++;
         disable_irq(68);
         ov7725_get_img();
      img_extract(img_buf , img_bin_buff, CAMERA_SIZE);   //图片解压
        ppy_qianzhan = ppy_img_mid(img_mid,img_buf);    //处理图像找中线,和前瞻  将图形中线放入中线数组 返回前瞻
        ppy_buxian(ppy_qianzhan,img_mid);      //传递前瞻加权求引导中线值 
        count_flage=1;
      
        /*img_extract(img_buf , img_bin_buff, CAMERA_SIZE);   //图片解压
        ppy_qianzhan = ppy_img_mid(img_mid,img_buf);    //处理图像找中线,和前瞻  将图形中线放入中线数组 返回前瞻
        ppy_buxian(ppy_qianzhan,img_mid);      //传递前瞻加权求引导中线值  */
     
          
        
        
   /*   NRF_MSG_send(COM_IMG,nrf_buff);    		//发送图像数据         
     do   {     status = NRF_MSG_send_state();		//获取发送状态            
     }while( status == TX_ISR_SEND);
         status = NRF_MSG_receive(&com,nrf_buff2);	//接收数据,并进行处理
		
		if(status == NRF_RESULT_RX_VALID )				//接收到有效数据
		{
			switch(com)
			{
                               
                                 case COM_2U32:         
                                       
                                   
                                   switch(ppy_receive) 
                                   {
                                     case 0x61:                 // 'a' KP1
                                       KP=ppy_receive1;
                                     break;
                                     
                                     case 0x62:                 // 'b' KI1
                                       KI=ppy_receive1;
                                     break;
                                     
                                     case 0x63:                 // 'c' KP2
                                       
                                     break;
                                     
                                     case 0x64:                 // 'd' KP2
                                     break;
                                     
                                     case 0x65:                 // 'e'   Angle_p
                                       Angle_p=ppy_receive1;
                                       LED_turn(LED0);
                                      // LED_DELAY_MS(200);
                                       LED_turn(LED0);
                                       break;
                                     
                                     case 0x66:                 //Angle_d
                                       Angle_d=ppy_receive1;
                                       LED_turn(LED0);
                                      // LED_DELAY_MS(200);
                                       LED_turn(LED0);
                                     break;
                                    
                                     case 0x67:                      //速度期望
                                       hope_speed=ppy_receive1;
                                     break;
                                     
                                     case 0x01:                 // 'd' KP2
                                     break;
                                     
                                     case 0x02:                 // 'd' KP2
                                     break;
                                     
                                     case 0x03:                 // 'd' KP2
                                     break;
                                   
                                   default:
                                 break; 
                                   }
                                   
                            break;  
                                          
                                          
                                                    
                                  
                               default:
                                 break;     
			}
		}*/
              //  enable_irq(68);
        }
          
       //  LED_DELAY_MS(3);
         
}
         while(0)
    {   
      
       
      if(zhili_flag==0)
      {//get_img_flag=1;
        i8++;
        disable_irq(68);
        ov7725_get_img();
       /* img_extract(img_buf , img_bin_buff, CAMERA_SIZE);   //图片解压
        ppy_qianzhan = ppy_img_mid(img_mid,img_buf);    //处理图像找中线,和前瞻  将图形中线放入中线数组 返回前瞻
        ppy_buxian(ppy_qianzhan,img_mid);      //传递前瞻加权求引导中线值  */
     
        
        
        
       /*NRF_MSG_send(COM_IMG,nrf_buff);    		//发送图像数据         
     do   {     status = NRF_MSG_send_state();		//获取发送状态            
     }while( status == TX_ISR_SEND);*/
      
     
      status = NRF_MSG_receive(&com,nrf_buff2);	//接收数据,并进行处理
		
		if(status == NRF_RESULT_RX_VALID )				//接收到有效数据
		{
			switch(com)
			{
                               
                                 case COM_2U32:         
                                       
                                   
                                   switch(ppy_receive) 
                                   {
                                     case 0x61:
                                       KP=ppy_receive1;
                                     break;
                                     
                                     case 0x62:
                                       KI=ppy_receive1;
                                     break;
                                     
                                     case 0x63:
                                     break;
                                     
                                     case 0x64:
                                     break;
                                     
                                     case 0x65:                //'e'
                                       Angle_p=ppy_receive1;
                                       LED_turn(LED0);
                                      // LED_DELAY_MS(200);
                                       LED_turn(LED0);
                                       break;
                                     
                                     case 0x66:
                                       Angle_d=ppy_receive1;
                                       LED_turn(LED0);
                                      // LED_DELAY_MS(200);
                                       LED_turn(LED0);
                                     break;
                                   
                                   
                                   default:
                                 break; 
                                   }
                                   
                            break;  
                                          
                                          
                                                    
                                  
                               default:
                                 break;     
			}
		}
     // get_img_flag=0;
      enable_irq(68); 
      
      }
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
      
           /*if(PTD6_IN==1&&PTD5_IN==1&&PTD4_IN==1&&Start==1)
           {
                  speed=19;
                  Direct_P=40;
                  Direct_D=50;   
                  CAR_SPEED_SETfDelta=speed;    
           }
           if(PTD6_IN==1&&PTD5_IN==1&&PTD4_IN==0&&Start==1)
           {
                 speed=20;
                 Direct_P=40;
                 Direct_D=50;     
                 CAR_SPEED_SETfDelta=speed; 
             
           }
           if(PTD6_IN==1&&PTD5_IN==0&&PTD4_IN==1&&Start==1)
           {
                 speed=21;
                 Direct_P=44;
                 Direct_D=50;   
                 CAR_SPEED_SETfDelta=speed; 
           }
           if(PTD6_IN==1&&PTD5_IN==0&&PTD4_IN==0&&Start==1)
           {
                 speed=22;
                 Direct_P=52;
                 Direct_D=70;  //60 
                 CAR_SPEED_SETfDelta=speed; 
           }
           if(PTD6_IN==0&&PTD5_IN==1&&PTD4_IN==1&&Start==1)
           {
                 speed=23;
                 Direct_P=62;//60
                 Direct_D=80;  //80      
                 CAR_SPEED_SETfDelta=speed; 
           }
           if(PTD6_IN==0&&PTD5_IN==1&&PTD4_IN==0&&Start==1)
           {
                 speed=24;
                 Direct_P=80;//60
                 Direct_D=80;  //80      
                 CAR_SPEED_SETfDelta=speed; 
           }
           if(PTD6_IN==0&&PTD5_IN==0&&PTD4_IN==1&&Start==1)
           {
                 speed=25;
                 Direct_P=100;//60
                 Direct_D=120;  //80      
                 CAR_SPEED_SETfDelta=speed; 
           }
           
           
           SHOW_data();
    
          LCD_TSL1401_SHOW();
           
                
        // NRF_MSG_send(COM_CCD,nrf_buff2); 
         // NRF_MSG_send(COM_CCD,nrf_buff2);      
        //  do {   status = NRF_MSG_send_state();		//获取发送状态            
       //  }while( status == TX_ISR_SEND);       	//如果图像发送中，则需要继续等待发送完成*/
    
    
      }
    
    
}


void ppy_stop(void)
{ 
 /* if(PTB20_IN==0)
     oled(2,1,"20",0); 
     else
      oled(2,1,"20",1); 
      if(PTB21_IN==0)
     oled(2,2,"21",0); 
     else
      oled(2,2,"21",1); */
     
 
  
  FTM_PWM_Duty(FTM0,CH2,20000);           //PTc3  
                   FTM_PWM_Duty(FTM0,CH3,0);           //PTc4 
                    int time1;
                    
                   for(time1=0;time1<10;time1++)
                  {
                    
//                     //ppy_irqdis();  //关中断避免影响采集
                ov7725_get_img();			            //采集图像 
//               // ppy_irqen();     //开中断
               //    NRF_MSG_send(COM_IMG,nrf_buff);    		//发送图像数据
                 //  img_sd_save((u8 *)img_bin_buff);  //sd 卡记录 50侦 平时60侦  不能与示波器同时用
                 img_extract(img_buf , img_bin_buff, CAMERA_SIZE);   //图片解压
                ppy_qianzhan = ppy_img_mid2(img_mid,img_buf);    //处理图像找中线,和前瞻  将图形中线放入中线数组 返回前瞻
                 ppy_buxian2(ppy_qianzhan,img_mid);      //传递前瞻加权求引导中线值 
                
                  ppy_duoji2(ppy_qianzhan);        // 打舵 
                //   ppy_speed=180;
                     FTM_PWM_Duty(FTM0,CH2,25000);           //PTc4 
                  //   ppy_speedcol2(ppy_qianzhan);   //速度控制
                    
                  }
                   
                  FTM_PWM_Duty(FTM0,CH2,0);           //PTc3  
                  
                 
                  for(time1=0;time1<40;time1++)
                  {
 //                    ppy_irqdis();  //关中断避免影响采集
                ov7725_get_img();			            //采集图像 
 //              // ppy_irqen();     //开中断
               //    NRF_MSG_send(COM_IMG,nrf_buff);    		//发送图像数据
                 //  img_sd_save((u8 *)img_bin_buff);  //sd 卡记录 50侦 平时60侦  不能与示波器同时用
                 img_extract(img_buf , img_bin_buff, CAMERA_SIZE);   //图片解压
                ppy_qianzhan = ppy_img_mid2(img_mid,img_buf);    //处理图像找中线,和前瞻  将图形中线放入中线数组 返回前瞻
                 ppy_buxian2(ppy_qianzhan,img_mid);      //传递前瞻加权求引导中线值  
                
                  ppy_duoji2(ppy_qianzhan);        // 打舵   
                 //  ppy_speed=0;
                  //   ppy_speedcol2(ppy_qianzhan);   //速度控制
                     FTM_PWM_Duty(FTM0,CH3,27000);           //PTc4 
                  }
                  
                    for(time1=0;time1<60;time1++)
                  {
//                     ppy_irqdis();  //关中断避免影响采集
                ov7725_get_img();			            //采集图像 
//               ppy_irqen();     //开中断
               //    NRF_MSG_send(COM_IMG,nrf_buff);    		//发送图像数据
                 //  img_sd_save((u8 *)img_bin_buff);  //sd 卡记录 50侦 平时60侦  不能与示波器同时用
                 img_extract(img_buf , img_bin_buff, CAMERA_SIZE);   //图片解压
                ppy_qianzhan = ppy_img_mid2(img_mid,img_buf);    //处理图像找中线,和前瞻  将图形中线放入中线数组 返回前瞻
                 ppy_buxian2(ppy_qianzhan,img_mid);      //传递前瞻加权求引导中线值  
                
                  ppy_duoji2(ppy_qianzhan);        // 打舵   
                    FTM_PWM_Duty(FTM0,CH2,0);           //PTc3  
                   FTM_PWM_Duty(FTM0,CH3,0);           //PTc4 
                  }
    //              py=ppy_key_s;
     //             ppyModel=ppy_standby;
  
                  return ;
} 


  
  
     
    
        
   
   


 
