pos.h

三軸伺服馬達控制程式利用軟體DDA方式讀回目前位置然後以取樣時間計算出下一點位置取誤差後利用DA將命令電壓送出做三軸控制

void SendDDA(void)
{
  float temp1,temp2,temp3;
  temp1 = DDA_Pulse1+Point_V1;
  Point_V1=temp1-(int)temp1;
  DDA_Pulse_Modi1=(int)temp1;   
  temp2 = DDA_Pulse2+Point_V2;
  Point_V2=temp2-(int)temp2;
  DDA_Pulse_Modi2=(int)temp2;   
  temp3 = DDA_Pulse3+Point_V3;
  Point_V3=temp3-(int)temp3;
  DDA_Pulse_Modi3=(int)temp3; 
  Pulse_Cnt1+=DDA_Pulse_Modi1;
  Pulse_Cnt2+=DDA_Pulse_Modi2;
  Pulse_Cnt3+=DDA_Pulse_Modi3;   
} 

void InitPos_Para(long speed,float tacc)
{
  int   step_num;
  float acc_p1,dec_p1,acc_p2,dec_p2,acc_p3,dec_p3;
  float total_time,total_time1,total_time2,total_time3;
  Point_V1=0.0;Point_V2=0.0;Point_V2=0.0;
  if (Distance_Pulse1>0) Speed1=(float)speed;
  else {
    if (Distance_Pulse1<0) Speed1=-(float)speed;
    else {
      Speed1=0.0;
      total_time1=tacc+tacc;
    }
  }  
  if (Distance_Pulse2>0) Speed2=(float)speed;
  else {
    if (Distance_Pulse2<0) Speed2=-(float)speed;
    else {
      Speed2=0.0;
      total_time2=tacc+tacc;
    }
  }         
  if (Distance_Pulse3>0) Speed3=(float)speed;
  else {
    if (Distance_Pulse3<0) Speed3=-(float)speed;
    else {
      Speed3=0.0;
      total_time3=tacc+tacc;
    }
  }                  
  
  acc_p1=(Speed1*tacc)/2;
  dec_p1=(Speed1*tacc)/2;  
  acc_p2=(Speed2*tacc)/2;
  dec_p2=(Speed2*tacc)/2;  
  acc_p3=(Speed3*tacc)/2;
  dec_p3=(Speed3*tacc)/2;
      
  if ( Distance_Pulse1>0) {
    if ( Distance_Pulse1<(acc_p1+dec_p1) ) total_time1=tacc+tacc;
    else total_time1=(Distance_Pulse1-acc_p1-dec_p1)/Speed1+tacc+tacc;
  }
  if (Distance_Pulse1<0) {
    if ( Distance_Pulse1>(acc_p1+dec_p1) ) total_time1=tacc+tacc;
    else total_time1=(Distance_Pulse1-acc_p1-dec_p1)/Speed1+tacc+tacc;
  }   
  
  if (Distance_Pulse2>0) {
    if ( Distance_Pulse2<(acc_p2+dec_p2) ) total_time2=tacc+tacc;
    else total_time2=(Distance_Pulse2-acc_p2-dec_p2)/Speed2+tacc+tacc;
  }
  if (Distance_Pulse2<0) {
    if ( Distance_Pulse2>(acc_p2+dec_p2) ) total_time2=tacc+tacc;
    else total_time2=(Distance_Pulse2-acc_p2-dec_p2)/Speed2+tacc+tacc;
  }  
   
  if ( Distance_Pulse3>0) {
    if ( Distance_Pulse3<(acc_p3+dec_p3) ) total_time3=tacc+tacc;
    else total_time3=(Distance_Pulse3-acc_p3-dec_p3)/Speed3+tacc+tacc;
  }
  if (Distance_Pulse3<0)  {
    if ( Distance_Pulse3>(acc_p3+dec_p3) ) total_time3=tacc+tacc;
    else total_time3=(Distance_Pulse3-acc_p3-dec_p3)/Speed3+tacc+tacc;
  }                    
                                   
  if (total_time1<total_time2) total_time=total_time2;
  else total_time=total_time1;
  if (total_time<total_time3) total_time=total_time3;
     
  step_num=(int)(total_time/DDA_TIME);
  if ( (total_time-(float)step_num*DDA_TIME) > 0 ) {
    step_num=step_num+1;
  }
  total_time=(float)step_num*DDA_TIME;

  Speed1=Distance_Pulse1/(total_time-tacc);
  Speed2=Distance_Pulse2/(total_time-tacc);
  Speed3=Distance_Pulse3/(total_time-tacc);
  
  Step1=tacc/DDA_TIME;
  Step3=tacc/DDA_TIME;
  Step2=step_num-Step3-Step1;
} 
 
void PosMove(long xpos,long ypos,long zpos,long vel,float tacc)
{
  int i;
  float  acc_pos1,acc_pos2,acc_pos3;
  float  dec_pos1,dec_pos2,dec_pos3;
  float  next_pos1,next_pos2,next_pos3;
  float  time_pass;

  Distance_Pulse1=xpos-Pulse_Cnt1;
  Distance_Pulse2=ypos-Pulse_Cnt2;
  Distance_Pulse3=zpos-Pulse_Cnt3;
   
  InitPos_Para(vel,tacc);
  Sys_Pos1=0.0;
  Sys_Pos2=0.0;
  Sys_Pos3=0.0;
// accelerate profile 
  for (i=1;i<=Step1;i++)
  {
    time_pass=(float)i*DDA_TIME;
    next_pos1=0.5*(Speed1/tacc)*(time_pass*time_pass);   
    next_pos2=0.5*(Speed2/tacc)*(time_pass*time_pass);
    next_pos3=0.5*(Speed3/tacc)*(time_pass*time_pass); 
    DDA_Pulse1=next_pos1-Sys_Pos1;    
    DDA_Pulse2=next_pos2-Sys_Pos2;
    DDA_Pulse3=next_pos3-Sys_Pos3; 
    Sys_Pos1=next_pos1;   
    Sys_Pos2=next_pos2;
    Sys_Pos3=next_pos3; 
    while ( DDAOK==0 );  //1 DDA cycle needs 20 delta DDA period   
    DDAOK=0; 
    SendDDA();
  }
  while ( DDAOK==0 );  
  acc_pos1=next_pos1;    
  acc_pos2=next_pos2;   
  acc_pos3=next_pos3;   
   
// constant profile
  for (i=1;i<=Step2;i++)
  {
    time_pass=(float)i*DDA_TIME;
    next_pos1=acc_pos1+Speed1*time_pass;    
    next_pos2=acc_pos2+Speed2*time_pass;
    next_pos3=acc_pos3+Speed3*time_pass;
    DDA_Pulse1=next_pos1-Sys_Pos1;
    DDA_Pulse2=next_pos2-Sys_Pos2; 
    DDA_Pulse3=next_pos3-Sys_Pos3; 
    Sys_Pos1=next_pos1;           
    Sys_Pos2=next_pos2;  
    Sys_Pos3=next_pos3;   
    while ( DDAOK==0 );   
    DDAOK=0;  
    SendDDA();
  }
  while ( DDAOK==0 );  
  dec_pos1=next_pos1;    
  dec_pos2=next_pos2;
  dec_pos3=next_pos3;
   
// decelerate profile
  for (i=1;i<=Step3;i++)
  {
    time_pass=(float)i*DDA_TIME;
    next_pos1=dec_pos1+((Speed1*time_pass)-0.5*(Speed1/tacc)*(time_pass*time_pass));    
    next_pos2=dec_pos2+((Speed2*time_pass)-0.5*(Speed2/tacc)*(time_pass*time_pass));  
    next_pos3=dec_pos3+((Speed3*time_pass)-0.5*(Speed3/tacc)*(time_pass*time_pass));  
    DDA_Pulse1=next_pos1-Sys_Pos1;   
    DDA_Pulse2=next_pos2-Sys_Pos2;   
    DDA_Pulse3=next_pos3-Sys_Pos3;  
    Sys_Pos1=next_pos1; 
    Sys_Pos2=next_pos2; 
    Sys_Pos3=next_pos3;  
    while ( DDAOK==0 );  
    DDAOK=0;  
    SendDDA();
  }  
  while ( DDAOK==0 );  
  
}  
 
void GetAbsEncoderPos(void)
{ 
  unsigned long k;  
  unsigned char near *InAdr;
  InAdr=(unsigned char near*) ENC13;
  k=0;
  AbsEncoderPos1=0; 
  Status1=*InAdr;
  InAdr--;
  k=*InAdr;
  k<<=8;
  InAdr--; 
  k|=(*InAdr); 
  k<<=8;
  InAdr--; 
  k|=(*InAdr); 
  if (k&0x00800000) k|=0xff000000;
  AbsEncoderPos1=k; 
   
  InAdr=(unsigned char near*) ENC23;
  k=0;
  AbsEncoderPos2=0; 
  Status2=*InAdr;
  InAdr--;
  k=*InAdr;
  k<<=8;
  InAdr--; 
  k|=(*InAdr); 
  k<<=8;
  InAdr--; 
  k|=(*InAdr); 
  if (k&0x00800000) k|=0xff000000;
  AbsEncoderPos2=k;                
  
  InAdr=(unsigned char near*) ENC33;
  k=0;
  AbsEncoderPos3=0; 
  Status3=*InAdr;
  InAdr--;
  k=*InAdr;
  k<<=8;
  InAdr--; 
  k|=(*InAdr); 
  k<<=8;
  InAdr--; 
  k|=(*InAdr); 
  if (k&0x00800000) k|=0xff000000;
  AbsEncoderPos3=k;   
   
}