shoes/motion_script/motion/press_down_detect.c

#include "press_down_detect.h"


int left_press_data[10];
int right_press_data[10];

int left_up_count;
int right_up_count;


int mean_press_data[PRESS_LEGTH];

int mean_min_val = 20000000;
int mean_max_val = 0;
int mean_up = 0;
int mean_up_true = 0;
int last_mean_min_val = 0;

int right_min_val = 20000000;
int right_max_val = 0;

int left_min_val = 20000000;
int left_max_val = 0;

int maybe_down = 0;
int tail_count  = 0;
int last_down_tag = 0;

int left_press_min_val = 20000000;
int right_press_min_val = 20000000;
int left_press_max_val = 0;
int right_press_max_val = 0;
int left_press_sum = 0;
int right_press_sum = 0;
int press_count = 0;

int left_press_low  = LEFT_PRESS_LOW;
int right_press_low = RIGHT_PRESS_LOW;

uint32_t jump_count;
uint32_t down_count;

int last_jump_stage;


uint32_t get_jump_count(void)
{
	return jump_count;
}

uint32_t get_down_count(void)
{
	return down_count;
}

int mid_data(int* press_data, int length)
{
	//ðÅÝÅÅÐò
	int temp = press_data[0];
	for (int i = 0; i < length; i++)
		for (int j = i; j < length; j++)
		{
			if (press_data[j] < press_data[i])
			{
				temp = press_data[i];
				press_data[i] = press_data[j];
				press_data[j] = temp;
			}
		}

	return press_data[length / 2 - 1];
}

int min_press_window(int *window, int length)
{
	int min_window_val = window[0];

	for (int i = 0; i < length; i++)
	{
		if (min_window_val > window[i])
		{
			min_window_val = window[i];
		}
	}

	return min_window_val;
}

int max_press_window(int *window, int length)
{
	int max_window_val = window[0];

	for (int i = 0; i < length; i++)
	{
		if (max_window_val < window[i])
		{
			max_window_val = window[i];
		}
	}

	return max_window_val;
}

int poloar_trend(int index, int *window, int window_size, int *stage, int *last_top, 
	int *last_down, int *now_top, int *now_down,int *count, int *min_count)
{
	int max_val = window[(index - 1) % window_size];
	int max_index = index;
	
	int min_val = max_val;
	int min_index = index;
	
	for (int i = 0; i < window_size; i++)
	{
		int temp = window[(index - i - 1) % window_size];
		if (max_val < temp)
		{
			max_val = temp;
			max_index = index - i;
		}
		if (min_val > temp)
		{
			min_val = temp;
			min_index = index - i;
		}
	}

	if (max_index < min_index)
	{
		if (*stage == 2)
		{
			*last_down = 200000000;
			*last_top = 0;
		}

		if (*last_down > min_val)
		{
			*last_down = min_val;
		}
		if (*last_top < max_val)
		{
			*last_top = max_val;
		}

		*stage = 1;
		*count = 0;
		*min_count = 0;

	}
	else if (max_index > min_index)
	{
		if (*stage == 1)
		{
			*now_down = 200000000;
			*now_top = 0;
		}

		if (*now_down > min_val)
		{
			*now_down = min_val;
		}
		if (*now_top < max_val)
		{
			*now_top = max_val;
		}

		*stage = 2;
		*count = *count + 1;
		if (window[((index - 1) % PRESS_LEGTH)] - window[((index - 2) % PRESS_LEGTH)] < 80000 &&
			window[((index - 1) % PRESS_LEGTH)] - window[((index - 2) % PRESS_LEGTH)] >0)
		{
			*min_count = *min_count + 1;
		}
	}

    return max_index == index;
}



int press_down_detect_new(int index, int left_press, int right_press, int left_high, int right_high, float acc)
{
	  static int left_data_press[PRESS_COUNT_MAX];
    static int right_data_press[PRESS_COUNT_MAX];
    static int press_count = 0;
		
	
//		if((left_high > 1 || right_high > 1) || fabsf(acc-9.8f) > 8.0f )
		if(left_high > 1 || right_high > 1)
		{
			press_count = 0;
			return 0;
		}

	
    left_data_press[press_count % PRESS_COUNT_MAX] = left_press;
    right_data_press[press_count % PRESS_COUNT_MAX] = right_press;
	
		
    
    if(press_count >= PRESS_COUNT_MAX)
    {
       int left_max_val = left_press;
       int left_min_val = left_press;
       
       int left_min_index = press_count;
       int left_max_index = press_count;
       
       int right_max_val = right_press;
       int right_min_val = right_press;
       
       int right_min_index = press_count;
       int right_max_index = press_count;
               
       for(int i =  press_count; i > press_count - PRESS_COUNT_MAX; i--)
       {
           if(left_data_press[i % PRESS_COUNT_MAX] > left_max_val)
           {
               left_max_val = left_data_press[i % PRESS_COUNT_MAX];
               left_max_index = i;
           }
           
           if(left_data_press[i % PRESS_COUNT_MAX] < left_min_val)
           {
               left_min_val = left_data_press[i % PRESS_COUNT_MAX];
               left_min_index = i;
           }
           
           if(right_data_press[i % PRESS_COUNT_MAX] > right_max_val)
           {
               right_max_val = right_data_press[i % PRESS_COUNT_MAX];
               right_max_index = i;
           }
           
           if(right_data_press[i % PRESS_COUNT_MAX] < right_min_val)
           {
               right_min_val = right_data_press[i % PRESS_COUNT_MAX];
               right_min_index = i;
           }
       }
       
       if(left_max_index == press_count && left_max_index - left_min_index > TIME_INTERVAL && left_max_val - left_min_val > PRESS_INTER
               && right_max_index == press_count && right_max_index - right_min_index > TIME_INTERVAL && right_max_val - right_min_val > PRESS_INTER)
       {
           press_count = press_count + 1;
           return 1;
       }
    }
    
    press_count = press_count + 1;
    
    return 0;
}

short press_jump_detect(int16_t *h_pos, int16_t *s_pos)
{
	if(h_pos[2] > 2)
	{
		left_up_count = left_up_count + 1;
	}
	else
	{
		left_up_count = 0;
	}
	
	if(s_pos[2] > 2)
	{
		right_up_count = right_up_count + 1;
	}
	else
	{
		right_up_count = 0;
	}
	
	if(right_up_count > 0 && left_up_count > 0)
	{
			if(last_jump_stage == 0 && right_up_count < 15 && left_up_count < 15)
			{
				jump_count = jump_count + 1;
				
				last_jump_stage = 1;
				
				return 1;
			}
			
			return 0;
	}
		
	last_jump_stage = 0;
	
	return 0;
	
}


void press_minimum_eval(int left_press, int right_press, short h_pos_z, short s_pos_z)
{
	if(right_press > right_press_max_val)
	{
		right_press_max_val = right_press;
	}
	
	if(right_press < right_press_min_val)
	{
		right_press_min_val = right_press;
	}
	
	if(left_press > left_press_max_val)
	{
		left_press_max_val = left_press;
	}
	
	if(left_press < left_press_min_val)
	{
		left_press_min_val = left_press;
	}
	
	if(left_press_max_val - left_press_min_val < 10 && right_press_max_val - right_press_min_val < 10 )
	{
		press_count = press_count + 1;
		
		right_press_sum = right_press_sum + right_press;
		
		left_press_sum  = left_press_sum + left_press;
		
	}
	else
	{
		press_count = 0;
		
		right_press_sum = 0;
		
		left_press_sum = 0;
		
		left_press_min_val = 20000000;
		right_press_min_val = 20000000;
		left_press_max_val = 0;
		right_press_max_val = 0;
	}
	
	if(press_count == 1000)
	{
		left_press_low  = left_press_sum / press_count * 256;
		
		right_press_low = right_press_sum/ press_count * 256;
		
		
		press_count = 0;
		
		right_press_sum = 0;
		
		left_press_sum = 0;
		
		left_press_min_val = 20000000;
		right_press_min_val = 20000000;
		left_press_max_val = 0;
		right_press_max_val = 0;
		
//		gpio_mt_run(500);
		
	}
}