#include "zf_common_headfile.h"

int  myabs(int dat)
{
    if(dat>=0)  return dat;
    else        return -dat;
}

double  myabs_double(double dat)
{
    if(dat>=0)  return dat;
    else        return -dat;
}

float limit(float x, int32 y)
{
    if(x>y)             return y;
    else if(x<-y)       return -y;
    else                return x;
}

float v1,v2,v3,v4;
int DIR1,DIR2,DIR3,DIR4;

extern int state_right;
extern int x_pic_right,y_pic_right;

/*************************************/
/************速度PID处理函数***********/
/*************************************/
#define KP 0
#define KI 1
#define KD 2

double speed_target[4] = {0,0,0,0};					//期望速度
double speed_out[4] = {0,0,0,0};            //速度pid后产生PWM输出
float speed_error0[4] = {0,0,0,0};          //速度偏差
float speed_error1[4] = {0,0,0,0};          //上一次速度偏差
float speed_error2[4] = {0,0,0,0};          //上上次速度偏差
//float speed_pid1[3]={23,0.2,40};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
//float speed_pid2[3]={20,0.2,20};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
//float speed_pid3[3]={21,0.2,40};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
//float speed_pid4[3]={20,0.2,40};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
//40,0.69
float speed_pid1[3]={20,0.2,0};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
float speed_pid2[3]={20,0.2,0};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
float speed_pid3[3]={20,0.22,0};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
float speed_pid4[3]={20,0.22,0};           //10,0.9    50,0.72   35,0.71  37,0.71  100 0.45
double speed_pid_left_front(int16 real_s)
{
	speed_error0[0]=speed_target[0]-real_s;  //计算初始偏差
	speed_out[0]=(speed_pid1[KP]*(speed_error0[0]-speed_error1[0])+speed_pid1[KI]*speed_error0[0]+speed_pid1[KD]*(speed_error0[0]-2*speed_error1[0]+speed_error2[0]));  //计算pid输出值
	speed_error2[0]=speed_error1[0];//更新上上次偏差
	speed_error1[0]=speed_error0[0];//更新上次偏差
 
	return speed_out[0];
}

double speed_pid_right_front(int16 real_s)
{
	speed_error0[1]=speed_target[1]-real_s;  //计算初始偏差
	speed_out[1]=(speed_pid2[KP]*(speed_error0[1]-speed_error1[1])+speed_pid2[KI]*speed_error0[1]+speed_pid2[KD]*(speed_error0[1]-2*speed_error1[1]+speed_error2[1]));  //计算pid输出值
	speed_error2[1]=speed_error1[1];//更新上上次偏差
	speed_error1[1]=speed_error0[1];//更新上次偏差
	return speed_out[1];
}

double speed_pid_right_back(int16 real_s)
{
	speed_error0[2]=speed_target[2]-real_s;  //计算初始偏差
	speed_out[2]=(speed_pid3[KP]*(speed_error0[2]-speed_error1[2])+speed_pid3[KI]*speed_error0[2]+speed_pid3[KD]*(speed_error0[2]-2*speed_error1[2]+speed_error2[2]));  //计算pid输出值
	speed_error2[2]=speed_error1[2];//更新上上次偏差
	speed_error1[2]=speed_error0[2];//更新上次偏差
	return speed_out[2];
}

double speed_pid_left_back(int16 real_s)
{
	speed_error0[3]=speed_target[3]-real_s;  //计算初始偏差
	speed_out[3]=(speed_pid4[KP]*(speed_error0[3]-speed_error1[3])+speed_pid4[KI]*speed_error0[3]+speed_pid4[KD]*(speed_error0[3]-2*speed_error1[3]+speed_error2[3]));  //计算pid输出值
	speed_error2[3]=speed_error1[3];//更新上上次偏差
	speed_error1[3]=speed_error0[3];//更新上次偏差
	return speed_out[3];
}

///*****************************************************/
///***************编码器读值函数************************/
///*****************************************************/
int16 encoder_real[4] = {0,0,0,0};
double coord_now[3] = {2,0,0};
double coord_temp[2] = {0,0};
double coord_sum[2] = {0,0};
double the_sin = 0;
double the_cos = 0;

//#define ppm		 28650.4f//108575.5f//63722.62845f//70802.9205f//20000.825f//108575.5f //58575.5f//28634.4f
//#define distance_x 41992.65540458f     //48992.65540458f
//#define distance_y 31380.5751553f      //45380.5751553f 

////参数左下
#define distance_x4  48992.65540458f
#define distance_y4  45380.5751553f 

////参数右下
#define distance_x5  41992.65540458f 
#define distance_y5  31380.5751553f  

//// 参数1          (2,3)->(2,25)           (2,3)->(2,4)   		 (2,3)->(2,5)        (2,3) -> (2,9)   (2,3)->(2,17)
#define distance_x1 41922.65540458f     //	
#define distance_y1 36380.5751553f     // 	38380.5751553f 		36380.5751553f       34380.5751553f   31380.5751553f 

////// 参数2
//#define distance_x2 37692.65540458f     //阶段2    2,3->12,14   (x1-x2)^2 + (y1-y2)^2 <= 221
//#define distance_y2 33880.5751553f      //阶段2    2,3->12,14

////// 参数3
//#define distance_x3 35192.65540458f     //阶段3     2,3->19,20   (x1-x2)^2 + (y1-y2)^2 <= 578
//#define distance_y3 32980.5751553f      //阶段3     2,3->19,20

#define pi     3.1415926f

double y_distance(float y)
{
	double y_value;
	if(fabs(y) <= 4)
		y_value = 35780.5751553f;
	else if (fabs(y) <= 8)
		y_value = 32580.5751553f;
	else if(fabs(y) <= 12)
		y_value = 31880.5751553f; //32180.5751553f
	else if(fabs(y) <= 14)
		y_value = 31380.5751553f;
	else
		y_value = 30980.5751553f; // This is estimated value
	
	return y_value;
}

double x_distance(float x)
{
	double x_value;
	if(fabs(x) <= 7)
			x_value = 41922.65540458f;
	else if(fabs(x) <= 9)
		x_value = 37522.65540458f; //39922.65540458f;
	else if(fabs(x) <= 11)
		x_value = 35822.65540458f;
	else if(fabs(x) <= 12)
		x_value = 34522.65540458f;
	else if(fabs(x) <= 15)
		x_value = 33922.65540458f;
	else 
		x_value = 35922.65540458f; // This is estimated value
	return x_value;
}

double dis_x, dis_y;

void encoder(void)
{
	encoder_real[0] = -encoder_get_count(QTIMER1_ENCODER1);
	encoder_real[2] = -encoder_get_count(QTIMER1_ENCODER2);
	encoder_real[3] = encoder_get_count(QTIMER2_ENCODER1);
	encoder_real[1] = encoder_get_count(QTIMER2_ENCODER2);
	
	encoder_clear_count(QTIMER1_ENCODER1);
	encoder_clear_count(QTIMER1_ENCODER2);
	encoder_clear_count(QTIMER2_ENCODER1);
	encoder_clear_count(QTIMER2_ENCODER2);
	 
	encoder_real[0] = (int16)(Kalman_front_left(encoder_real[0])*12/tim_0);
	encoder_real[1] = (int16)(Kalman_front_right(encoder_real[1])*12/tim_0);
	encoder_real[3] = (int16)(Kalman_back_left(encoder_real[3])*12/tim_0);
	encoder_real[2] = (int16)(Kalman_back_right(encoder_real[2])*12/tim_0);
	
//	encoder_real[0] = (int16)((encoder_real[0])*12/tim_0);
//	encoder_real[1] = (int16)((encoder_real[1])*12/tim_0);
//	encoder_real[3] = (int16)((encoder_real[3])*12/tim_0);
//	encoder_real[2] = (int16)((encoder_real[2])*12/tim_0);
	
	
	
//	//temporarily for now
//	if(sport_state == 1 || sport_state == 0)
//	{
//		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[2]-encoder_real[3])/distance_x1);
//		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/distance_y1);
//	}
//	else
//	{
//		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[2]-encoder_real[3])/dis_x);
//		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/dis_y);
//	}
	
		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[3]-encoder_real[2])/distance_x1);
		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/distance_y1);

	
	
//	if(coord_error[0] <= 221 &&coord_error[0] > 50)
//	{
//		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[2]-encoder_real[3])/distance_x2);
//		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/distance_y2);
//	}
//	
//	if(coord_error[0] <= 578 &&coord_error[0] > 221)
//	{
//		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[2]-encoder_real[3])/distance_x3);
//		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/distance_y3);
//	}
	
//	if(coord_error[1] <= 0  &&coord_error[2] <= 0)
//	{
//		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[2]-encoder_real[3])/distance_x4);
//		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/distance_y4);
//	}
//	
//	if(coord_error[0] <= 221 &&coord_error[0] >= 50)
//	{
//		coord_temp[0] = (double)((encoder_real[0]-encoder_real[1]+encoder_real[2]-encoder_real[3])/distance_x5);
//		coord_temp[1] = (double)((encoder_real[0]+encoder_real[1]+encoder_real[2]+encoder_real[3])/distance_y5);
//	}
	
	
	
	//temporarily for now
	the_sin = sin(coord_now[2] /180*pi);
	the_cos = cos(coord_now[2] /180*pi);
	
	//coord_now[1] += (double)((the_cos*coord_temp[1])-(the_sin*coord_temp[0]));                   //x
	//coord_now[0] += (double)((the_cos*coord_temp[0])+(the_sin*coord_temp[1]));                   //y
	coord_now[0] +=coord_temp[0];
  coord_now[1] +=coord_temp[1];
}

/*****************************************************/
/***************角度获取***************************/
/*****************************************************/
double LowPass_gyro(float data_new, float data_last, float data_pre)
{
    return 0.5f * data_new + 0.3f * data_last + 0.2f * data_pre;
}
float z_gyro_pre = 0, z_gyro_last = 0, z_gyro_now = 0, z_gyro = 0, yaw1 = 0, yaw_angle = 0;
float dt_gyro = 0.004;
int yaw_angle_flag=0;
void Self_posture()                                  // should be in interruput
{
//    z_gyro_pre = z_gyro_last;
//    z_gyro_last = z_gyro;
//		imu963ra_get_gyro();
//    z_gyro_now = (float)imu963ra_gyro_z - GyroOffset.Zdata;             
//    z_gyro = LowPass_gyro(z_gyro_now, z_gyro_last, z_gyro_pre);
//    yaw1 = (float)(z_gyro / 14.3f) * dt_gyro;                         //imu 14.3   icm 16.4
//    if(fabs(yaw1) < 0.01) yaw1 = 0;
//    yaw_angle -= yaw1;  

	if(ImageFlag.Zebra_Flag==0)
	{
		if(cross_flag==2&&yaw_angle_flag==0)
		{
			yaw_angle=0;
			yaw_angle_flag=1;
		}
		
		if(ImageFlag.image_element_rings!=0&&yaw_angle_flag==0)//测试
		{
			yaw_angle=0;
			yaw_angle_flag=1;
		}
		if(coord_now_flag==1)
		{
			yaw_angle=yaw_angle-360;
			coord_now_flag=2;
		}
		if(coord_now_flag_right==1)
		{
			yaw_angle=yaw_angle+360;
			coord_now_flag_right=2;
		}
	}
    z_gyro_pre = z_gyro_last;
    z_gyro_last = z_gyro;
	imu963ra_get_gyro();
    z_gyro_now = (float)imu963ra_gyro_z;             
    z_gyro = LowPass_gyro(z_gyro_now, z_gyro_last, z_gyro_pre);
    yaw1 = (float)(z_gyro / 14.3f) * dt_gyro;                         //imu 14.3   icm 16.4
    if(fabs(yaw1) < 0.01) yaw1 = 0;
    yaw_angle -= yaw1;   
	
//    if (yaw_angle>=0&&yaw_angle<360) 
//		{
//        yaw_angle=yaw_angle;
//    } 
//		else if (yaw_angle< 0) 
//		{
//      yaw_angle= 360+yaw_angle;
//    }	
//		else if(yaw_angle>360)
//		{
//			yaw_angle=yaw_angle-360;
//		}
//		ips200_show_float(80, 60, yaw_angle , 5,5);
//		ips200_show_float(80, 80, GyroOffset.Zdata , 5,5);
}

void accgyro_to_coord(void)
{
//	  ICM_getEulerianAngles();
//	coord_now[2] = eulerAngle.yaw;
	  Self_posture();
	  coord_now[2] = yaw_angle;
}
double get_aim_groy(double *aim_coord)
{
	double aim_angle = 0;
	double e_l = 0,e_f =0;
	e_l =coord_now[0] -aim_coord[0];
	e_f =coord_now[1] -aim_coord[1];
	aim_angle = (90.0-(atan(e_f/e_l)*180/pi));
	return aim_angle;
}

/*****************************************************/
/***************电机控制函数**************************/
/*****************************************************/
double duty_temp[4] = {0,0,0,0};

void motor_control(void)
{
	
	
	       duty_temp[0] += speed_pid_left_front(encoder_real[0]);
	       duty_temp[1] += speed_pid_right_front(encoder_real[1]);
	       duty_temp[2] += speed_pid_right_back(encoder_real[2]);
	       duty_temp[3] += speed_pid_left_back(encoder_real[3]);

		 
	     
	     
	     
	       //1
         if(duty_temp[0]<0)
         {
         v1 = -duty_temp[0];
          DIR1=0;		
         }
         else
         {
         v1=	duty_temp[0];
         DIR1=1;
				 }
				 
				 //2
         if(duty_temp[1]<0)
         {
          v2= -duty_temp[1];
          DIR2=0;		
         }
         else
         {
         v2=duty_temp[1];
         DIR2=1;
         }
				 
         //3
         if(duty_temp[2]<0)
         {
          v3= -duty_temp[2];
          DIR3=0;		
         }
         else
         {
         v3=duty_temp[2];
         DIR3=1;
         }
				 
         //4
         if(duty_temp[3]<0)
         {
          v4= -duty_temp[3];
          DIR4=0;		
         }
         else
         {
         v4=duty_temp[3];
         DIR4=1; 
         }	
				 
				 
         if(v1>6000)
         {
         v1=6000;
         }
         if(v2>6000)
         {
         v2=6000;
         }
         if(v3>6000)
         {
         v3=6000;
         }
         if(v4>6000)
         {
         v4=6000;
         }

		 
		
		 
         pwm_set_duty(PWM2_MODULE0_CHA_C6, v4);
         pwm_set_duty(PWM2_MODULE1_CHA_C8, v3);
         pwm_set_duty(PWM2_MODULE3_CHB_D3, v2);
         pwm_set_duty(PWM2_MODULE2_CHB_C11, v1);
         
         gpio_set_level(C10, DIR1);
         gpio_set_level(D2, DIR2);
         gpio_set_level(C9, DIR3);
         gpio_set_level(C7, DIR4);
         
}
/*****************************************************/
/***************位置式PID*****************************/
/*****************************************************/
int16 enlarge_k = 10;

//float location_pid_x[4]={30,0.0,200};
//float location_pid_y[4]={30,0.0,200};



float location_pid_out = 0.0;

float er_kp = 0.0;
float er_kd = 0.0;

double x_speed = 0;
double y_speed = 0;

double coord_error0[3] = {0,0,0};
double coord_error1[3] = {0,0,0};
double coord_er0[3] = {0,0,0};
double coord_er1[3] = {0,0,0};
double error_add[3] = {0,0,0};

float angle = 0;

float vector_x = 0;
float vector_y = 0;
float angle_z = 0;

float location_pid_x[4]={80,0,10};
float location_pid_y[4]={80,0,10};

float location_x_pid(double *coord_now,double *coord_aim)
{
//	static float x_speed,error,last_error,error_sum;
	coord_error0[0] = coord_aim[0] - coord_now[0];
	error_add[0] += coord_error0[0];
	x_speed = location_pid_x[KP]*coord_error0[0] + location_pid_x[KI]*error_add[0] +location_pid_x[KD]*(coord_error0[0] - coord_error1[0]);
	coord_error1[0] = coord_error0[0];
	x_speed = limit(x_speed,12);
	return x_speed;
}

float location_y_pid(double *coord_now,double *coord_aim)
{
//	static float y_speed,error,last_error,error_sum;
	coord_error0[1] = coord_aim[1] - coord_now[1];
	error_add[1] += coord_error0[1];
	y_speed = location_pid_y[KP]*coord_error0[1] + location_pid_y[KI]*error_add[1] +location_pid_y[KD]*(coord_error0[1] - coord_error1[1]);
	coord_error1[1] = coord_error0[1];
	y_speed = limit(y_speed,12);
	return y_speed;
}


//float location_x_pid_pic(int coord_now,int coord_aim)
//{
////	static float x_speed,error,last_error,error_sum;
//	static double pic_error0,pic_aim,pic_now,pic_error_add,pic_error1;
//	pic_error0 = pic_aim - pic_now;
//	pic_error_add += pic_error0;
//	x_speed = location_pid_x[KP]*pic_error0 + location_pid_x[KI]*pic_error_add +location_pid_x[KD]*(pic_error0 - pic_error1);
//	pic_error1 = pic_error0;
//	x_speed = limit(x_speed,200);
//	return x_speed;
//}

//float location_y_pid_pic(int coord_now,int coord_aim)
//{
////	static float y_speed,error,last_error,error_sum;
//	static double pic_error0,pic_aim,pic_now,pic_error_add,pic_error1;
//	pic_error0 = pic_aim - pic_now;
//	pic_error_add += pic_error0;
//	x_speed = location_pid_x[KP]*pic_error0 + location_pid_x[KI]*pic_error_add +location_pid_x[KD]*(pic_error0 - pic_error1);
//	pic_error1 = pic_error0;
//	y_speed = limit(y_speed,200);
//	return y_speed;
//}
/*****************************************************/
/***************角度环PID*****************************/
/*****************************************************/
double aim_angle = 0;
//float pid_turn[3]={1,0.0,150};

//float pid_turn[3]={0.4,0,0};//xun0.2

float pid_turn[3]={0.7,0,0.7};//pic//0.7  0.45

//int16 turn_pid(float now_angle,float aim) 
//{
//	
//	static float angle_pwm=0,angle_error0=0,angle_error1=0,integeral_error=0;
//	angle_error0 = aim - now_angle;
//	if(angle_error0<=-180)
//	{
//		angle_error0 = (360+angle_error0);
//	}
//	else if(angle_error0>-180)
//	{
//		angle_error0 = angle_error0;
//	}
//	integeral_error += angle_error0;
//	angle_pwm = pid_turn[KP]*angle_error0 + pid_turn[KD]*(angle_error0 - angle_error1);
//	angle_error1 = angle_error0;
//	angle_pwm = limit((int16)angle_pwm,60);
//		ips200_show_int(80, 80, -angle_pwm , 5);
//	return -angle_pwm;
//}
float turn_pid(float now_angle,float aim)   //  -    left    20
{
	static float angle_pwm=0,angle_error0=0,angle_error1=0,integeral_error=0;
	angle_error0 = aim - now_angle;
	integeral_error += angle_error0;
	angle_pwm = pid_turn[KP]*angle_error0 + pid_turn[KD]*(angle_error0 - angle_error1);
	angle_error1 = angle_error0;
	angle_pwm = limit(angle_pwm,8);
    if(myabs_double(angle_error0)<1)
	{
		angle_pwm=0;
	}
	return -angle_pwm;
}

/*****************************************************/
/***************速度矢量和****************************/
/*****************************************************/
void target_flash(double x,double y,double z)
{
	speed_target[0] = (3*y + 3*x + 10.5*z);
	speed_target[1] = (3*y - 3*x - 10.5*z);
	speed_target[3] = (3*y + 3*x - 10.5*z);
	speed_target[2] = (3*y - 3*x + 10.5*z);
} 
void target_flash_lowspeed(double x,double y,double z)
{
	speed_target[0] =(2.5*y + 2.5*x + 10.5*z);
	speed_target[1] =(2.5*y - 2.5*x - 10.5*z);
	speed_target[3] =(2.5*y + 2.5*x - 10.5*z);
	speed_target[2] =(2.5*y - 2.5*x + 10.5*z);
}
void target_flash_diss(double x,double y,double z)
{
	speed_target[0] = (2.1*y + 2.1*x + 10*z);
	speed_target[1] = (2.1*y - 2.1*x - 10*z);
	speed_target[3] = (2.1*y + 2.1*x - 10*z);
	speed_target[2] = (2.1*y - 2.1*x + 10*z);
}
void speed_coord(double *coord_now,double *coord_aim)
{
	float pid_turn[3]={0.3,0,0};//pic
	vector_x = (location_x_pid(coord_now,coord_aim));
	vector_y = (location_y_pid(coord_now,coord_aim));
	angle_z = turn_pid(coord_now[2],encoder_pic_angle);
	target_flash(vector_x,vector_y,angle_z);	
}

void speed_coord_xun()
{
//	float pid_turn[3]={1.5,0,5};//xun0.2	
	vector_x = 0;
//	vector_y = 20;	
	pid_turn[0]=0.5;
	pid_turn[1]=0;
	pid_turn[2]=0.8;

    angle_z = turn_pid(ImageStatus.Det_True,0);
	target_flash(vector_x,vector_y,angle_z);
	
}
/*********************error position pid***********************************/
//float pid_pic_location_x[3]={2.7,0,6.5};   //4.65    1.84,0,4      1.95,0,5  2.0,0,5.5
//float pid_pic_location_y[3]={2.7,0,6.5};   //4.65    1.84,0,4     1.95,0,5   2.0,0,5.5

double pid_pic_location_x[3] = {0.0005, 0, 0};  // 降低 KP 值
double pid_pic_location_y[3] = {0.0005, 0, 0};  // 降低 KP 值
float pic_vector_x = 0;
float pic_vector_y = 0;
float pic_angle_z = 0;

#define MAX_INTEGRAL 100.0


float pic_x_pid(double error_X) {
    static float pic_x, pic_error_x, pic_last_error_x, pic_error_sum_x;
    pic_error_x = -error_X;
    
    pic_error_sum_x += pic_error_x;
    
    // 限制积分项范围
    if (pic_error_sum_x > MAX_INTEGRAL) {
        pic_error_sum_x = MAX_INTEGRAL;
    } else if (pic_error_sum_x < -MAX_INTEGRAL) {
        pic_error_sum_x = -MAX_INTEGRAL;
    }
    
    pic_x = pid_pic_location_x[KP] * pic_error_x + pid_pic_location_x[KI] * pic_error_sum_x + pid_pic_location_x[KD] * (pic_error_x - pic_last_error_x);
    pic_last_error_x = pic_error_x;
    
    pic_x = limit(pic_x, 20);
    
    return pic_x;
}

float pic_y_pid(double error_Y) {
    static float pic_y, pic_error_y, pic_last_error_y, pic_error_sum_y;
    pic_error_y = error_Y;
    
    pic_error_sum_y += pic_error_y;
    
    // 限制积分项范围
    if (pic_error_sum_y > MAX_INTEGRAL) {
        pic_error_sum_y = MAX_INTEGRAL;
    } else if (pic_error_sum_y < -MAX_INTEGRAL) {
        pic_error_sum_y = -MAX_INTEGRAL;
    }
    
    pic_y = pid_pic_location_y[KP] * pic_error_y + pid_pic_location_y[KI] * pic_error_sum_y + pid_pic_location_y[KD] * (pic_error_y - pic_last_error_y);
    pic_last_error_y = pic_error_y;
    
    pic_y = limit(pic_y, 20);
    
    return pic_y;
}

//float pid_pic_location_x[3]={0.1,0,0};   //4.65    1.84,0,4      1.95,0,5  2.0,0,5.5
//float pid_pic_location_y[3]={0.1,0,0};   //4.65    1.84,0,4     1.95,0,5   2.0,0,5.5

//float pic_vector_x = 0;
//float pic_vector_y = 0;
//float pic_angle_z = 0;
//float pic_x_pid(double error_X)
//{
//	static float pic_x,pic_error_x,pic_last_error_x,pic_error_sum_x;
//	pic_error_x = -error_X;
//	
//	//pic_error_sum_x += pic_error_x;
//	pic_x = pid_pic_location_x[KP]*pic_error_x + pid_pic_location_x[KI]*pic_error_sum_x +pid_pic_location_x[KD]*(pic_error_x - pic_last_error_x);
//	pic_last_error_x = pic_error_x;
//	ips200_show_int(180, 60,  pic_x , 5);
//	//pic_x = limit(pic_x,20);
//	return pic_x;
//	
//}

//float pic_y_pid(double error_Y)
//{
//	static float pic_y,pic_error_y,pic_last_error_y,pic_error_sum_y;
//	pic_error_y = error_Y;
//	
//	pic_error_sum_y += pic_error_y;
//	pic_y = pid_pic_location_y[KP]*pic_error_y + pid_pic_location_y[KI]*pic_error_sum_y +pid_pic_location_y[KD]*(pic_error_y - pic_last_error_y);
//	pic_last_error_y = pic_error_y;
//	pic_y = limit(pic_y,20);
//	//ips200_show_int(180, 80,  pic_y , 5);
//	return pic_y;

//}
///*********************************pic error clean*******************************/
//float pic_angle_kp = 20;
//float pic_angle_kd = 20;
///***************************************error angle pid******************************************/
//int16 turn_error_pid(double angle_turn)
//{
//	static float angle_pwm_pic=0,angle_error_pic0=0,angle_error_pic1=0,pic_integeral_error=0;
//	angle_error_pic0 = angle_turn;
//	if(angle_error_pic0<0)
//	{
//		angle_error_pic0 = 360+angle_error_pic0;
//	}
//	else if(angle_error_pic0>0)
//	{
//		angle_error_pic0 = angle_error_pic0;
//	}
//	pic_integeral_error += angle_error_pic0;
//	angle_pwm_pic = pic_angle_kp*angle_error_pic0 + pic_angle_kd*(angle_error_pic0 - angle_error_pic1);
//	angle_error_pic1 = angle_error_pic0;
//	angle_pwm_pic = limit((int16)angle_pwm_pic,120);
//	return angle_pwm_pic;
//}

/**************************************sum vecter velocity**************************************/
//void pic_speed_coord(double Error_x,double Error_y,double er_angle)
//{
//	pic_vector_x = (pic_x_pid(Error_x));
//	pic_vector_y = (pic_y_pid(Error_y));
//	pic_angle_z = turn_pid(coord_now[2],0);
//	target_flash_diss(pic_vector_x,pic_vector_y,pic_angle_z);
//}
void pic_speed_coord(double Error_x,double Error_y,double er_angle)//保持车身正直
{
	pic_angle_z = turn_pid(coord_now[2],encoder_pic_angle);	
	target_flash_diss(0,0,pic_angle_z);
}

void speed_coord_color()                                           //矩形识别确定位置(左边)
{
	pic_vector_x = (pic_x_pid(-y_pic));
	pic_vector_y = (pic_y_pid(x_pic));
	if(myabs(y_pic)<7&&myabs(x_pic)<7)
	{
		pic_vector_x=0;
		pic_vector_y=0;
	}
	coord_aim[0] = coord_now[0]+pic_vector_x;          //
	coord_aim[1] = coord_now[1]+pic_vector_y;	         //回到原点	
	speed_coord(coord_now,coord_aim);
	//angle_z = turn_pid(coord_now[2],encoder_pic_angle);	
	//target_flash_diss(pic_vector_x,pic_vector_y,angle_z);		
}


void speed_coord_color_right()                                           //矩形识别确定位置(右边)
{
	
	pic_vector_x = (pic_x_pid(y_pic_right));
	pic_vector_y = (pic_y_pid(-x_pic_right));
	if(myabs(y_pic_right)<7&&myabs(x_pic_right)<7)
	{
		pic_vector_x=0;
		pic_vector_y=0;
	}
	coord_aim[0] = coord_now[0]+pic_vector_x;          //
	coord_aim[1] = coord_now[1]+pic_vector_y;	         //回到原点	
	speed_coord(coord_now,coord_aim);
	

	//angle_z = turn_pid(coord_now[2],encoder_pic_angle);	
	//target_flash_diss(pic_vector_x,pic_vector_y,angle_z);		
}


void test_angle(double angle)
{
	double z;
	pid_turn[0] = 5;
	pid_turn[1] = 150;
	z=turn_pid(coord_now[2],angle);
	speed_target[0] =+z;
	speed_target[1] =-z;
	speed_target[2] =-z; 
	speed_target[3] =+z;
}

void stop(void)
{
	speed_target[0] = 0;
	speed_target[1] = 0;
	speed_target[2] = 0;
	speed_target[3] = 0;
}

void back(void)
{
   speed_coord(coord_now,coord_aim);
}




void roate_shun()
{
	if(roate_determine_angle_flag==0)
	{
		roate_determine_angle=coord_now[2]-180;
		roate_determine_angle_flag=1;
	}
	pic_angle_z = turn_pid(coord_now[2],roate_determine_angle);	
	target_flash_diss(0,0,pic_angle_z/3);
}

void roate_ni()
{
	if(roate_determine_angle_flag==0)
	{
		roate_determine_angle=coord_now[2]+180;
		roate_determine_angle_flag=1;
	}

	pic_angle_z = turn_pid(coord_now[2],roate_determine_angle);	
	target_flash_diss(0,0,pic_angle_z/3);
}

int roate_tiaozhen_flag;//转向flag


void roate_tiaozhen()
{	
	vector_x = 0;
	pid_turn[0]=0.5;
	pid_turn[1]=0;
	pid_turn[2]=0.8;
	
	if(roate_tiaozhen_flag==0)
	{
		if(myabs(ImageStatus.Det_True)>2)
		{
			while(myabs(ImageStatus.Det_True)>2)
			{
				angle_z = turn_pid(ImageStatus.Det_True,0);
				target_flash(0,0,angle_z);
				roate_tiaozhen_flag=1;
			}
		}
		else
		{
			roate_tiaozhen_flag=1;
		}
	}
}