shoes/chenjunchao/esb_prx/press_down_detect.c

#include <stdio.h>
#include <stdint.h>
#include <string.h>

extern void gpio_mt_run(uint32_t tim);

#define PRESS_LEGTH 15
#define DETECT_DOWN_SIZE 2

//#define LEFT_PRESS_LOW 10400000
//#define RIGHT_PRESS_LOW 10050000

#define LEFT_PRESS_LOW 9860000
#define RIGHT_PRESS_LOW 10570000

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)
{
	left_press_data[(index - 1) % PRESS_LEGTH] = left_press;
	right_press_data[(index - 1) % PRESS_LEGTH] = right_press;

	mean_press_data[(index - 1) % PRESS_LEGTH] = (left_press + right_press) / 2;

	int is_down = 0;

	if (index > 10)
	{
		int now_mean_min_val = min_press_window(mean_press_data, PRESS_LEGTH);

		if (now_mean_min_val >= last_mean_min_val)
		{
			mean_up = mean_up + 1;
            
            if(now_mean_min_val > last_mean_min_val)
            {
                mean_up_true = mean_up_true + 1;
            }
			if (now_mean_min_val < mean_min_val)
			{
				mean_min_val = now_mean_min_val;
			}

			if (now_mean_min_val > mean_max_val)
			{
				mean_max_val = now_mean_min_val;
			}
            
            if(left_press > left_max_val)
            {
                left_max_val  = left_press;
            }
       
           if(left_press < left_min_val)
           {
               left_min_val  = left_press;
           }
       
           if(right_press > right_max_val)
           {
               right_max_val  = right_press;
           }
       
           if(right_press < right_min_val)
           {
               right_min_val  = right_press;
           }
            
		}

		if (now_mean_min_val < last_mean_min_val || left_press < left_press_low + 200000 || right_press < right_press_low + 200000)
		{
			mean_up = 0;
            mean_up_true = 0;
			mean_min_val = 20000000;
			mean_max_val = 0;
            
            right_min_val = 20000000;
            right_max_val = 0;

            left_min_val = 20000000;
            left_max_val = 0;
		}
        
        //if(mean_up_true > 20 && mean_max_val - mean_min_val > 500000)
        if(mean_up_true > 15 && 
          (right_max_val - right_min_val > 600000 && left_max_val - left_min_val > 600000)
         )
        {
            is_down = 1;
        }
        
		//if (mean_up > 20 && mean_max_val - mean_min_val > 500000)
        if(mean_up > 20 && 
          (right_max_val - right_min_val > 750000 || left_max_val - left_min_val > 750000)
          && mean_max_val - mean_min_val > 200000)
		{
			maybe_down = 1;
		}
        else if(maybe_down == 1)
        {
            maybe_down = 0;
            tail_count = 5;
        }
        
        if(tail_count > 0)
        {
            tail_count = tail_count -1;
            
            if(tail_count == 0 && left_press - left_press_data[(index - 7) % PRESS_LEGTH] > -500000
                    && right_press - right_press_data[(index - 7) % PRESS_LEGTH] > -500000)
            {
				down_count = down_count + 1;
                is_down = 1;
            }
        }
        
		last_mean_min_val = now_mean_min_val;
	}
	else
	{
		last_mean_min_val = (left_press + right_press) / 2;
	}
    if(last_down_tag == 0 && is_down == 1)
    {    
        last_down_tag = is_down;
        return 1;
    }
    
    last_down_tag = is_down;
    
    return 0;
}
*/
int press_down_detect_new(int index, int left_press, int right_press)
{
	static int left_data_press[30];
    static int right_data_press[30];
    static int press_count = 0;
    
    left_data_press[press_count % 30] = left_press;
    right_data_press[press_count % 30] = right_press;
    
    if(press_count >= 30)
    {
       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 = left_press;
       int right_min_val = left_press;
       
       int right_min_index = press_count;
       int right_max_index = press_count;
               
       for(int i =  press_count; i > press_count - 30; i--)
       {
           if(left_data_press[i % 30] > left_max_val)
           {
               left_max_val = left_data_press[i % 30];
               left_max_index = i;
           }
           
           if(left_data_press[i % 30] < left_min_val)
           {
               left_min_val = left_data_press[i % 30];
               left_min_index = i;
           }
           
           if(right_data_press[i % 30] > right_max_val)
           {
               right_max_val = right_data_press[i % 30];
               right_max_index = i;
           }
           
           if(right_data_press[i % 30] < right_min_val)
           {
               right_min_val = right_data_press[i % 30];
               right_min_index = i;
           }
       }
       
       if(left_max_index == press_count && left_max_index - left_min_index > 20 && left_max_val - left_min_val > 500000
               && right_max_index == press_count && right_max_index - right_min_index > 20 && right_max_val - right_min_val > 500000)
       {
           press_count = press_count + 1;
           return 1;
       }
    }
    
    press_count = press_count + 1;
    
    return 0;
}

short press_jump_detect(short *h_pos, short *s_pos)
{
	if(h_pos[2] > 0)
	{
		left_up_count = left_up_count + 1;
	}
	else
	{
		left_up_count = 0;
	}
	
	if(s_pos[2] > 0)
	{
		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;
	
}


//short press_jump_detect(int index)
//{
//    if(left_press_data[(index - 1) % PRESS_LEGTH] < left_press_low + 100000)
//    {
//        left_up_count = left_up_count + 1;
//    }
//    else
//    {
//        left_up_count = 0;
//    }

//    if(right_press_data[(index - 1) % PRESS_LEGTH] < right_press_low + 100000)
//    {
//        right_up_count = right_up_count + 1;
//    }
//    else
//    {
//        right_up_count = 0;
//    }
//    
//    
//	if(index >= 10)
//	{
//		int max_left_press  = max_press_window(left_press_data,  PRESS_LEGTH);
//		int max_right_press = max_press_window(right_press_data, PRESS_LEGTH);

//		if( left_up_count> 4 && right_up_count > 4
////			(right_press_data[(index - 1) % PRESS_LEGTH] - max_right_press < -1000000
////			 || left_press_data[(index - 1) % PRESS_LEGTH] - max_left_press < -1000000)
//			 )
//        {
//			if(last_jump_stage == 0)
//			{
//				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);
		
	}
}