#include "zf_common_headfile.h"

/***********************全局初始化***************************/
void global_init(void)
{
	/***********************蜂鸣器初始化*************************/
//	gpio_init(D4, GPO, 0, GPIO_PIN_CONFIG);
	/************************中断初始化**************************/
//	pit_init();
//  pit_ms_init(PIT_CH0, 4);
	/**************************屏幕初始化************************/
    ips200_init(IPS200_TYPE_PARALLEL8);
	/************************PWM初始化**************************/
//	pwm_init(PWM1_MODULE3_CHB_D1,16000,0);    //left_front  positive
//	pwm_init(PWM2_MODULE3_CHA_D2,16000,0);    //right_front  positive
//	pwm_init(PWM1_MODULE1_CHA_D14,16000,0);   //left_back  positive
//	pwm_init(PWM1_MODULE0_CHA_D12,16000,0);   //right_back  positive
//	
//	pwm_init(PWM1_MODULE3_CHA_D0,16000,0);    //left_front  negative
//	pwm_init(PWM2_MODULE3_CHB_D3,16000,0);    //right_front  negative
//	pwm_init(PWM1_MODULE1_CHB_D15,16000,0);   //left_back  negative
//	pwm_init(PWM1_MODULE0_CHB_D13,16000,0);   //right_back  negative
	gpio_init(C7, GPO, 0, GPO_PUSH_PULL);
    pwm_init(PWM2_MODULE0_CHA_C6, 17000, 0);
    gpio_init(C9, GPO, 0, GPO_PUSH_PULL);
    pwm_init(PWM2_MODULE1_CHA_C8, 17000, 0); 
    
    gpio_init(D2, GPO, 0, GPO_PUSH_PULL);
    pwm_init(PWM2_MODULE3_CHB_D3, 17000, 0);
    gpio_init(C10, GPO, 0, GPO_PUSH_PULL);
    pwm_init(PWM2_MODULE2_CHB_C11, 17000,0); 
	
	
//    pwm_init(PWM1_MODULE3_CHB_D1, 330,  1500);//1500(8200)-4900
//    pwm_init(PWM1_MODULE3_CHA_D0, 330,  5500);//4000-8000
//    pwm_init(PWM4_MODULE2_CHA_C30, 330, 7500);//7700-2000
//    pwm_init(PWM1_MODULE2_CHB_D17, 330, 8100);//8100-6600
	
	
    pwm_init(PWM1_MODULE3_CHB_D1, 330,  4900);
    pwm_init(PWM1_MODULE3_CHA_D0, 330,  7000);
    pwm_init(PWM4_MODULE2_CHA_C30, 330, 3500);
    pwm_init(PWM1_MODULE2_CHB_D17, 330, 6800);	




	gpio_init(D4, GPO, 0, GPO_PUSH_PULL);
  /************************PWM电磁铁初始化**************************/
//	pwm_init(IRON0,16000,0);       //吸盘
//	pwm_init(IRON1,16000,0);			 //左
//	pwm_init(IRON2,16000,0);       //前
//	pwm_init(IRON3,16000,0);       //后
//	pwm_init(IRON4,16000,0);       //右
	
//	pwm_init(STEER1,50,3700);      //上        release: steer1:4500   steer2:3700
//	pwm_init(STEER2,50,3700);      //下        grab: steer1:2000   steer2:1100
//	pwm_init(STEER1,50,2000);      //上        release: steer1:4000   steer2:3600
//	pwm_init(STEER2,50,1800);
//	pwm_init(STEER0,50,4000);      //转盘 0=1000->360=6400(逆时针) 前=4000,右=5300,后=1500,右=2700,中=3400;
	//4000:Initialization    3200:left   1500: hou    4800:right  
	//classification: right: 1.1800 2.3440 3.5200    left: 1.4400 2.1100 3.2700
	//1->left 2->right 3->after(hou) : relative to box
	/************************编码器初始化**************************/
//	qtimer_quad_init(QTIMER_1, QTIMER1_TIMER0_C0, QTIMER1_TIMER1_C1);    //left_front
//	qtimer_quad_init(QTIMER_3, QTIMER3_TIMER2_B18, QTIMER3_TIMER3_B19);  //right_front
//	qtimer_quad_init(QTIMER_1, QTIMER1_TIMER2_C2, QTIMER1_TIMER3_C24);   //left_back
//	qtimer_quad_init(QTIMER_2, QTIMER2_TIMER0_C3, QTIMER2_TIMER3_C25);   //right_back
	encoder_dir_init(QTIMER1_ENCODER1, QTIMER1_ENCODER1_CH1_C0, QTIMER1_ENCODER1_CH2_C1);
	encoder_dir_init(QTIMER1_ENCODER2, QTIMER1_ENCODER2_CH1_C2, QTIMER1_ENCODER2_CH2_C24);
	encoder_dir_init(QTIMER2_ENCODER1, QTIMER2_ENCODER1_CH1_C3, QTIMER2_ENCODER1_CH2_C4);
	encoder_dir_init(QTIMER2_ENCODER2, QTIMER2_ENCODER2_CH1_C5, QTIMER2_ENCODER2_CH2_C25);
	/*************************按键GPIO初始化**********************/
//	gpio_init(B9, GPO, 0, GPIO_PIN_CONFIG);												
//	gpio_init(C4, GPI, 0, GPIO_PIN_CONFIG);
//	gpio_init(C5, GPI, 0, GPIO_PIN_CONFIG);
//	gpio_init(B15, GPI, 0, GPIO_PIN_CONFIG);
//	gpio_init(B14, GPI, 0, GPIO_PIN_CONFIG);
//	gpio_init(B16, GPI, 0, GPIO_PIN_CONFIG);
	
	/************************icm20602初始化*********************/
//	icm20602_init_spi();
   imu963ra_init();
	gyroOffset_init();
	mt9v03x_init();
uart_init(UART_1, 115200, UART1_TX_B12, UART1_RX_B13);
uart_init(UART_4, 115200, UART4_TX_C16, UART4_RX_C17);
	/***************************串口初始化***********************/
//	uart_init(USART_1,115200,UART1_TX_B12,UART1_RX_B13); 
//	mini_for_classfiy_init();
//	mini_for_orientation_init();
//	mini_for_orientation_init();
  /***********************/
//	pit_interrupt_ms(PIT_CH0, tim_0);
//	pit_interrupt_ms(PIT_CH1, tim_1);
//	pit_interrupt_ms(PIT_CH2, tim_2);

/****************************speaker*****************************/
//void speak(void)
//{
//		gpio_toggle(D4);
//		systick_delay_ms(300);
//		gpio_toggle(D4);
//}
/**********************串口中断初始化*************************/
//void mini_for_classfiy_init(void)																//分类art_mini 初始化
//{

//		mini_receivexfer.dataSize = 1;
//    mini_receivexfer.data = &mini_rx_buffer; 										//配置串口接收的缓冲区及缓冲区长度

//		uart_set_handle(USART_8, &mini_g_lpuartHandle, artmini_callback, NULL, 0, mini_receivexfer.data, 1); //设置中断函数及其参数
//   
//    NVIC_SetPriority(LPUART8_IRQn,4);   
//    uart_rx_irq(USART_8,1);
//}

//void mini_for_orientation_init(void)														//位置矫正art_mini初始化
//{
//	uart_init(USART_3,115200,UART3_TX_B22,UART3_RX_B23);	
//	
//		mini_receivexfer_tag.dataSize = 1;
//    mini_receivexfer_tag.data = &mini_rx_buffer_tag; 						//配置串口接收的缓冲区及缓冲区长度

//		uart_set_handle(USART_3,&mini_g_lpuartHandle_tag, mini_uart_callback_tag, NULL, 0, mini_receivexfer_tag.data, 1); //设置中断函数及其参数
//   
//    NVIC_SetPriority(LPUART3_IRQn,3);   
//    uart_rx_irq(USART_3,1);
//}
/****************************坐标点复刻版********************************/
//double aim[20][2] = {0};
//void coord_trans(void)
//{
//	for(int i = 0;i<city_num;i++)
//	{
//		aim[i][0] = (double)(try_coord[i][0]);
//		aim[i][1] = (double)(try_coord[i][1]);
//	}
//}

/***************************过滤零点坐标***********************************/
//double filtered[12][2] = {0};
//void coord_filter(void)
//{
//	for(int i=0;i<20;i++)
//	{
//		if(aim[i][0]!=0&&aim[i][1]!=0)
//		{
//			filtered[i][0] = aim[i][0];
//			filtered[i][1] = aim[i][1];
//		}
//		else
//		{
//			continue;
//		}
//	}
//}



    uart_rx_interrupt(UART_1, ZF_ENABLE);
    interrupt_set_priority(LPUART1_IRQn, 0); 
    uart_rx_interrupt(UART_4, ZF_ENABLE);
    interrupt_set_priority(LPUART4_IRQn, 0);  

    pit_ms_init(PIT_CH1, tim_1);
  	pit_ms_init(PIT_CH0, tim_0);
	pit_ms_init(PIT_CH2, tim_0);
//	NVIC_SetPriority(PIT_IRQn, 2); // 设置PIT_CH0中断优先级
//	    // 设置PIT_CH0中断优先级
//    NVIC_SetPriority(PIT0_IRQn, 1); // 设置优先级为1（数值越低优先级越高）
//    NVIC_EnableIRQ(PIT0_IRQn);      // 使能PIT_CH0中断

//    // 设置PIT_CH1中断优先级
//    NVIC_SetPriority(PIT1_IRQn, 2); // 设置优先级为2（数值越低优先级越高）
//    NVIC_EnableIRQ(PIT1_IRQn);      // 使能PIT_CH1中断
}
/***********************卡尔曼滤波***************************/
int Kalman_front_left(int data)
{
  static int p1=6;
  static int c1=5;
  int q=5;
  int r=10;
  float kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
int Kalman_front_right(int data)
{
  static int p1=6;
  static int c1=5;
  int q=5;
  int r=10;
  float kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
int Kalman_back_left(int data)
{
  static int p1=6;
  static int c1=5;
  int q=5;
  int r=10;
  float kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
int Kalman_back_right(int data)
{
  static int p1=6;
  static int c1=5;
  int q=5;
  int r=10;
  float kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
double Kalman_x(double data)
{
  static double p1=6;
  static double c1=5;
  int q=5;
  int r=10;
  double kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
double Kalman_y(double data)
{
  static double p1=6;
  static double c1=5;
  int q=5;
  int r=10;
  double kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
double Kalman_x2(double data)
{
  static double p1=6;
  static double c1=5;
  int q=5;
  int r=10;
  double kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
double Kalman_y2(double data)
{
  static double p1=6;
  static double c1=5;
  int q=5;
  int r=10;
  double kg,c2,p2;
  
	
  p2=p1+q;
  kg=p2*p2/(p2*p2+r*r);
  c2=c1+kg*(data-c1);
  p1=(1-kg)/p2;
  c1=c2;

  return c1; 
}
