com_f43b.h

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

/***************************************************************/
/*  DATE:     1998.12.24 THURSDAY                              */
/*  AUTHOR:   LI XIUFENG                                       */
/*  CONTENT:  Common function of BP algorism                   */
/*  VERSION:  1.0					       */
/***************************************************************/

#include "graphics.h"
#include "math.h"
#include "stdio.h"
#include "stdlib.h"
#include "process.h"
#include "conio.h"
#include "string.h"
#include "defi43b.h"
#include "com_d43b.h"

extern void   store_data(void);                  /* weights stored at "data" file */
extern void   read_results(void);                /* weights read from "data" file */
extern struct input_data data_input(int type, float DISTANCE,
				    float THETA, float ALPHA);
extern void   output_calculation(int NUM_sample, int time);
extern void   power_adjustment(int NUM_sample, int time);
extern struct input_data div_vol(float distance, float theta, float alpha);
					/* calculate input data */
extern float  net_tran_fun(float net);
extern void   w_assignment(int type);
extern void   error_calculation(int NUM_sample, int time);
extern void   data_output(float distance, float theta);
extern struct output_data estimate_dis_theta(struct input_data in_data);
extern void   setup_table(void);
extern int    input_sample_data(void);
extern void   change_length(void);
extern void   read_sample_data(void);

/* store results */
void store_data(void)
{
  int i, j, k;
  FILE *fp;

  if (!(fp=fopen("data43c","w+t"))) {
    printf("cannot open file\n");
    exit(1);
  }

  fprintf(fp, "%f\n", step_length);

  for (i=0; i<NUM_layer; i++) {
    for (j=0; j<NUM_node[i+1]; j++) {
	fprintf(fp, "%f\n", th[i][j]);
  }}

  for (i=0; i<NUM_layer; i++) {
    for (j=0; j<NUM_node[i]; j++) {
      for (k=0; k<NUM_node[i+1]; k++) {
	fprintf(fp, "%f\n", w1[i][j][k]);
  }}}
  fprintf(fp, "%f\n", error1);

  fclose(fp);
}  /* end of store results */

/* read results */
void read_results(void)
{
  int i, j, k;
  FILE *fp;

  if (!(fp=fopen("data43c","r"))) {
    step_length = 0.00005;
    step_length_w = step_length;
    error1        = 1000.;
  }

  else {
    fscanf(fp, "%f", &step_length);
    step_length_w = step_length;

    for (i=0; i<NUM_layer; i++) {
      for (j=0; j<NUM_node[i+1]; j++) {
	fscanf(fp, "%f", &th[i][j]);
    }}

    for (i=0; i<NUM_layer; 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]);
    }}}
    fscanf(fp, "%f", &error1);

    fclose(fp);
  }
}  /* end of read results */

/* data input */
struct input_data data_input(int type, float DISTANCE,
			     float THETA, float ALPHA)
{
   struct input_data es;

   if (type==REAL_DATA) {
     es.e[0] = DISTANCE;
     es.e[1] = THETA;
     es.e[2] = ALPHA;
   }
   else es = div_vol(DISTANCE, THETA/180.*pi, ALPHA/180.*pi);

  return es;
}  /* end of data input */

/* nodes' output calculation */
void output_calculation(int NUM_sample, int time)
{
  int l, i, j;
  float net;

  for (l=0; l<NUM_sample; l++) {
    for (i=0; i<NUM_node[1]; i++) {
      y1[i][l] = net_tran_fun(w1[0][0][i]*y0[i][l+time*MAX_cal_sample]-
			      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]*y1[j][l];
      }
      y2[i][l] = 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]*y2[j][l];
      }
      y3[i][l] = net_tran_fun(net-th[2][i]);
    }                                 	/* third layer output */
  }
  return;
}  /* end of nodes' output calculation */

/* power adjustment */
void power_adjustment(int NUM_sample, int time)
{
  int sample, k, j, i;

  for (sample=0; sample<NUM_sample; sample++) {
    for (k=0; k<NUM_node[3]; k++) {
      DELTA_w2[sample][k] = y3[k][sample]*(1-y3[k][sample])*
		 (real_y[k][sample+time*MAX_cal_sample]-y3[k][sample]);
    }
    for (j=0; j<NUM_node[2]; j++) {
      DELTA_w1[sample][j] = 0.;
      for (k=0; k<NUM_node[3]; k++) {
	DELTA_w1[sample][j] = DELTA_w1[sample][j] +
				y2[j][sample]*(1-y2[j][sample])*
				DELTA_w2[sample][k]*w1[2][j][k];
  }}}

  for (j=0; j<NUM_node[2]; j++) {
    for (k=0; k<NUM_node[3]; k++) {
      w2[2][j][k] = w3[2][j][k];
      for (sample=0; sample<NUM_sample; sample++) {
	w2[2][j][k] = w2[2][j][k] + step_length_w*DELTA_w2[sample][k]*
		      y2[j][sample];
      }
      w3[2][j][k] = w2[2][j][k];
  }}

  for (i=0; i<NUM_node[1]; i++) {
    for (j=0; j<NUM_node[2]; j++) {
      w2[1][i][j] = w3[1][i][j];
      for (sample=0; sample<NUM_sample; sample++) {
	w2[1][i][j] = w2[1][i][j] + step_length_w*DELTA_w1[sample][j]*
		      y1[i][sample];
      }
      w3[1][i][j] = w2[1][i][j];
  }}

}  /* end of power adjustment */

/* division of voltages */
struct input_data div_vol(float distance, float theta, float alpha)
{
  struct input_data ef;
  int    i;
  float  d[6];
  float  phy_2, a_0, theta_c, k;
  float  z1, z2, z3, z4, z5, z6, delta;

  theta_c = 19.5/180*pi;
  k       = 0.204;
  a_0     = 0.1;
  phy_2   = 17.;
  delta   = 0.5;

  d[0] = distance - phy_2*cos(alpha)*tan(theta)/2.;
  d[1] = distance + phy_2*sin(alpha)*tan(theta)/2.;
  d[2] = distance - phy_2*sin(alpha)*tan(theta)/2.;
  d[3] = d[0] + delta;
  d[4] = d[1] + delta;
  d[5] = d[2] + delta;

  z1 = a_0 + k*tan(theta_c)*pow(2.*d[0]*cos(theta)*cos(theta), 3./2.);
  z2 = a_0 + k*tan(theta_c)*pow(2.*d[1]*cos(theta)*cos(theta), 3./2.);
  z3 = a_0 + k*tan(theta_c)*pow(2.*d[2]*cos(theta)*cos(theta), 3./2.);
  z4 = a_0 + k*tan(theta_c)*pow(2.*d[3]*cos(theta)*cos(theta), 3./2.);
  z5 = a_0 + k*tan(theta_c)*pow(2.*d[4]*cos(theta)*cos(theta), 3./2.);
  z6 = a_0 + k*tan(theta_c)*pow(2.*d[5]*cos(theta)*cos(theta), 3./2.);

  ef.e[0] = z2*z2/(z1*z1)*exp(pow(d[1]*sin(2.*theta)/z2,2.)-
			  pow(d[0]*sin(2.*theta)/z1,2));
  ef.e[1] = z3*z3/(z2*z2)*exp(pow(d[2]*sin(2.*theta)/z3,2.)-
			  pow(d[1]*sin(2.*theta)/z2,2));
  ef.e[2] = z5*z5/(z4*z4)*exp(pow(d[4]*sin(2.*theta)/z5,2.)-
			  pow(d[3]*sin(2.*theta)/z4,2));
  ef.e[3] = z6*z6/(z4*z4)*exp(pow(d[5]*sin(2.*theta)/z6,2.)-
			  pow(d[3]*sin(2.*theta)/z4,2));
//  ef.e[4] = z4*z4/(z1*z1)*exp(pow(d[3]*sin(2.*theta)/z4,2.)-
//			  pow(d[0]*sin(2.*theta)/z1,2));
//  ef.e[5] = z5*z5/(z2*z2)*exp(pow(d[4]*sin(2.*theta)/z5,2.)-
//			  pow(d[1]*sin(2.*theta)/z2,2));
//  ef.e[6] = z6*z6/(z3*z3)*exp(pow(d[5]*sin(2.*theta)/z6,2.)-
//			  pow(d[2]*sin(2.*theta)/z3,2));
//  ef.e[0] = 1./(z1*z1)*exp(-pow(d[0]*sin(2.*theta)/z1,2));
//  ef.e[1] = 1./(z2*z2)*exp(-pow(d[1]*sin(2.*theta)/z2,2));
//  ef.e[2] = 1./(z3*z3)*exp(-pow(d[2]*sin(2.*theta)/z3,2));

  return ef;
}  /* end of division of voltages */


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));
}


void w_assignment(int type)
{
   int i, j, k;

   if (type==3) {
     for (j=0; j<NUM_node[2]; j++) {
      for (k=0; k<NUM_node[3]; k++) {
	w1[2][j][k] = w0[2][j][k];
     }}
     for (i=0; i<NUM_node[1]; i++) {
      for (j=0; j<NUM_node[2]; j++) {
	w1[1][i][j] = w0[1][i][j];
     }}
   }
   if (type==2) {
     for (j=0; j<NUM_node[2]; j++) {
      for (k=0; k<NUM_node[3]; k++) {
	w0[2][j][k] = w1[2][j][k];
     }}
     for (i=0; i<NUM_node[1]; i++) {
      for (j=0; j<NUM_node[2]; j++) {
	w0[1][i][j] = w1[1][i][j];
     }}
   }
   if (type==1) {
     for (j=0; j<NUM_node[2]; j++) {
      for (k=0; k<NUM_node[3]; k++) {
	w1[2][j][k] = w3[2][j][k];
     }}
     for (i=0; i<NUM_node[1]; i++) {
      for (j=0; j<NUM_node[2]; j++) {
	w1[1][i][j] = w3[1][i][j];
     }}
   }
   if (type==0) {
     for (j=0; j<NUM_node[2]; j++) {
      for (k=0; k<NUM_node[3]; k++) {
	w3[2][j][k] = w1[2][j][k];
     }}
     for (i=0; i<NUM_node[1]; i++) {
      for (j=0; j<NUM_node[2]; j++) {
	w3[1][i][j] = w1[1][i][j];
     }}
   }
}

void error_calculation(int NUM_sample, int time)
{
    int sample, i;

    for (sample=0; sample<NUM_sample; sample++) {
      for (i=0; i<NUM_node[3]; i++) {
	error = error + fabs(real_y[i][sample+time*MAX_cal_sample]-
			     y3[i][sample]);
    }}

    return;
}

struct output_data estimate_dis_theta(struct input_data in_data)
{
  int i, j, k, counter;
  float dis, the;
  struct output_data out_data[20];

  for (i=0; i<NUM_input; i++) {
    y0[i][0] = in_data.e[i];
  }

  output_calculation(1, 0);
  out_data[0].theta    = y3[0][0]*10;
  out_data[0].distance = y3[0][0]*10;
  out_data[0].alpha    = y3[2][0]*100.;

//  printf("%f %f %f", out_data[0].distance, out_data[0].theta,
//		     (y3[2][0]-0.1)*100.);

  return out_data[0];
}

void   data_output(float distance, float theta)
{
  return;
}

void setup_table(void)
{
  int    i, j, k, l, counter, counter1, n, ii;
  int    Calculate_num_sample, time, NUM_sample;
  float  factor, alpha, optimize_step;

//  Total_num_sample = input_sample_data();

  Total_num_sample = 6;
  read_sample_data();

  for (i=0; i<NUM_layer; i++) {
      for (j=0; j<NUM_node[i+1]; j++) {
	th[i][j] = (random(100)+1)/101.;
  }}

  /* weight w[][][] random assignment */
  for (i=0; i<NUM_layer; i++) {
    for (j=0; j<NUM_node[i]; j++) {
      for (k=0; k<NUM_node[i+1]; k++) {
	if (i==0) w1[i][j][k] = 1.;
	else w1[i][j][k] = (random(100)+1)/1001.;
  }}}

  read_results();

  w_assignment(2);

  counter = 0;
  counter1 = 0;

  for (n=0; n<20; n++) {
    for (ii=0; ii<500; ii++) {
      counter++;
      counter1++;

      if (counter!=2) {
	time = 0;
	w_assignment(0);
	for (NUM_sample=0; NUM_sample<Total_num_sample; NUM_sample++) {
	  /* output calculation */
	  output_calculation(1, time);
	  /* power adjustment */
	  power_adjustment(1, time);
	  time++;
	}
	w_assignment(1);
      }
      else {
	counter = 0;
	time    = 0;
	error   = 0.;

	for (NUM_sample=0; NUM_sample<Total_num_sample; NUM_sample++) {
	  output_calculation(1, time);
	  error_calculation(1, time);
	  time++;
	}
	if (counter1==50) {
	  printf("%f %f %f\n",error, error1, step_length_w);
	  counter1 = 0;
	}
	change_length();
      }

//      if (error<0.8) break;
  }}

  w_assignment(3);

  store_data();

  printf("The error is %f \n", error);

  return;
}

void read_sample_data(void)
{
  int NUM_sample, i, j, k;
  FILE *fp;

  if (!(fp=fopen("sam_d2.dat","r"))) {
    printf("cannot open file\n");
    exit(1);
  }

  NUM_sample = 0;
  for (i=0; i<Total_num_sample; i++) {
    for (j=0; j<NUM_input; j++) {
      fscanf(fp, "%f", &y0[j][NUM_sample]);
    }
    fscanf(fp, "%f", &real_y[0][NUM_sample]);
//    fscanf(fp, "%f", &real_y[1][NUM_sample]);
    NUM_sample++;
  }

  fclose(fp);

  return;
}

int input_sample_data(void)
{
  int    NUM_sample, l, i, j, k;
  struct input_data e;
  float  alpha;


  NUM_sample = 0;
  for (alpha=90; alpha<91; alpha=alpha+10) {
    for (j=SAMPLE_start_dis/0.2; j<SAMPLE_end_dis/0.2+1; ++j) {
      for (i=SAMPLE_start_theta/0.2; i<SAMPLE_end_theta/0.2; ++i) {
	e = div_vol(j*0.2, i/180.*pi*0.2, alpha/180*pi);

	for (k=0; k<NUM_input; k++) {
	    y0[k][NUM_sample] = e.e[k];
	}

	real_y[0][NUM_sample] = i*0.02;
	real_y[1][NUM_sample] = j*0.02;
//	real_y[2][NUM_sample] = alpha*0.01;
	NUM_sample++;
  }}}

  return NUM_sample;
}

void change_length(void)
{
    int   i, j, k, R1, R2;

    R1 = 100;
    R2 = 100;

    if (error<error1/1.1) {
      for (j=0; j<NUM_node[2]; j++) {
	 for (k=0; k<NUM_node[3]; k++) {
	   w0[2][j][k] = w1[2][j][k];
      }}
      for (i=0; i<NUM_node[1]; i++) {
	 for (j=0; j<NUM_node[2]; j++) {
	   w0[1][i][j] = w1[1][i][j];
      }}
//      step_length = step_length/(1+random(R1)/20000.);
//      step_length_w = step_length;
      error1 = error;
    }
    else if (error<error1) {
      for (j=0; j<NUM_node[2]; j++) {
	for (k=0; k<NUM_node[3]; k++) {
	  w0[2][j][k] = w1[2][j][k];
      }}
      for (i=0; i<NUM_node[1]; i++) {
	for (j=0; j<NUM_node[2]; j++) {
	  w0[1][i][j] = w1[1][i][j];
      }}
      if (error/error1>0.995) step_length = step_length*1.0013271;
      step_length_w = step_length;
      error1 = error;
    }

//    else if (error1>1380.) {
//       step_length_w = (random(R1)+step_length)/((random(R2)+500));
//    }
    else  {
      for (j=0; j<NUM_node[2]; j++) {
	for (k=0; k<NUM_node[3]; k++) {
	  w1[2][j][k] = w0[2][j][k];
      }}
      for (i=0; i<NUM_node[1]; i++) {
	for (j=0; j<NUM_node[2]; j++) {
	  w1[1][i][j] = w0[1][i][j];
      }}
      step_length = step_length/1.01379;
      if (step_length<0.00003) {
	   step_length = (random(R1)+step_length)/((random(R2)+500));
      }
      step_length_w = step_length;
    }
}