wentingwei/func_count5.m

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

    global front_flag2;
    global back_flag2;
    global FrontLiftPoint2;
    global BackLiftPoint2;
    global FrontDownPoint2;
    global BackDownPoint2;
    global SD2;
    global accl2;
    global counter2;
    
    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
        FrontDownPoint2 = queue_front(2);
    else
       FrontLiftPoint2 = queue_front(2);
    end
    
    if queue_back(2)-queue_back(1)>=0
        BackDownPoint2 = queue_back(2);    
    else
        BackLiftPoint2 = queue_back(2);
    end
    
    
     acc_flag = 0;
     for i=1:1:2
         if abs(acc(i)-2048)>200
             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;
     SD2 = sqrt(D);
     
    %%%%%%%%%%%%%%%%%
     
    if (queue_front(2)-FrontLiftPoint2>2500)
         front_flag2 = 1;
     end
     
     if (queue_front(2)-FrontDownPoint2<-1000)
        front_flag2 = 0; 
     end
     
    
     if (queue_back(2)-BackLiftPoint2>2500)
         back_flag2 = 1;
     end
     
     if queue_back(2)-BackDownPoint2<-1000
         back_flag2 = 0;
     end
   

      if up_flag == 0
          if (front_flag2==1||back_flag2==1)
                 step=1;
                 up_flag = 1; 
                 accl2 = 100000;
                 counter2 = 0;
           else
                 step = 0;
                 up_flag = 0;
          end
      else
          step = 0;
          counter2 = counter2+1;
          if  (abs(acc(3)-2000)<350)
               accl2 = acc(3); 
          end
          if counter2>2

             if (acc(3)-accl2>1000)&&(accl2~=100000)
                  up_flag = 0;
             else
                  up_flag = 1;
             end
          
          end
       
     end
     
end