/* This program is the simulation of the continuous indirect */
/* adaptive method for the inverted pendulum */

# include <stdio.h>
# include <math.h>
# include "rungekutta.h"
# include "cifuzz.h"

/* System parameters */

# define m1 0.086184
# define l1 0.113
# define g 9.8066
# define J1 1.301e-3
# define C1 2.979e-3
# define ap 33.04
# define Kp 74.89
double K1;

# define a1 (-ap)
# define a2 (-K1*ap/J1)
# define a3 (m1*g*l1/J1)
# define a4 (-C1/J1)
# define b1 (Kp)
# define b2 (K1*Kp/J1)

/* Design parameters */

# define T 0.001
# define kappa0 8.0
# define beta0 -100.0
# define Da 0.5
# define Db 1.1
# define epsilon 0.01
# define eta 1.0
# define Me 0.4
# define GAMMA 1.85

double PY1(double);
double PY2(double);
double PY3(double);
double PY4(double);
double SwingUp(double);

double u=0.0;

int main(void)
{
  int i;
  FILE *fp;
  double tf, es, eeps, esbar, x[6], e0, e0dot, sat, alfa, beta, nu, uce, usi,
         ubi, kbi, alfa1, beta1, alfak, betak;
  F_SYS fuzzyalfa, fuzzybeta;
  int Balance = 1;

  fp=fopen("penddat", "w");
  printf("Indirect adaptive fuzzy control for the inverted pendulum.\n");
  printf("Simulation time? ");
  scanf("%lf", &tf);

  initfuzz(&fuzzyalfa, 0);
  initfuzz(&fuzzybeta, 1);

  initialvalues(4, 0.0, 0.0, 0.0, PI, 0.0);
      /* here: yi(1) = base angle
	       yi(2) = base velocity
               yi(3) = pendulum angle (system output)
	       yi(4) = pendulum velocity */

  while (xi <= tf) {
    if ((int)(xi/T) % 4 == 0) {
      fprintf(fp,"%f  %f  %f %f %f %f\n", xi, yi(1), yi(2), yi(3), yi(4), u);
    }
    if ((yi(3)-PI/2)>0 && (yi(3)-PI/2)<PI)
      K1=1.9e-3;
    else
      K1=-1.9e-3;
    if ((int)(xi/T) % 1 == 0) {
      if (Balance) {
	if (yi(3) <= 0.3)
	  Balance = 0;
	if ((2*PI - yi(3)) <= 0.3) {
	  Balance = 0;
	  chgyi(3, yi(3)-2*PI);
	}
	u = SwingUp(GAMMA);
      }
      else {
	e0 = 0.0 - yi(3);
	e0dot = 0.0 - yi(4);
	es = kappa0*e0 + e0dot;
	esbar = kappa0*e0dot;
	x[0] = yi(3);
	x[1] = yi(1);
	x[2] = yi(2);
	x[3] = sin(yi(3));
	x[4] = yi(4);
	if ((es/epsilon)>=1.0)
	  sat = 1.0;
	else if ((es/epsilon)<=-1.0)
	  sat = -1.0;
	else
	  sat = es/epsilon;
	eeps = es - epsilon*sat;
	x[5] = -0.9*eeps;              /* adaptation */

# ifdef NOUK
	alfak = 0.0;
	betak = 0.0;
# endif

# ifdef YESUK
	alfak = a2*yi(2)+a3*sin(yi(3))+a4*yi(4);    /* "known" functions */
	betak = b2;
# endif

	beta = fuzzsys(x, &fuzzybeta);   /* system identification */
	x[3] = sin(yi(3));
	alfa = fuzzsys(x, &fuzzyalfa);
	nu = eta*eeps + esbar;
	uce = (1.0/(betak+beta))*(-(alfak+alfa)+nu);   /* certainty equivalence term */
	usi = (1.0/beta0)*(Da+Db*abs(uce))*sgn(es);
	if (abs(es) <= Me)
	  ubi = 0.0;
	else {
	  alfa1 = 70.0*abs(yi(2))+75.0*abs(yi(3))+10.0*abs(yi(4));
	  beta1 = 140.0;
	  kbi = (1.0/beta0)*(abs(alfa)+alfa1+(abs(beta)+beta1)*abs(uce))+abs(usi);
	  ubi = kbi*sgn(es);
	}
	u = uce + usi + ubi;

/*      u=k1*yi(1)+k2*yi(2)+k3*yi(3)+k4*yi(4); */       /* just LQR */

/*      if (u >= 5.0)
	  u = 5.0;
	if (u <= -5.0)
	  u = -5.0;*/

	fuzzupdate(x, &fuzzyalfa);
	x[5] *= uce;
	fuzzupdate(x, &fuzzybeta);
      }
      rungekutta(T, (double (*)(double)) PY1,
		 (double (*)(double)) PY2, 
		 (double (*)(double)) PY3,
		 (double (*)(double)) PY4);
    }
  }
  printf("Final contents of A_alpha.\n");
  dumpmatrix(&fuzzyalfa);
  printf("Final contents of A_beta.\n");
  dumpmatrix(&fuzzybeta);
  clearfuzz(&fuzzyalfa);
  clearfuzz(&fuzzybeta);
  fclose(fp);
  return (0);
}

double PY1(double t)
{
  return (yi(2));
}

double PY2(double t)
{
  return (a1*yi(2) + b1*u);
}

double PY3(double t)
{
  return (yi(4));
}

double PY4(double t)
{
  return (a2*yi(2) + a3*sin(yi(3)) + a4*yi(4) + b2*u);
}


/************************************************************************
*       Swing-Up Control (heuristic approach):
*************************************************************************/
double SwingUp(double Gamma)
{
double
	theta0ref,
	e0,
	V;

static int
	xsign_ = 1,
	xsign  = 1;

        /* Controller */

	if((yi(3)-PI)<0) {        /* Swing the base to left */
	  theta0ref = -Gamma;
	  xsign = -1;
	}
	else {                   /* Swing the base to right */
	  theta0ref = Gamma;
	  xsign = 1;
	}
	if(xsign != xsign_) {
	  xsign_ = xsign;
	}

		/* Error */

	e0 = (theta0ref - yi(1));

		/* Proportional Control: Kp = 0.75 */

	V = 0.75*e0;
	return(V);
}
/************************************************************************/