shoes_project/touch_floor_by_imu/motion/press_down_detect.c

#include "press_down_detect.h"


int press_down_detect_new(int index, uint16_t left_press, uint16_t right_press, float left_acc_z, float right_acc_z,
			float pitch_left, float pitch_right, uint8_t _rssi)
{
	 static int16_t valid_count = 0;

	
	 if(fabsf(left_acc_z - 1.0f) < 0.3f && fabsf(right_acc_z - 1.0f) < 0.3f)
	 {
		 valid_count ++;
	 }
	 else
	 {
		 valid_count  = 0;
	 }
	 
	 
	 int16_t valid_laser = 0;
	 
	 if(_rssi < 30)
	 {
		 if((left_press < 800 && left_press > 100) || (right_press > 100 && right_press < 800))
		 {
			 valid_laser = 1;
		 }
	 }
	 else
	 {
		 if((left_press < 800 && left_press > 100) && (right_press > 100 && right_press < 800))
		 {
			 valid_laser = 1;
		 }
	 }
	 
   if(valid_count >= 10 && valid_laser == 1) 
    return 1;
	 
	 return 0;
}



int cancel_down = 0;
int virus_flag  = 0;

float real_max_val_right = 0.0f;
float real_max_val_left  = 0.0f;

float real_max_front_left = 0.0f;
float real_max_front_right = 0.0f;

int press_down_detect_press(int index, float front_mag_left, float back_mag_left,
									float front_mag_right, float back_mag_right, int *front_down, int *back_down)
{
	  static float front_mag_norm_left[PRESS_COUNT_MAX];
    static float front_mag_norm_right[PRESS_COUNT_MAX];
	
		static float back_mag_norm_left[PRESS_COUNT_MAX];
    static float back_mag_norm_right[PRESS_COUNT_MAX];

	
	
		memcpy(front_mag_norm_left, front_mag_norm_left + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		front_mag_norm_left[PRESS_COUNT_MAX - 1] = front_mag_left;

		memcpy(back_mag_norm_left, back_mag_norm_left + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		back_mag_norm_left[PRESS_COUNT_MAX - 1] =  back_mag_left;

		memcpy(front_mag_norm_right, front_mag_norm_right + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		front_mag_norm_right[PRESS_COUNT_MAX - 1] = front_mag_right;

		memcpy(back_mag_norm_right, back_mag_norm_right + 1, (PRESS_COUNT_MAX - 1) * sizeof(float));

		back_mag_norm_right[PRESS_COUNT_MAX - 1] =  back_mag_right;
		

		int front_down_tmp = 0;
	  int back_down_tmp  = 0;
    

	 float left_max_front_val = front_mag_left;
	 float left_min_front_val = front_mag_left;
	 
	 int left_min_front_index = PRESS_COUNT_MAX - 1;
	 int left_max_front_index = PRESS_COUNT_MAX - 1;
	
	 float left_max_back_val = back_mag_left;
	 float left_min_back_val = back_mag_left;
	 
	 int left_min_back_index = PRESS_COUNT_MAX - 1;
	 int left_max_back_index = PRESS_COUNT_MAX - 1;
	 
	 float right_max_front_val = front_mag_right;
	 float right_min_front_val = front_mag_right;
	 
	 int right_min_front_index = PRESS_COUNT_MAX - 1;
	 int right_max_front_index = PRESS_COUNT_MAX - 1;
	
	 float right_max_back_val = back_mag_right;
	 float right_min_back_val = back_mag_right;
	 
	 int right_min_back_index = PRESS_COUNT_MAX - 1;
	 int right_max_back_index = PRESS_COUNT_MAX - 1;
	 
	
	 
	 for(int i =  0; i < PRESS_COUNT_MAX; i++)
	 {
			 if(front_mag_norm_left[i] > left_max_front_val)
			 {
					 left_max_front_val = front_mag_norm_left[i];
					 left_max_front_index = i;
			 }
			 
			 if(front_mag_norm_left[i] < left_min_front_val)
			 {
					 left_min_front_val = front_mag_norm_left[i];
					 left_min_front_index = i;
			 }
			 
			 if(back_mag_norm_left[i] > left_max_back_val)
			 {
					 left_max_back_val = back_mag_norm_left[i];
					 left_max_back_index = i;
			 }
			 
			 if(back_mag_norm_left[i] < left_min_back_val)
			 {
					 left_min_back_val = back_mag_norm_left[i];
					 left_min_back_index = i;
			 }
			 
			 if(front_mag_norm_right[i] > right_max_front_val)
			 {
					 right_max_front_val = front_mag_norm_right[i];
					 right_max_front_index = i;
			 }
			 
			 if(front_mag_norm_right[i] < right_min_front_val)
			 {
					 right_min_front_val = front_mag_norm_right[i];
					 right_min_front_index = i;
			 }
			 
			 if(back_mag_norm_right[i] > right_max_back_val)
			 {
					 right_max_back_val = back_mag_norm_right[i];
					 right_max_back_index = i;
			 }
			 
			 if(back_mag_norm_right[i] < right_min_back_val)
			 {
					 right_min_back_val = back_mag_norm_right[i];
					 right_min_back_index = i;
			 }
			 
		 }
		 //判断前蹲
		 /*
		 if(left_max_front_index < left_min_front_index && left_max_front_val  > left_min_front_val + 250.0f
			 && right_max_front_index < right_min_front_index && right_max_front_val  > right_min_front_val + 250.0f
			 && abs(right_min_front_index - left_min_front_index) < 4)
		 {
			 front_down_tmp = 1;
		 }
		 */
		 
		 if(left_max_front_index < left_min_front_index && left_max_front_val  > left_min_front_val + 100.0f)
		 {
			 real_max_front_left = real_max_front_left > left_max_front_val ? real_max_front_left : left_max_front_val;
		 }
		 else
		 {
			 real_max_front_left  = 0.f;
		 }
		 
		 if(right_max_front_index < right_min_front_index && right_max_front_val  > right_min_front_val + 100.0f)
		 {
			 real_max_front_right = real_max_front_right > right_max_front_val ? real_max_front_right : right_max_front_val;
		 }
		 else
		 {
			 real_max_front_right  = 0.f;
		 }
		 
		 if(real_max_front_left - left_min_front_val > 200.f && real_max_front_right - right_min_front_val > 200)
		 {
			 front_down_tmp = 1;
		 }
		 
		 
		 //判断后蹲
		 
		 if(left_max_back_index < left_min_back_index && left_max_back_val  > left_min_back_val + 150.0f
			 && right_max_back_index < right_min_back_index && right_max_back_val  > right_min_back_val + 150.0f
			 && left_max_back_val < 3100.f && right_max_back_val < 3100.f)
		 {
			 back_down_tmp = 1;
		 }
		 /*
		 * 取消前蹲引发的后蹲判断
		 */
		 if(left_max_back_index < left_min_back_index && left_max_back_val  > left_min_back_val + 100.0f)
		 {
			 real_max_val_left  = left_max_back_val > real_max_val_left ? left_max_back_val : real_max_val_left;
		 }
		 else
		 {
			 real_max_val_left = 0.0f;
		 }
		
		 if(right_max_back_index < right_min_back_index && right_max_back_val  > right_min_back_val + 100.0f)
		 {
			 real_max_val_right = right_max_back_val > real_max_val_right ? right_max_back_val : real_max_val_right;
		 }
		 else
		 {
			 real_max_val_right = 0.0f;
		 }
			
		 if(real_max_val_right > 3100.0f && real_max_val_left > 3100.0f)
		 {
			 back_down_tmp = 0;
		 }
		 
		 *front_down = front_down_tmp;
		 *back_down  = back_down_tmp;


    
    return 0;
}

//short press_jump_detect(int16_t *h_pos, int16_t *s_pos)
//{
//	static int last_h_z;
//	static int last_s_z;
//	static int left_up;
//	static int right_up;
//	
//	static int left_up_count;
//	static int right_up_count;

//	
//	if(h_pos[2] - last_h_z > 0)
//	{
//		left_up = 1;
//	}
//	else if((h_pos[2] - last_h_z < 0) || h_pos[2] <= 0)
//	{
//		left_up = -1;
//	}
//	
//	if(left_up == 1)
//	{
//		left_up_count ++;
//	}
//	else
//	{
//		left_up_count = 0;
//	}
//	
//	if(s_pos[2] - last_s_z > 0)
//	{
//		right_up = 1;
//	}
//	else if((s_pos[2] - last_s_z < 0) || s_pos[2] <= 0)
//	{
//		right_up = -1;
//	}
//	
//	if(right_up == 1)
//	{
//		right_up_count ++;
//	}
//	else
//	{
//		right_up_count = 0;
//	}
//	
//	last_h_z = h_pos[2];
//	last_s_z = s_pos[2];
//	
//	if(left_up == 1 && right_up == 1 && right_up_count < 30 && left_up_count < 30
//		&& right_up_count > 2 && left_up_count > 2 &&  s_pos[2] < 5 && h_pos[2] < 5 && 
//		s_pos[2] > 1 && h_pos[2] > 1)
//	{
//		return 1;
//	}
//	
//	return 0;
//	
//	
//}

//由于仅靠IMU来探测触底了，高度估计得不太对，所以用加速度来算起跳动作

void max_min_window(float *data, int length, float *max_val , int *max_index, float * min_val, int *min_index)
{
	 *max_val = data[0];
		
	 *max_index = 0;
	
	 *min_val = data[0];
	
	 *min_index = 0;
		
	 for(int i = 0; i < length; i++)
	 {
		if(*max_val < data[i])
		{
			*max_val = data[i];
			*max_index = i;
		}
		
		if(*min_val > data[i])
		{
			*min_val = data[i];
			*min_index = i;
		}
	 }
	
	
}

short press_jump_detect(float left_acc_z, float right_acc_z)
{
	
	static float right_data_z_buff[10];
	static float left_data_z_buff[10];
		
	memcpy(right_data_z_buff, right_data_z_buff + 1, 9 * sizeof(float));
	memcpy(left_data_z_buff, left_data_z_buff + 1, 9 * sizeof(float));
	
	
	right_data_z_buff[9] = right_acc_z;
	left_data_z_buff[9]  = left_acc_z;
	
	float max_val_right, min_val_right, max_val_left, min_val_left;
	
	int max_index_right, min_index_right, max_index_left, min_index_left;
	
	
	max_min_window(right_data_z_buff, 10, &max_val_right , &max_index_right, &min_val_right, &min_index_right);
	
	max_min_window(left_data_z_buff, 10, &max_val_left , &max_index_left, &min_val_left, &min_index_left);
	
	if(max_index_right < min_index_right && max_index_left < min_index_left &&
		
		max_val_right - min_val_right > 1.0f && max_val_left - min_val_left > 1.0f &&
	
		min_val_left < 0.2f && min_val_right < 0.2f && max_val_left > 1.5f &&  max_val_right > 1.5f &&
	
		fabsf(max_val_right - max_val_left) < 2.0f && fabsf(min_val_right - min_val_left) < 2.0f &&
	
		abs(max_index_right- max_index_left) < 4 && abs(min_index_right- min_index_left) < 4
	)
	{
		
		return 1;
	}
	
	return 0;
	
}