arc.c

交叉耦合控制(cross-coupling control)對誤差的影響

/*
   Simulation program for
       - cross-coupling control (PID)
	   & feedback control (PID)
	   & feedforward control
       - tracking an arc
*/

#include <dos.h>
#include <conio.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

const float Sampling_time=0.002;

/* system parameters */

float Kp_x,Kp_y,Ki_x,Ki_y,Kd_x,Kd_y;	/* UC gains */
float Kp_c,Ki_c,Kd_c;			/* CCC gains */
float Km_x,Km_y,tm_x,tm_y;		/* parameters of servos */
float Fric_x,Fric_y;                    /* static friction loads */

/* main program */

void main()
{

  /* temperary parameters in main program */

  FILE *fptr;
  float Velocity, theta_start, theta_end, Delta_theta, Radius;
  float gr_x, gr_y, arf_x, arf_y, bet_x, bet_y, Dx, Dy;
  float Time, Rx, Ry, Px, Py, Fx, Fy;
  float Rx_last, Ry_last, Px_last, Py_last;
  float Ex, Ey, Et, Ec, Ux, Uy;             // axial feedback
  float Ex_last, Ey_last, Et_last, Ec_last, Ux_last, Uy_last;
  float Up_x, Up_y, Ui_x, Ui_y, Ui_x_last, Ui_y_last, Ud_x, Ud_y;
  float Up_c, Ui_c, Ui_c_last, Ud_c;        // cross-coupling
  float Uf_x, Uf_y, Uf_x_last, Uf_y_last;   // feedforward
  float Rx_last_last, Ry_last_last;
  float Theta_pre, Theta_pre_pre, Rx_pre_pre, Ry_pre_pre, Rx_pre, Ry_pre;
  float Ufb_x, Ufb_y, Uff_x, Uff_y, Ucc_x, Ucc_y;
  float Wfb, Wff, Wcc;
  float EcMax, EcRms, Overshoot, Etrack, Epos;
  float Path, Theta, Theta_last;
  float cos_theta, sin_theta, tan_x, tan_y, temp, temp1, temp2;
  long int i, iter_end, R_reach_No, P_reach, P_reach_No;
  float Ecc, Ett;
  float data_out[2000][4];
  char data_file[10] = "data2.out";
  long int count;
  iter_end = 1400;

  /* define system parameters */

  Wfb = 1;				/* weightings */
  Wcc = 0;
  Wff = 1;
  Kp_x=300; 				/* controller gains */
  Ki_x=0*Sampling_time;
  Kd_x=20/Sampling_time;
  Kp_y=Kp_x;
  Ki_y=Ki_x;
  Kd_y=Kd_x;
  Kp_c=300;
  Ki_c=20000*Sampling_time;
  Kd_c=20/Sampling_time;

  Km_x=1.0;Km_y=1.05; tm_x=0.05;tm_y=0.06;
  Fric_x=0.0;Fric_y=0.0;                        /* static friction loads */

  gr_x = exp(-Sampling_time/tm_x);
  gr_y = exp(-Sampling_time/tm_y);
  arf_x = Km_x*(Sampling_time-tm_x+tm_x*gr_x);
  arf_y = Km_y*(Sampling_time-tm_y+tm_y*gr_y);
  bet_x = Km_x*(tm_x-tm_x*gr_x-Sampling_time*gr_x);
  bet_y = Km_y*(tm_y-tm_y*gr_y-Sampling_time*gr_y);

  /* path planning */

  Velocity = 30.0; 	/* [mm/s] */
  theta_start = 0.0;
  theta_end = 2*3.14159;
  Radius = 10;
  Delta_theta = Sampling_time*Velocity/Radius;

  /* initial conditions */

  Theta_last = theta_start;
    cos_theta = cos(Theta_last);
    sin_theta = sin(Theta_last);
    Rx = Radius*cos_theta;
    Ry = Radius*sin_theta;
    Rx_last = Rx;		Ry_last = Ry;
    Rx_last_last = Rx_last;     Ry_last_last = Ry_last;
  Px_last = Rx; 		Py_last = Ry;
  Px = Px_last; 		Py = Py_last;
  Ex_last = Rx-Px; 		Ey_last = Ry-Py;
  Et_last = 0.0;    		Ec_last = 0.0;
  Ux_last = 0.0; 		Uy_last = 0.0;
  Ui_x_last = 0.0; 		Ui_y_last = 0.0;
  Ui_c_last = 0.0;
  Uf_x_last = 0.0;		Uf_y_last = 0.0;
  R_reach_No = 0;	P_reach_No = 0;	     P_reach = 0;
  Overshoot = 0.0;	EcRms = 0.0;         EcMax = 0.0;

  for (i=1;i<=iter_end;++i)
  {

    /* calculate the references */
    Time = Sampling_time*(i-1);
    Theta=Theta_last+Delta_theta;
    if (Theta > theta_end)
    {
      Theta = theta_end;
    }
    cos_theta = cos(Theta);
    sin_theta = sin(Theta);
    Rx = Radius*cos_theta;
    Ry = Radius*sin_theta;
    Path = Radius*Theta;

    Theta_pre=Theta+Delta_theta;
    if (Theta_pre > theta_end) {Theta_pre=theta_end;}
    Theta_pre_pre=Theta_pre+Delta_theta;
    if (Theta_pre_pre > theta_end) {Theta_pre_pre=theta_end;}
    Rx_pre = Radius*cos(Theta_pre);
    Ry_pre = Radius*sin(Theta_pre);
    Rx_pre_pre = Radius*cos(Theta_pre_pre);
    Ry_pre_pre = Radius*sin(Theta_pre_pre);

    /* calculate the errors */
    Ex = Rx - Px;
    Ey = Ry - Py;
    Ett = Ex*tan_x + Ey*tan_y;
    Ecc = sqrt(Px*Px+Py*Py)-Radius;
    tan_x = -sin_theta + Ey/2/Radius;
    tan_y = cos_theta  - Ex/2/Radius;
    Et = Ex*tan_x + Ey*tan_y;
    Ec = - Ex*tan_y + Ey*tan_x;

    if (Theta_last < theta_end && Theta >= theta_end)
    {
      R_reach_No=i;
    }
    else if(R_reach_No == 0)
    {
      EcRms = EcRms + Ec*Ec;
      if(EcMax*EcMax < Ec*Ec)
      {
	 EcMax = Ec;
      }
    }

    Etrack = -Et;
    if(P_reach == 0)
    {
      if( Et_last > 0 && Et <= 0 )
      {
	 P_reach = 1;
	 P_reach_No = i;
      }
    }
    if(P_reach == 1)
    {
      if(Overshoot < Etrack)
      { Overshoot = Etrack; }
    }

    Theta_last = Theta;

    /* calculate the control signals */
    /* PID feedback control */
    Up_x = Ex*Kp_x;
    Up_y = Ey*Kp_y;
    Ui_x = Ui_x_last + Ki_x*Ex;
    Ui_y = Ui_y_last + Ki_y*Ey;
    Ud_x = Kd_x*(Ex-Ex_last);
    Ud_y = Kd_y*(Ey-Ey_last);
    Ui_x_last = Ui_x;
    Ui_y_last = Ui_y;
    Ex_last = Ex;
    Ey_last = Ey;
    /* cross-coupling control */
    Up_c = Ec*Kp_c;
    Ui_c = Ui_c_last + Ki_c*Ec;
    Ud_c = Kd_c*(Ec-Ec_last);
    Ui_c_last = Ui_c;
    Ec_last = Ec;
    Et_last = Et;     /* for checking the overshoot */
    /* feedforward contriol */
    //
//    Uf_x = ( Rx-(1+gr_x)*Rx_last+gr_x*Rx_last_last - bet_x*Uf_x_last )/arf_x;
//    Uf_y = ( Ry-(1+gr_y)*Ry_last+gr_y*Ry_last_last - bet_y*Uf_y_last )/arf_y;
    // causal
//    Uf_x = ( Rx-(1+gr_x)*Rx_last+gr_x*Rx_last_last )/(arf_x+bet_x);
//    Uf_y = ( Ry-(1+gr_y)*Ry_last+gr_y*Ry_last_last )/(arf_y+bet_y);
    // noncausal
    Uf_x = ( Rx_pre_pre-(1+gr_x)*Rx_pre+gr_x*Rx )/(arf_x+bet_x);
    Uf_y = ( Ry_pre_pre-(1+gr_y)*Ry_pre+gr_y*Ry )/(arf_y+bet_y);
    Uf_x_last = Uf_x;
    Uf_y_last = Uf_y;
    Rx_last_last = Rx_last;
    Ry_last_last = Ry_last;
    Rx_last = Rx;
    Ry_last = Ry;

    /* calculate the controls (summation) */
    Ufb_x = Wfb*(Up_x+Ui_x+Ud_x);
    Ufb_y = Wfb*(Up_y+Ui_y+Ud_y);
    Uff_x = Wff*Uf_x;
    Uff_y = Wff*Uf_y;
    temp = Wcc*(Up_c+Ui_c+Ud_c);
    Ucc_x = - temp*tan_y;
    Ucc_y = temp*tan_x;

    /* calculate the disturbances */
    temp1 = Ufb_x  + Uff_x + Ucc_x;
    temp2 = Ufb_y  + Uff_y + Ucc_y;
    if (Fx>0) {Dx = Fric_x;}
    else if (Fx<0) {Dx = -Fric_x;}
    else {Dx =  temp1;}
    if (Dx > Fric_x) Dx = Fric_x;
    if (Dx < -Fric_x) Dx = -Fric_x;
    if (Fric_x==0) Dx=0;

    if (Fy>0) {Dy = Fric_y;}
    else if (Fy<0) {Dy = -Fric_y;}
    else {Dy =  temp2;}
    if (Dy > Fric_y) Dy = Fric_y;
    if (Dy < -Fric_y) Dy = -Fric_y;
    if (Fric_y==0) Dy=0;
/*
    if(temp1>3000) temp1=3000;
    if(temp1<-3000) temp1=-3000;
    if(temp2>3000) temp2=3000;
    if(temp2<-3000) temp2=-3000;
*/
    /* calculate the controls */
    Ux = temp1 - Dx;
    Uy = temp2 - Dy;

    /* calculate the outputs */
    temp1 = (1+gr_x)*Px - gr_x*Px_last + arf_x*Ux + bet_x*Ux_last;
    temp2 = (1+gr_y)*Py - gr_y*Py_last + arf_y*Uy + bet_y*Uy_last;
    Px_last = Px;
    Py_last = Py;
    Px = temp1;
    Py = temp2;
    Ux_last = Ux;
    Uy_last = Uy;
    Fx = (Px-Px_last)/Sampling_time;
    Fy = (Py-Py_last)/Sampling_time;

    /* save the data */
//    data_out[i][0] = Time;
//    data_out[i][1] = Ec;
//    data_out[i][2] = Path-Et;
//    data_out[i][3] = Path;

    count=i/1;
    if( (i-count*1) < 0.99 )
    {
//      data_out[count][0] = Time;
//      data_out[count][1] = Ec;
      data_out[count][0] = Uf_x;
      data_out[count][1] = Uf_y;
      data_out[count][2] = Path-Et;
      data_out[count][3] = Path;
    }

 }

 /* simulation outputs */
 fptr = fopen(data_file, "w");

 if(R_reach_No == 0) {R_reach_No=iter_end;}
 if(P_reach_No == 0) {P_reach_No=iter_end;}
 EcRms = sqrt(EcRms/R_reach_No);
 Epos = sqrt(Ex*Ex+Ey*Ey);
//
//  fprintf(fptr, "Max. Ec   =%8.3f\n", EcMax);
//  fprintf(fptr, "Rms of Ec =%8.3f\n", EcRms);
//  fprintf(fptr, "Overshoot =%8.3f\n", Overshoot);
//  fprintf(fptr, "Et-ss =%8.3f\n", Epos);
//  fprintf(fptr, "Time lag = %6.2f\n", 0.01*(P_reach_No-R_reach_No));
//   fprintf(fptr, "R_reach_No = %ld\n", R_reach_No);
//   fprintf(fptr, "P_reach_No = %ld\n", P_reach_No);
//   fprintf(fptr, "********************\n");

// for (i=1;i<iter_end;++i)
 for (i=1;i<iter_end/1;++i)
 {
     fprintf(fptr, "%8.3f %8.3f %8.3f %8.3f\n",
		data_out[i][0],data_out[i][1],data_out[i][2],data_out[i][3]);
 }
 fclose(fptr);

/* end of main program */
}