tma.bak

激光加工控制系统 可以读入plt文件

  oldx = e_x;
  oldy = e_y;
  oldi = I;
  oldj = J;
  strcpy(buf1,"P0 0");
  put_a_line(to_file,buf1);
}

float count_angle(float x0, float y0, float x1, float y1)
{
  float a;

  if ((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0) < 1e-8)
    return 0;
  a = acos((x1-x0)/sqrt((x1-x0)*(x1-x0)+(y1-y0)*(y1-y0)));
  if (y1 < y0)
    a = 2*PI - a;
  return a;
}

recover_data()
{
  int i;

  xmaxv = xmaxv3;
  ymaxv = ymaxv3;
//  test_dog();
  zmaxv = zmaxv3;
  xmaxa = xmaxa3;
  ymaxa = ymaxa3;
  zmaxa = zmaxa3;
  xmaxv1 = xmaxv;               // Max Speed units(G CODE) per danwei
  ymaxv1 = ymaxv;
  x_once_max_v_pulses = xmaxv*xsteps*interval/(1000.0);
  y_once_max_v_pulses = ymaxv*ysteps*interval/(1000.0);
  if (danwei == MINUTES){
    x_once_max_v_pulses /= 60.0;
    y_once_max_v_pulses /= 60.0;
  }
  x_times = x_once_max_v_pulses/x_once_max_a_pulses;
  if (x_times > 1)
    x_once_max_v_pulses = x_once_max_a_pulses*x_times;
  x_len_to_acc = 0;
  for (i=0;i<x_times;i++){
    x_len_to_acc += (i+1)*x_once_max_a_pulses;
  }
  y_times = y_once_max_v_pulses/y_once_max_a_pulses;
  if (y_times > 1)
    y_once_max_v_pulses = y_once_max_a_pulses*y_times;
  y_len_to_acc = 0;
  for (i=0;i<y_times;i++){
    y_len_to_acc += (i+1)*y_once_max_a_pulses;
  }
}

set_data()
{
  int i;

  xmaxv3 = xmaxv;
  ymaxv3 = ymaxv;
  zmaxv3 = zmaxv;
  xmaxa3 = xmaxa;
  ymaxa3 = ymaxa;
  zmaxa3 = zmaxa;
  xmaxv = xmaxv2;
  ymaxv = ymaxv2;
  zmaxv = zmaxv2;
  xmaxa = xmaxa2;
  ymaxa = ymaxa2;
  zmaxa = zmaxa2;
  xmaxv1 = xmaxv;               // Max Speed units(G CODE) per danwei
  ymaxv1 = ymaxv;
  x_once_max_v_pulses = xmaxv*xsteps*interval/(1000.0);
  y_once_max_v_pulses = ymaxv*ysteps*interval/(1000.0);
  if (danwei == MINUTES){
    x_once_max_v_pulses /= 60.0;
    y_once_max_v_pulses /= 60.0;
  }
  x_times = x_once_max_v_pulses/x_once_max_a_pulses;
  if (x_times > 1)
    x_once_max_v_pulses = x_once_max_a_pulses*x_times;
  x_len_to_acc = 0;
  for (i=0;i<x_times;i++){
    x_len_to_acc += (i+1)*x_once_max_a_pulses;
  }
  y_times = y_once_max_v_pulses/y_once_max_a_pulses;
  if (y_times > 1)
    y_once_max_v_pulses = y_once_max_a_pulses*y_times;
  y_len_to_acc = 0;
  for (i=0;i<y_times;i++){
    y_len_to_acc += (i+1)*y_once_max_a_pulses;
  }
}

float vel_per_x_p=0.1,vel_per_y_p=0.1;
float cur_v=0;
int cur_x_v=0,cur_y_v=0;
int run_x_v=0,run_y_v=0;
long cur_x_p=0,cur_y_p=0;
float lx_per_pulses=0.00001;//m
float ly_per_pulses=0.00001;//m
int delta_x=4,delta_y=4;

proc_line(int b_p,FILE *to_file)
{
  float len,xx,yy,oldxx1,oldyy1,oldxx2,oldyy2,len1;
  int i,x,y,times,j,s,s1,s2;
  float vmax,ll,curx,cury,xx1,yy1;
  char buf1[100],buf2[20];
  long x_pulses,y_pulses;

  b_x = cur_x_p * lx_per_pulses;
  b_y = cur_y_p * ly_per_pulses;
  if (buf[b_p] == 'X'){
    e_x = atof(&(buf[b_p+1]));
    i = b_p + 1;
    while ((buf[i] != 'Y')&&(buf[i] != 0))
      i ++;
    if (buf[i] == 'Y'){
      e_y = atof(&(buf[i+1]));
    }
    else{
      e_y = b_y;
    }
  }
  else{
    e_x = b_x;
    e_y = atof(&(buf[b_p+1]));
  }

  x_pulses = (e_x-b_x)/lx_per_pulses;
  y_pulses = (e_y-b_y)/ly_per_pulses;
  cur_x_p += x_pulses;
  cur_y_p += y_pulses;

  run_x_v = cur_speed * (e_x - b_x)/(sqrt((e_x-b_x)*(e_x-b_x)+(e_y-b_y)*(e_y-b_y)));
  run_y_v = cur_speed * (e_y - b_y)/(sqrt((e_x-b_x)*(e_x-b_x)+(e_y-b_y)*(e_y-b_y)));
  run_x_v_p = run_x_v/lx_per_pulses;
  run_y_v_p = run_y_v/ly_per_pulses;

  num_add_x_recs = 0;
  dis = run_x_v - cur_x_v;
  if (dis > 0){
    ps = 0;
    t = cur_x_v;
    while (t+delta_x < run_x_v){
      t += delta_x;
      ps += t;
      pulses_of_segs[num_add_x_recs] = t;
      num_add_x_recs ++;
    }
  }
  else{
    ps = 0;
    t = cur_x_v;
    while (t-delta_x > run_x_v){
      t -= delta_x;
      ps -= t;
      pulses_of_segs[num_add_x_recs] = t;
      num_add_x_recs ++;
    }
  }

  if (labs(x_pulses) < abs(ps)){
    printf("error, seg is too short or accelerator is too small.\n");
    return -1;
  }

  if (end_x_flag == 0){ //means have sub speed time
    num_sub_x_recs = 0;
    dis = run_x_v;
    if (dis > 0){
      ps = 0;
      t = cur_x_v;
      while (t+delta_x < run_x_v){
	t += delta_x;
	ps += t;
	pulses_of_segs[num_add_x_recs] = t;
	num_add_x_recs ++;
      }
  }
  else{
    ps = 0;
    t = cur_x_v;
    while (t-delta_x > run_x_v){
      t -= delta_x;
      ps -= t;
      pulses_of_segs[num_add_x_recs] = t;
      num_add_x_recs ++;
    }
  }

  if (labs(x_pulses) < abs(ps)){
    printf("error, seg is too short or accelerator is too small.\n");
    return -1;
  }

  }

  if (fabs(e_x-b_x) + fabs(e_y-b_y) < 0.0001)
    return -1;
  oldx = curx = b_x;
  oldy = cury = b_y;
  if (fabs(e_x-b_x) > fabs(e_y-b_y)){
    len = sqrt((e_x-b_x)*(e_x-b_x)+(e_y-b_y)*(e_y-b_y));
    if (len*xsteps > 2*x_len_to_acc){
      s = (len*xsteps - 2*x_len_to_acc) -
	  (int)((len*xsteps - 2*x_len_to_acc)/vmax)*vmax;
      s2 = s1 = s/2;
      for (i=0;i<x_times;i++){
	strcpy(buf1,"P");
	if (s1 > 0){
	  if (s1 > 1){
	    xx = (i+1)*x_once_max_a_pulses + 2;
	    s1 -= 2;
	  }
	  else{
	    xx = (i+1)*x_once_max_a_pulses + 1;
	    s1 --;
	  }
	}
	else
	  xx = (i+1)*x_once_max_a_pulses;
	yy = xx*fabs(e_y-b_y)/fabs(e_x-b_x);
    xx1 = xx;
    yy1 = yy;
    xx = xx*xx1/sqrt(xx1*xx1+yy1*yy1);
    yy = yy*xx1/sqrt(xx1*xx1+yy1*yy1);
	if (e_x < b_x)
	  xx = -xx;
	if (e_y < b_y)
	  yy = -yy;
	curx = oldx + xx/xsteps;
	cury = oldy + yy/ysteps;
	xx = (curx - oldx)*xsteps;
	yy = (cury - oldy)*ysteps;
	oldx = curx;
	oldy = cury;
	if (xx > 0)
	  x = xx + x_adj + 0.5;
	else
	  x = xx + x_adj - 0.5;
	if (yy > 0)
	  y = yy + y_adj + 0.5;
	else
	  y = yy + y_adj - 0.5;
	x_adj += xx - x;
	y_adj += yy - y;
	if (((oldxx > 0)&&(x < 0))||((oldxx < 0)&&(x > 0))){
	  strcpy(buf1,"P");
	  itoa(0,buf2,10);
	  strcat(buf1,buf2);
	  itoa(0,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	  strcpy(buf1,"P");
	  itoa(x,buf2,10);
	  strcat(buf1,buf2);
	  itoa(y,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	}
	else if (((oldyy > 0)&&(y < 0))||((oldyy < 0)&&(y > 0))){
	  strcpy(buf1,"P");
	  itoa(0,buf2,10);
	  strcat(buf1,buf2);
	  itoa(0,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	  strcpy(buf1,"P");
	  itoa(x,buf2,10);
	  strcat(buf1,buf2);
	  itoa(y,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	}
	else{
	  strcpy(buf1,"P");
	  itoa(x,buf2,10);
	  strcat(buf1,buf2);
	  itoa(y,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	}

	oldxx = x;
	oldyy = y;
	x_count += x;
	y_count += y;
      }

      times = (int)((len*xsteps - 2*x_len_to_acc)/vmax);
      for (i=0;i<times;i++){
	strcpy(buf1,"P");
	xx = vmax*fabs(e_x-b_x)/sqrt((e_x-b_x)*(e_x-b_x)+(e_y-b_y)*(e_y-b_y));
	yy = xx*fabs(e_y-b_y)/fabs(e_x-b_x);
//	xx = vmax;
//	yy = xx*fabs(e_y-b_y)/sqrt((e_x-b_x)*(e_x-b_x)+(e_y-b_y)*(e_y-b_y));
/*
    xx1 = xx;
    yy1 = yy;
    xx = xx*xx1/sqrt(xx1*xx1+yy1*yy1);
    yy = yy*yy1/sqrt(xx1*xx1+yy1*yy1);
*/
	if (e_x < b_x)
	  xx = -xx;
	if (e_y < b_y)
	  yy = -yy;
	curx = oldx + xx/xsteps;
	cury = oldy + yy/ysteps;
	xx = (curx - oldx)*xsteps;
	yy = (cury - oldy)*ysteps;
	oldx = curx;
	oldy = cury;
	if (xx > 0)
	  x = xx + x_adj + 0.5;
	else
	  x = xx + x_adj - 0.5;
	if (yy > 0)
	  y = yy + y_adj + 0.5;
	else
	  y = yy + y_adj - 0.5;
	x_adj += xx - x;
	y_adj += yy - y;
    if (((oldxx > 0)&&(x < 0))||((oldxx < 0)&&(x > 0))){
      strcpy(buf1,"P");
      itoa(0,buf2,10);
      strcat(buf1,buf2);
      itoa(0,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
      strcpy(buf1,"P");
      itoa(x,buf2,10);
      strcat(buf1,buf2);
      itoa(y,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
    }
    else if (((oldyy > 0)&&(y < 0))||((oldyy < 0)&&(y > 0))){
      strcpy(buf1,"P");
      itoa(0,buf2,10);
      strcat(buf1,buf2);
      itoa(0,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
      strcpy(buf1,"P");
      itoa(x,buf2,10);
      strcat(buf1,buf2);
      itoa(y,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
    }
    else{
      strcpy(buf1,"P");
      itoa(x,buf2,10);
      strcat(buf1,buf2);
      itoa(y,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
    }

    oldxx = x;
    oldyy = y;
	x_count += x;
	y_count += y;
      }

	x_count += x;
	y_count += y;
      for (i=0;i<x_times;i++){
	strcpy(buf1,"P");
	if (s2 > 0){
	  if (s2 > 1){
	    xx = (x_times - i)*x_once_max_a_pulses + 2;
	    s2 -= 2;
	  }
	  else{
	    xx = (x_times - i)*x_once_max_a_pulses + 1;
	    s2 --;
	  }
	}
	else
	  xx = (x_times - i)*x_once_max_a_pulses;
	yy = xx*fabs(e_y-b_y)/fabs(e_x-b_x);
    xx1 = xx;
    yy1 = yy;
    xx = xx*xx1/sqrt(xx1*xx1+yy1*yy1);
    yy = yy*xx1/sqrt(xx1*xx1+yy1*yy1);
    if (e_x < b_x)
	  xx = -xx;
	if (e_y < b_y)
	  yy = -yy;
	if (i != x_times-1){
	  curx = oldx + xx/xsteps;
	  cury = oldy + yy/ysteps;
	}
	else{
	  curx = e_x;
	  cury = e_y;
	}
	xx = (curx - oldx)*xsteps;
	yy = (cury - oldy)*ysteps;
	oldx = curx;
	oldy = cury;
	if (xx > 0)
	  x = xx + x_adj + 0.5;
	else
	  x = xx + x_adj - 0.5;
	if (yy > 0)
	  y = yy + y_adj + 0.5;
	else
	  y = yy + y_adj - 0.5;
	x_adj += xx - x;
	y_adj += yy - y;
    if (((oldxx > 0)&&(x < 0))||((oldxx < 0)&&(x > 0))){
      strcpy(buf1,"P");
      itoa(0,buf2,10);
      strcat(buf1,buf2);
      itoa(0,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
      strcpy(buf1,"P");
      itoa(x,buf2,10);
      strcat(buf1,buf2);
      itoa(y,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
    }
    else if (((oldyy > 0)&&(y < 0))||((oldyy < 0)&&(y > 0))){
      strcpy(buf1,"P");
      itoa(0,buf2,10);
      strcat(buf1,buf2);
      itoa(0,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
      strcpy(buf1,"P");
      itoa(x,buf2,10);
      strcat(buf1,buf2);
      itoa(y,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
    }
    else{
      strcpy(buf1,"P");
      itoa(x,buf2,10);
      strcat(buf1,buf2);
      itoa(y,buf2,10);
      strcat(buf1," ");
      strcat(buf1,buf2);
      put_a_line(to_file,buf1);
    }
    oldxx = x;
    oldyy = y;
	x_count += x;
	y_count += y;

      }
    }
    else{
      len /= 2;
      times = 0;
      ll = 0;
      while (ll < len){
        times ++;
        xx = times*x_once_max_a_pulses/xsteps;
        yy = xx*fabs(e_y-b_y)/fabs(e_x-b_x);
    xx1 = xx;
    yy1 = yy;
    xx = xx*xx1/sqrt(xx1*xx1+yy1*yy1);
    yy = yy*xx1/sqrt(xx1*xx1+yy1*yy1);
	ll += sqrt(xx*xx + yy*yy);
      }
      ll -= sqrt(xx*xx + yy*yy);
      times --;
      s1 = s2 = (len - ll)*xsteps;
      for (i=0;i<times;i++){
	strcpy(buf1,"P");
	if (s1 > 0){
	  if (s1 > 1){
	    xx = (i+1)*x_once_max_a_pulses + 2;
	    s1 -= 2;
	  }
	  else{
	    xx = (i+1)*x_once_max_a_pulses + 1;
	    s1 --;
	  }
	}
	else
	  xx = (i+1)*x_once_max_a_pulses;
	yy = xx*fabs(e_y-b_y)/fabs(e_x-b_x);
    xx1 = xx;
    yy1 = yy;
    xx = xx*xx1/sqrt(xx1*xx1+yy1*yy1);
    yy = yy*xx1/sqrt(xx1*xx1+yy1*yy1);
    if (e_x < b_x)
	  xx = -xx;
	if (e_y < b_y)
	  yy = -yy;
	curx = oldx + xx/xsteps;
	cury = oldy + yy/ysteps;
	xx = (curx - oldx)*xsteps;
	yy = (cury - oldy)*ysteps;
	oldx = curx;
	oldy = cury;
	if (xx > 0)
	  x = xx + x_adj + 0.5;
	else
	  x = xx + x_adj - 0.5;
	if (yy > 0)
	  y = yy + y_adj + 0.5;
	else
	  y = yy + y_adj - 0.5;
	x_adj += xx - x;
	y_adj += yy - y;
	if (((oldxx > 0)&&(x < 0))||((oldxx < 0)&&(x > 0))){
	  strcpy(buf1,"P");
	  itoa(0,buf2,10);
	  strcat(buf1,buf2);
	  itoa(0,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	  strcpy(buf1,"P");
	  itoa(x,buf2,10);
	  strcat(buf1,buf2);
	  itoa(y,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	}
	else if (((oldyy > 0)&&(y < 0))||((oldyy < 0)&&(y > 0))){
	  strcpy(buf1,"P");
	  itoa(0,buf2,10);
	  strcat(buf1,buf2);
	  itoa(0,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	  strcpy(buf1,"P");
	  itoa(x,buf2,10);
	  strcat(buf1,buf2);
	  itoa(y,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	}
	else{
	  strcpy(buf1,"P");
	  itoa(x,buf2,10);
	  strcat(buf1,buf2);
	  itoa(y,buf2,10);
	  strcat(buf1," ");
	  strcat(buf1,buf2);
	  put_a_line(to_file,buf1);
	}

	oldxx = x;