wentingwei/func_count6.m

function [step,up_flag] = func_count6(front,back,up_flag,acc)

    global front_flag;
    global back_flag;
    global FrontLiftPoint;
    global BackLiftPoint;
    global FrontDownPoint;
    global BackDownPoint;
    global counter;
    %global FrontHighPoint;
    %global FrontLowPoint;
    %global BackHighPoint;
    %global BackLowPoint;
    global SD;
    global accl;
    
    k = length(front);
    

    queue_front(2) = front(k);
    queue_front(1) = front(k-1);
    queue_back(2) = back(k);
    queue_back(1) = back(k-1);
    
    if queue_front(2)-queue_front(1)>=0
        FrontDownPoint = queue_front(2);
    else
       FrontLiftPoint = queue_front(2);
    end
    
    if queue_back(2)-queue_back(1)>=0
        BackDownPoint = queue_back(2);    
    else
        BackLiftPoint = queue_back(2);
    end

    
    
    
     acc_flag = 0;
     for i=1:1:2
         if abs(acc(i)-2000)>100
             acc_flag = acc_flag+1;
         end
     end
    %¼ÆËã¼ÓËٶȷ½²î
     MV = (acc(1)+acc(2))/2;
     D = 0;
     for i=1:1:2
       
       D = D + (acc(i) - MV)^2; 
       
     end
   
     D = D/2;
     SD = sqrt(D);
    %%%%%%%%%%%%%%%%%
     if (queue_front(2)-FrontLiftPoint>2500)
         front_flag = 1;
     end
     
     if (queue_front(2)-FrontDownPoint<-1000)
        front_flag = 0; 
     end
     
    
     if (queue_back(2)-BackLiftPoint>2500)
         back_flag = 1;
     end
     
     if queue_back(2)-BackDownPoint<-1000
         back_flag = 0;
     end
   

      if up_flag == 0
          if (front_flag==1||back_flag==1)
                 step=1;
                 up_flag = 1; 
                 accl = 100000;
                 counter = 0;
           else
                 step = 0;
                 up_flag = 0;
          end
      else
          
          counter = counter+1;
          step = 0;
          if  (abs(acc(3)-2000)<350)
               accl = acc(3); 
          end     
          if counter>2
             if (acc(3)-accl>1000)&&(accl~=100000)
                  up_flag = 0;
             else
                  up_flag = 1;
             end
          end
       end
     
end