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

# include <stdio.h>
# include <math.h>
# include "rungekutta.h"
# include "cdfuzz.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 Ksd 0.01         /* sliding mode term gain */
# define epsilon 0.01
# define Me 0.6
# define beta0 -100.0
# define eta 1.0
# 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, nu, uk, uhat, ubd, kbd,
         usd, alfa1;
  F_SYS fuzzy;
  int Balance = 1;

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

  initfuzz(&fuzzy);

  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 = 8.0*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.001*eeps;             /* adaptation: 0.9 for FLTYPE, 0.001 for LQRtype */

	nu = 8.0*e0 + 2.0*e0dot;

# ifdef LQRTYPE
	uk = 0.0;
# endif

# ifdef LQRk
	uk = kk1*yi(1)+kk2*yi(2)+kk3*yi(3)+kk4*yi(4);
# endif

# ifdef LFTYPE
	uk = (1.0/b2) * (-(a2*yi(2)+a3*sin(yi(3))+a4*yi(4)) + nu);
# endif

	uhat = fuzzsys(x, &fuzzy) + uk;       /* certainty equivalence term */
	usd = -Ksd*sgn(es);
	if (abs(es) <= Me)
	  ubd = 0.0;
	else {
	  alfa1 = 70.0*abs(yi(2))+75.0*abs(yi(3))+10.0*abs(yi(4));
	  kbd = abs(uhat) + abs(usd) + (alfa1 + abs(nu))/beta0;
	  ubd = kbd*sgn(es);
	}
	u = uhat + usd + ubd;
	fuzzupdate(x, &fuzzy);

/*	u = uk;            */                    /* feedback linearization */

/*        u=kk1*yi(1)+kk2*yi(2)+kk3*yi(3)+kk4*yi(4);  */      /* just LQR */

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

    }
    rungekutta(T, (double (*)(double)) PY1,
	       (double (*)(double)) PY2, 
	       (double (*)(double)) PY3,
	       (double (*)(double)) PY4);
  }
  dumpmatrix(&fuzzy);
  clearfuzz(&fuzzy);
  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);
}
/************************************************************************/