mpu_func.h

scara机器人的一种简单的实现方法

#include "mpu_prt.h"

extern struct three_data Sample(void);
extern struct angle_distance  Angle_distance_calculation(void);
extern void   Error_treatment(int state);
extern struct print_ch ch_from_data(float data);
extern int    Cp_adjustment(void);
extern int    Func_step_movement(int stepped);

void   Turn_an_alignment(void);
float  net_tran_fun(float net);
int    slope_data_input(void);
int    Initia_position_adjustment(void);
int    search_screw_center(void);
void   Scara_move(struct cal_point scara_pt);
struct cal_point Scara_theta_cal(struct cal_point input_point);
struct cal_point Screw_center_cal(struct cal_point input_point,
			  float delta_x, float delta_y, float theta);
struct cal_point Scara_point_cal(struct cal_point input_point);
struct cal_point Scara_aligned_point_cal(struct cal_point input_point,
					 float  delta_z, float theta);

extern int standard;

int   Func_step_movement(int stepped)
{
  int    i;
  struct move_point m_p[2];
  struct three_data mpu_standard_data;
  char   key_value;
  struct print_ch p_ch[2];
  int    finish_state = TRUE;
  int    z_initia_move_direction;

  switch (stepped) {
    case 1:  //"0. Stand-by..."
      for (i=0; i<6;  i++) {    mpu_slope_data[i].d[0] = 0;
				mpu_slope_data[i].d[1] = 0;
				mpu_slope_data[i].d[2] = 0;   }

      screw_center_data[0].d[0] = 0;      screw_center_data[1].d[0] = 0;
      screw_center_data[0].d[0] = 0;      screw_center_data[1].d[1] = 0;
      screw_center_data[0].d[0] = 0;      screw_center_data[1].d[2] = 0;

      sc_search_state.x_move_direction = 1;
      sc_search_state.x_move_state     = FAST;
      sc_search_state.y_move_direction = 1;
      sc_search_state.y_move_state     = FAST;
      delta_y = 0.;
      delta_x = 0.;

      Pscara.coordinate[0] = x_0;
      Pscara.coordinate[1] = y_0;
      Pscara.coordinate[2] = z_0;
      Pscara.coordinate[3] = 0.;
      Pscara.alpha         = 0.;

      Pscara  = Scara_theta_cal(Pscara); // calculate theta of initial point
      Pscrewi = Screw_center_cal(Pscara, 0., 0., 0.);  // initial screw center

      Step_1_printout();
//      step = 5;
      break;
    case 2:  //"1. Initialization"
      Step_2_erase_print();
      RS_SEND(Func_F);
      delay(100);
//      if (RS_SEND(Func_A)==FALSE) Error_treatment(Initial_state_treatment);
//      delay(500);
//      mpu_standard_data = Sample();
//      standard = mpu_standard_data.d[0];
      Step_2_printout();
      break;
    case 3:  //"2. Move to initial position"
      Step_3_erase_print();

      RS_SEND(Func_A);

      Scara_move(Pscara); // move to initial position

      getch();

      for (;;) {
	     z_initia_move_direction = Initia_position_adjustment();
	     if (z_initia_move_direction!=0)  {
	       finish_state = FALSE;
	       Pscara.coordinate[2]  = Pscara.coordinate[2] +
				  z_initia_move_direction;
	       if (Pscara.coordinate[2]>max_z_coordinate) {
	         printf("Z direction move too deep!");
	         exit(1);
	       }
	       Pscrewi.coordinate[2] = Pscara.coordinate[2];
	       Scara_move(Pscara); // move along z axis
        } // obtain proper distance
	     else {
	       finish_state = TRUE;
	       break;
      }}

      if (RS_SEND(Func_A)==FALSE) Error_treatment(Angle_ditect);

      Step_3_printout(z_initia_move_direction);
      break;
    case 4:  //"3. Turn angle alignment"
      Step_4_erase_print();

      for (;;) {
      	Turn_an_alignment();

// 	   getch();

	      finish_state = turn_angle.flag;
         Step_4_printout(turn_angle.flag);
	      if (finish_state==TRUE) break;
      }

      break;
    case 5:  //"4. Slope angle detection"
      Step_5_erase_print();

      finish_state = slope_data_input();
      if (finish_state==TRUE)  {
	     ang_dis = Angle_distance_calculation();

	     Pscara.coordinate[2]  = Pscara.coordinate[2] +
				ang_dis.distance - center_distance;
	     if (Pscara.coordinate[2]>max_z_coordinate) {
	       printf("Z direction move too deep!");
	       exit(1);
	     }
	     Pscrewi.coordinate[2] = Pscara.coordinate[2];
	     delta_x = 0.;
	     delta_y = 0.;
	     Pscrewc = Screw_center_cal(Pscrewi, -58.2+delta_x, delta_y, 0.);
        Pscara = Scara_point_cal(Pscrewc);

	     Scara_move(Pscara); // move along z axis and
			    // move back to original screw center

	     Step_5_printout(ang_dis);
      }
      break;
    case 6:  //"5. Search screw center"
        Step_6_erase_print();

        outtextxy(200, 200,"Please wait for a while!");

        finish_state = search_screw_center();

      if (finish_state==TRUE)  {
	     Step_6_printout();
      }
//      else  outtextxy(200, 200,"Please wait for a while!");

      break;
    case 7:  //"6. Bolt installation"
      Step_7_erase_print();

      Pscara = Pscrewc;
      Pscara.coordinate[2]  = 0.;
      Scara_move(Pscara); // move along z axis
      outtextxy(200, 200,"Please install bolt!");
      getch();
      break;
    case 8:  //"7. Insert bolt"
      Step_8_erase_print();

      Pscara.coordinate[2]  = Pscrewi.coordinate[2] - screw_hight +
			      center_distance + inside_z;
      Pscrewi.coordinate[2] = Pscara.coordinate[2];
      Pscrewc.coordinate[2] = Pscara.coordinate[2];
      Pscrewc.coordinate[3] = Pscara.coordinate[3];
//      Pscara.coordinate[0]  = Pscrewc.coordinate[0];
//      Pscara.coordinate[1]  = Pscrewc.coordinate[1];

      Scara_move(Pscara); // move along z axis

      Step_8_printout();
      break;
    case 9:  //"8. Bolt pose alignment"
      Step_9_erase_print();

      Pscara = Scara_aligned_point_cal(Pscrewc, inside_z, ang_dis.angle);
      Scara_move(Pscara); // move along z axis

      Step_9_printout();
      break;
    case 10:  //"9. Screwing bolt"
      Step_10_erase_print();

      Pscara.coordinate[3] = 179.5;
      Scara_move(Pscara); // move along z axis
      delay(200);
      Pscara.coordinate[3] = -179.5;
      Scara_move(Pscara); // move along z axis

      Step_10_printout();
      break;
    case 11:  //"10. Move back to original position"
      setviewport(125,70,624,464,1);
      setcolor(CYAN);
      outtextxy(200, 100,"9. Screwing bolt");
      setcolor(WHITE);
      outtextxy(200, 100,"10. Move back to original position");

      Pscara.coordinate[2] = 0.;
      Scara_move(Pscara); // move along z axis
      Pscara.coordinate[3] = 0.;
      Scara_move(Pscara); // move along z axis

      break;
  }
  return finish_state;
}

struct three_data Sample(void)
{
   struct three_data data;
   struct read_data rs_input[6];
   int    i;
   float  check;
/*
   float  x0 = 411.7, y0 = 262.8;
   float  xs = 408.2, ys = 263.1;
   float  x[3], y[3], ro[3];

   x[0]  = xs + delta_x*cos(1.357) + (4.5+delta_y)*cos(1.357+pi/2.);
   y[0]  = ys + delta_x*sin(1.357) + (4.5+delta_y)*sin(1.357+pi/2.);
   x[1]  = xs + (-4.5+delta_x)*cos(1.357) + delta_y*cos(1.357+pi/2.);
   y[1]  = ys + (-4.5+delta_x)*sin(1.357) + delta_y*sin(1.357+pi/2.);
   x[2]  = xs + (4.5+delta_x)*cos(1.357) + delta_y*cos(1.357+pi/2.);
   y[2]  = ys + (4.5+delta_x)*sin(1.357) + delta_y*sin(1.357+pi/2.);

   for (i=0; i<3; i++) {
     ro[i] = sqrt((x[i]-x0)*(x[i]-x0) + (y[i]-y0)*(y[i]-y0));
     if (ro[i]<4.0 || ro[i]>5.0) data.d[i] = 900 + random(100);
     else if (ro[i]<4.5)
       data.d[i] = 900 + 5200*(ro[i]-4.0) + random(100);
     else
       data.d[i] = 900 + 5200*(5.0-ro[i]) + random(100);

     printf("%f %d ", ro[i], data.d[i]);
   }
*/

   if (RS_SEND(Func_C)==FALSE) Error_treatment(Angle_ditect);
   for (i=0; i<6; i++) {
       rs_input[i] = RS_RECEIVE();
       if (rs_input[i].flag==FALSE) Error_treatment(Angle_ditect);
   }

   for (i=0; i<3; i++) {
     data.d[i] = rs_input[2*i+1].d*256 + rs_input[2*i].d;
     print_sample(data);
   }

   delay(50);
   return data;
}

struct angle_distance Angle_distance_calculation(void)
{
  struct angle_distance result;
  struct three_data angle_input1, angle_input2;
  float input[6], first_lay[6], second_lay[13], third_lay[2];
  float w3[3][13][13], w2[3][13][13], w1[3][13][13], w0[3][13][13];
  float th[3][13];
  float net, angle, distance;
  int   i, j, k, counter;
  int   NUM_node[4] = {3, 3, 7, 1};
  FILE  *fp;

  angle_input1.d[0] = (mpu_slope_data[0].d[0]+mpu_slope_data[1].d[0]+
		       mpu_slope_data[2].d[0])/3. - 984.;
  angle_input1.d[1] = (mpu_slope_data[0].d[1]+mpu_slope_data[1].d[1]+
		       mpu_slope_data[2].d[1])/3. - 984.;
  angle_input1.d[2] = (mpu_slope_data[0].d[2]+mpu_slope_data[1].d[2]+
		       mpu_slope_data[2].d[2])/3. - 984.;

  input[0] = (1.*angle_input1.d[0])/(2400 - 984.);
  input[1] = (1.*angle_input1.d[0])/(1.*angle_input1.d[1]);
  input[2] = (1.*angle_input1.d[1])/(1.*angle_input1.d[2]);

  if (input[2]<2.5) {
    if (!(fp=fopen("res_t1.dat","r"))) {
	printf("cannot open file\n");
	exit(1);
    }
  }
  else {
    if (!(fp=fopen("res_t2.dat","r"))) {
	printf("cannot open file\n");
	exit(1);
    }
  }

  fscanf(fp, "%f", &result);
  for (i=0; i<3; i++) {
      for (j=0; j<NUM_node[i+1]; j++) {
	fscanf(fp, "%f", &th[i][j]);
  }}
  for (i=0; i<3; i++) {
      for (j=0; j<NUM_node[i]; j++) {
	for (k=0; k<NUM_node[i+1]; k++) {
	  fscanf(fp, "%f",&w1[i][j][k]);
  }}}
  fclose(fp);  //power weight input from the file of data_q.dat


  for (i=0; i<NUM_node[1]; i++) {
      first_lay[i] = net_tran_fun(w1[0][0][i]*input[i] - th[0][i]);
  } 					/* first layer output */
  for (i=0; i<NUM_node[2]; i++) {
      net = 0.;
      for (j=0; j<NUM_node[1]; j++) {
	net = net + w1[1][j][i]*first_lay[j];
      }
      second_lay[i] = net_tran_fun(net-th[1][i]);
  }					/* second layer output */
  for (i=0; i<NUM_node[3]; i++) {
      net = 0.;
      for (j=0; j<NUM_node[2]; j++) {
	net = net + w1[2][j][i]*second_lay[j];
      }
      third_lay[i] = net_tran_fun(net-th[2][i]);
  }                                 	/* third layer output */

  if (input[2]<2.5)   angle = third_lay[0]*6.82;
  else                angle = 2.728 + (third_lay[0]-0.2)*6.82;


//  result.distance = 3.0 + (third_lay[1]-0.26)*3.125;

  if (input[2]<2.5) {
    if (!(fp=fopen("res_d1.dat","r"))) {
	printf("cannot open file\n");
	exit(1);
    }
  }
  else {
    if (!(fp=fopen("res_d2.dat","r"))) {
	printf("cannot open file\n");
	exit(1);
    }
  }

  fscanf(fp, "%f", &result);
  for (i=0; i<3; i++) {
      for (j=0; j<NUM_node[i+1]; j++) {
	fscanf(fp, "%f", &th[i][j]);
  }}
  for (i=0; i<3; i++) {
      for (j=0; j<NUM_node[i]; j++) {
	for (k=0; k<NUM_node[i+1]; k++) {
	  fscanf(fp, "%f",&w1[i][j][k]);
  }}}
  fclose(fp);  //power weight input from the file of data_q.dat


  for (i=0; i<NUM_node[1]; i++) {
      first_lay[i] = net_tran_fun(w1[0][0][i]*input[i] - th[0][i]);
  } 					/* first layer output */
  for (i=0; i<NUM_node[2]; i++) {
      net = 0.;
      for (j=0; j<NUM_node[1]; j++) {
	net = net + w1[1][j][i]*first_lay[j];
      }
      second_lay[i] = net_tran_fun(net-th[1][i]);
  }					/* second layer output */
  for (i=0; i<NUM_node[3]; i++) {
      net = 0.;
      for (j=0; j<NUM_node[2]; j++) {
	net = net + w1[2][j][i]*second_lay[j];
      }
      third_lay[i] = net_tran_fun(net-th[2][i]);
  }                                 	/* third layer output */

  distance    = 3.0 + (third_lay[0]-0.26)*3.125;

  result.angle    = angle*1.83; //measured at real place
  result.distance = distance;
//  printf("\n %f %f %d  \n", result.angle, result.distance, counter);

  return result;
}

void Error_treatment(int state)
{
  printf("Error happened at state:	");
  printf("%d\n ", state);
  getch();
  exit(1);

  return;
}

float net_tran_fun(float net)
{
  if (net>10000000. || net<-10000000.) {
    printf("Argument too large in expretion function!");
    exit(1);
  }
  return 1./(1.+exp(-net));
}

struct print_ch ch_from_data(float data)
{
  struct print_ch ch;
  int    n, i;

  if (data<0.) {
    ch.ch[0] = 45;
    data = -data;
  }
  else ch.ch[0] = 43;
  for (i=1; i<7; i++) {
    n = data/1000.;
    ch.ch[i] = 48 + n;
    data = (data - n*1000.)*10.;
  }
  ch.ch[7] = 0;

  return  ch;
}

int   Initia_position_adjustment(void)
{
  struct three_data mpu_ipa_data;
  int    data, i;
  int    max_data_three_point = 0;
  int    min_data_three_point = 10000;
  int    z_move_direction;

  mpu_ipa_data = Sample();
  RS_SEND(Func_A);
  for (i=0; i++; i<3) {
    if (mpu_ipa_data.d[i]>max_data_three_point)
      max_data_three_point = mpu_ipa_data.d[i];
    if (mpu_ipa_data.d[i]<min_data_three_point)
      min_data_three_point = mpu_ipa_data.d[i];
  }

  if (min_data_three_point<1000 && max_data_three_point>2300) {
    printf("Initial position at rong place!");
    exit(1);
  }

  data = mpu_ipa_data.d[0]+mpu_ipa_data.d[1]+mpu_ipa_data.d[2];

  if(data<6200)          z_move_direction = +1;
  else if (data>11000)   z_move_direction = -1;
  else                   z_move_direction =  0;

  return z_move_direction;
}

int  Cp_adjustment(void)
{
  struct three_data mpu_cp_data[2];
  int    i, j, data_min, data_max;
  int    state;

  for (i=0; i<50; i++) {
    RS_SEND(Func_A);
    delay(500);