t.c

完成cnc系统的T曲线控制

/*************************************************************************
 *	MotionMode.c: theT_curve mode of the current axis		
 *
 *	Author:  shaojizhou
 *	Date:    3 ,24, 2008
 *	Version: 1.0
 *
 **************************************************************************
 *  Change Log
 *----------------------------------------------------------------
 *  Date    |   Author          | Reason for change
 *----------------------------------------------------------------
                     
 *************************************************************************/  
 /***********************************************************************
general variable
 ************************************************************************/

 unsigned int     cur_v = 0;//当前速度
 unsigned int     next_v = 0;
 unsigned int     max_v = 0;
 long             cur_p = 0;//改变速度或加速度时的位置
 long             last_p = 0;
 long             next_p = 0;
 long             p1 = 0;//T曲线的第一个拐点
 long             p2 = 0;//T曲线的第二个拐点
 char             p_n_cycle = 0;//‘0’ 表示正转，‘1’表示反转
 char             p_state   = 0;//T形曲线的状态
 unsigned int     app_acc = 0; //在梯形曲线中应用的实际加速度为有符号型的；
 unsigned int     next_acc = 0;
 char             T_curve_change1 = 0;//当前的T曲线是否改变了
 unsigned int     temp = 0;
 char             temp_dir = 0;
 /*********************************************************************
 judge the direction void judge_dir(void)
 ************************************************************************/
 void judge_dir(void)
 {
 if(cur_p <= next_p && cur_v ==0)
  p_n_cycle =0;
  else if(cur_p >next_p && cur_v==0)
  p_n_cycle = 1;
 }
 /********************************************************************
 long pow(unsigned int p,int x)
 ***********************************************************************/
 unsigned long pow(unsigned int p,char x)
 {
 unsigned long temp = p;
  return temp * temp;
 }
 /*********************************************************************
 long  chge_to _0(int t_chge_v,int  t_acc)
 **********************************************************************/
 
 long chge_to_0(unsigned int t_chge_v,unsigned int t_acc,long cur,char flag)
 {
  long s = pow(t_chge_v,2)/(2* t_acc);
  if(flag == 0)
  return (cur + s);
  else
  return (cur - s);
 }
/*********************************************************************
 long  chge_max _0(int t_chge_v,int  t_acc)
 **********************************************************************/
 long chge_max_0(unsigned int t_chge_v,unsigned int t_max_v,unsigned int t_acc,long cur,char flag)
 {
  long s = (long)((2 * pow(t_max_v,2) - pow(t_chge_v ,2))/(2 * t_acc));
  if(flag == 0)
  return (cur + s);
  else
  return (cur - s);
 }
  /*********************************************************************
 long  chge_rmax _0(int t_max_v,int  t_acc)
 **********************************************************************/
long  chge_rmax_0(unsigned int t_chge_v,unsigned int t_max_v,unsigned int t_acc,long cur,char flag)
 {
  long s = 0;
  if(flag == 0)  
  s = cur + pow(t_chge_v,2) /(2 * t_acc) - pow(t_max_v,2) /t_acc;
  else
  s = cur - pow(t_chge_v,2) /(2 * t_acc) + pow(t_max_v,2) /t_acc;
  return s;
 }
 /********************************************************************
 void T_curve_initial()
 ***********************************************************************/
 void T_curve_initial()
 {
 
 //add program
 }
 
 /**********************************************************************
 specify  next  postion ,max_v and max_acc,and then calculate segment of time ,the current velocity and postion 
 from positive to negative
void T_curve_change( int v,int acc,long pos)
 ***********************************************************************/
 void T_curve_change(unsigned int v,unsigned int acc,long pos)
 {
  char t_state = 0;
 // unsigned int  t_acc = 0;
  long t_p1 = 0;
  long t_p2 = 0;
             
  long c_p;
  long m_p; 
  long cm_p;
  temp = 0;
  judge_dir();
  c_p  = chge_to_0(cur_v, acc, cur_p, p_n_cycle);
  m_p  = chge_max_0(cur_v, v, acc, cur_p, p_n_cycle);
  cm_p = chge_rmax_0(cur_v, v, acc, cur_p, p_n_cycle);
  
  if(c_p <= pos && pos <=m_p && cur_v <= v && p_n_cycle == 0)
  {
  t_p1 = (pos + cur_p - pow(cur_v ,2) /(2 * acc)) /2;
  t_state = 1;
 // t_acc = acc;
  }
  
  else if(m_p <= pos && pos <= c_p && v >= cur_v && p_n_cycle == 1)
  {
   t_p1 = (cur_p + pos +(long)(pow(cur_v,2)/(2 * acc)))/2;
   t_state = 1;
  // t_acc =  - acc;
  }  
  else if(pos > m_p && cur_v <= v && p_n_cycle == 0)
  {
  t_p1 = cur_p + (pow(v,2) - pow(cur_v,2)) /(2 * acc);
  t_p2 = pos - pow(v,2)/(2 * acc);
  t_state = 2;
 // t_acc = acc;
  }
  else if(pos < m_p && v >= cur_v && p_n_cycle == 1)
  {
  t_p1 = cur_p - pow(v,2)/(2 * acc) + pow(cur_v,2 )/(2 * acc);
  t_p2 = pos + pow(v,2)/(2 * acc);
  t_state = 2;
 // t_acc = -acc;
  }
  else if(pos > c_p && cur_v > v && p_n_cycle == 0)
  {
  t_p1 = cur_p + (pow(cur_v,2) - pow(v,2))/(2 * acc);
  t_p2 = pos - pow(v,2)/(2 * acc);
  t_state = 3;
  //t_acc = acc;
  }
  else if( pos < c_p && cur_v > v && p_n_cycle == 1)
  {
  t_p1 = cur_p - (pow(cur_v ,2) -pow(v ,2))/(2 * acc);
  t_p2 = pos + pow(v,2)/(2 * acc);
  t_state = 3;
 // t_acc = -acc;
  }
  else if(cm_p <= pos && pos < c_p && p_n_cycle == 0)
  {
  t_p1 = (cur_p + pos + pow(cur_v,2)/(2 * acc))/2;
  t_state = 4;
  temp_dir =0;
 // t_acc = acc;
  }  
  else if(c_p < pos && pos < cm_p && p_n_cycle == 1)
  {
   t_p1 = (cur_p + pos - (long)(pow(cur_v,2))/(2 * (long)acc))/2;
   t_state = 4;
   temp_dir =1;
 //  t_acc = - acc;
  }
  else if(pos < cm_p &&  p_n_cycle == 0)
  {
  if(cur_v >= v)
  t_p1 = cur_p +(pow(cur_v,2) -pow(v,2))/(2 * acc);
  else
  t_p1 = cur_p +(pow(v,2) -pow(cur_v,2))/(2 * acc);
  t_p2 = pos + pow(v,2) /(2 * acc);
  t_state = 5;
  temp_dir = 0;
//  t_acc = acc;
  }
  else if(pos > cm_p && p_n_cycle == 1)
  {
  t_p1 = cur_p -(pow(cur_v,2) -pow(v,2))/(2 * acc);
  t_p2 = pos - pow(v,2) /(2 * acc);
  t_state = 5;
  temp_dir = 1;
 // t_acc = - acc;
  }
  p1 = t_p1;
  p2 = t_p2;
 // app_acc = t_acc;
   max_v = v;
   last_p = pos;
   app_acc = acc;
  p_state = t_state;
  }
  
 /**********************************************************************
scan wheather the T_curve is change ,and then set current velocity and positin 
void T_curve_period_control( )
 ***********************************************************************/
 
 void T_curve_period_control( void)
 {
 //add program 
 unsigned int t_cur_v = 0;
 t_cur_v = cur_v;
 if(T_curve_change1 !=0)
   {
   T_curve_change( next_v,next_acc,next_p);
   T_curve_change1 = 0;
   } 
   
   if(p_n_cycle == 0)
   cur_p += t_cur_v ;
   else if(p_n_cycle == 1)
   cur_p -= t_cur_v;
   
   switch(p_state)
  {  
  case 1:
  {
   if((cur_p <= p1 && p_n_cycle == 0 )||(cur_p >= p1 && p_n_cycle == 1))
   {
   if((cur_p + t_cur_v > p1 && p_n_cycle ==0)||(cur_p - t_cur_v < p1 && p_n_cycle==1))
    {
   // temp +=1;
   // if(temp ==1)
  //  t_cur_v -= app_acc;
  //  else
    t_cur_v +=0;
   // t_cur_v -= app_acc;
    } 
   else
   {
   t_cur_v +=app_acc; 
   if(t_cur_v >max_v)//verify 2008-05-15
   t_cur_v = max_v;//verify 2008-05-15
   }
   }
   else if((cur_p > p1 && p_n_cycle == 0)||(cur_p < p1 && p_n_cycle == 1))
   {
   temp =0;
   if(t_cur_v >= 2*app_acc)
   t_cur_v -= app_acc;
   else if(((last_p-cur_p  >=app_acc ) && p_n_cycle ==0)||((cur_p - last_p >= app_acc) && p_n_cycle ==1))
   t_cur_v =app_acc;
  else
  t_cur_v =abs(last_p - cur_p);
   }
   break;
  }
   case 2:
  {
   if((cur_p <= p1 && p_n_cycle == 0 )||(cur_p >= p1 && p_n_cycle == 1))
   {
   t_cur_v += app_acc;  
   if(t_cur_v > max_v)
   t_cur_v = max_v;
   }
   else if((cur_p >= p1 && cur_p<= p2 && p_n_cycle == 0) || (cur_p <= p1 && cur_p >= p2 && p_n_cycle == 1))
   {
   
   if((cur_p + t_cur_v > p2 && p_n_cycle ==0)||(cur_p - t_cur_v <p2 && p_n_cycle ==1))
    t_cur_v -=app_acc;
    else
    t_cur_v = max_v;
   }
   else if((cur_p > p2 && p_n_cycle == 0 )||(cur_p < p2 && p_n_cycle == 1))
   {
   
   if(t_cur_v >= 2 * app_acc)
   t_cur_v -= app_acc;
   else if(((last_p-cur_p  >=app_acc ) && p_n_cycle ==0)||((cur_p - last_p >= app_acc) && p_n_cycle ==1))
   t_cur_v =app_acc;
  else
  t_cur_v =abs(last_p - cur_p);
   
   }
   break;
  }
   case 3:
   {
   if((cur_p <= p1 && p_n_cycle == 0) || (cur_p >= p1 && p_n_cycle == 1))
   {
   t_cur_v -= app_acc;
   if(t_cur_v <max_v)
   t_cur_v = max_v;
   }
   else if((cur_p >= p1 && cur_p <= p2 && p_n_cycle == 0) || (cur_p <= p1 && cur_p >= p2 && p_n_cycle == 1))
   {
   
    if((cur_p + t_cur_v > p2 && p_n_cycle ==0)||(cur_p - t_cur_v <p2 && p_n_cycle ==1))
    t_cur_v -=app_acc;
    else
    t_cur_v = max_v;
   }
   else if((cur_p > p2 && p_n_cycle == 0) ||(cur_p < p2 && p_n_cycle == 1))
   {
   if(t_cur_v >= 2 * app_acc)
   t_cur_v -= app_acc;
   else if(((last_p-cur_p  >=app_acc ) && p_n_cycle ==0)||((cur_p - last_p >= app_acc) && p_n_cycle ==1))
   t_cur_v =app_acc;
  else
  t_cur_v =abs(last_p - cur_p);
   }
   break;
   }
   case 4:
   {
  //if(( cur_p >= p1 && p_n_cycle == 0 && temp_dir ==0) || (cur_p>=p1 && p_n_cycle == 1 && temp_dir ==1))
   if(( p_n_cycle == 0 && temp_dir ==0) || (p_n_cycle == 1 && temp_dir ==1))
   {
   if(t_cur_v > app_acc)
     {
     t_cur_v -=app_acc;   
     }
	else
	{
	t_cur_v = app_acc - t_cur_v;
    p_n_cycle = p_n_cycle ^1;
    }
    
   }
    else if(( cur_p >= p1 && p_n_cycle == 1 && temp_dir ==0) || (cur_p <= p1 && p_n_cycle == 0 && temp_dir ==1))
   {
   if((cur_p - cur_v <= p1 && p_n_cycle ==1 && temp_dir ==0)||(cur_p + cur_v >= p1 && p_n_cycle ==0 && temp_dir ==1))
   {
   t_cur_v +=0;
   }
   else
   {
   t_cur_v +=app_acc;
   if(t_cur_v > max_v)
   t_cur_v = max_v;
   }
   }  
   else if(( cur_p < p1 && p_n_cycle == 1 && temp_dir ==0) || (cur_p > p1 && p_n_cycle == 0 && temp_dir ==1))
   {
   
   if(t_cur_v >= 2 * app_acc)
   t_cur_v -= app_acc;
   else if(((cur_p-last_p  >=app_acc ) && p_n_cycle ==1 && temp_dir ==0)||((last_p - cur_p >= app_acc) && p_n_cycle ==0 && temp_dir ==1))
   t_cur_v =app_acc;
  else
  t_cur_v =abs(last_p - cur_p);
  
   }
   break;
   }
   case 5:
   {
   if( p_n_cycle == 0 && temp_dir ==0||  p_n_cycle == 1 && temp_dir ==1)
   {
   if(t_cur_v >= app_acc)
   t_cur_v -= app_acc;
   else
   {
   t_cur_v = app_acc - t_cur_v;
   p_n_cycle = p_n_cycle ^1;
   }
   }
    else if( cur_p >= p1 && p_n_cycle == 1 && temp_dir ==0 || cur_p < p1 && p_n_cycle == 0 && temp_dir ==1)
   {
   t_cur_v += app_acc;
   if(t_cur_v >max_v)
   t_cur_v = max_v;
   }  
   else if(( cur_p < p1 && cur_p > p2 && p_n_cycle == 1 && temp_dir ==0 ) || ( cur_p > p1 && cur_p < p2 && p_n_cycle == 0 && temp_dir ==1))
   {
   
    if((cur_p - t_cur_v <= p2 && p_n_cycle ==1 && temp_dir ==0)||(cur_p + t_cur_v >= p2 && p_n_cycle ==0 && temp_dir ==1))
    t_cur_v -=app_acc;
    else
    t_cur_v = max_v;  
   }
   else if((cur_p <= p2 && p_n_cycle == 1 && temp_dir ==0) ||( cur_p >= p2 && p_n_cycle == 0 && temp_dir ==1))
   {
   
   if(t_cur_v >= 2 * app_acc)
   t_cur_v -= app_acc;
   else if(((cur_p-last_p  >=app_acc ) && p_n_cycle ==1 && temp_dir ==0)||((last_p - cur_p <= app_acc) && p_n_cycle == 0 && temp_dir ==1))
   t_cur_v =app_acc;
  else
  t_cur_v =abs(last_p - cur_p); 
  
   }
   break;
   }
   default :
   {
   break;
   }
  }
  cur_v = t_cur_v;

}