/* This program is the simulation of an adaptive (Sastry) feedback */
/* linearizing controller for the inverted pendulum */

# include <stdio.h>
# include <math.h>
# include "rungekutta.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 = -1.9e-3;

# 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 alfa1 8.0 /*100.0*/
# define alfa2 9.0 /*20.0*/
# define beta1 0.1
# define beta2 0.1
# define lambda 1.0
# define lambda2 1.0
# define GAMMA 1.85

double PY1(double);
double PY2(double);
double PY3(double);
double PY4(double);
void initializetheta(double *);
void adapt(double, double, double, double, double *);
double SwingUp(double);

double u=0.0;

int main(void)
{
  int i;
  FILE *fp;
  double tf, e0, e0dot, nu, Lf2he, LgLfhe, e1, w[32], theta[32];
  int Balance = 1;

  fp=fopen("penddat", "w");
  printf("Adaptive feedback linearization for the inverted pendulum.\n");
  printf("Simulation time? ");
  scanf("%lf", &tf);

  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 */

  initializetheta(theta);

  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 = yi(3) - 0.0;
	e0dot = yi(4) - 0.0;

	nu = 0.0 - alfa2*e0dot - alfa1*e0;

	LgLfhe = theta[27];
	Lf2he = theta[9]*yi(2) + theta[10]*sin(yi(3)) + theta[11]*yi(4);

	printf("LgLfhe=%f ", LgLfhe);
	printf("Lf2he=%f\n", Lf2he);
	u = (1.0/LgLfhe)*(-Lf2he + nu);

/*      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;*/

	e1 = beta2*e0dot + beta1*e0;
	adapt(e1, nu, Lf2he, LgLfhe, theta);
      }
    }
    rungekutta(T, (double (*)(double)) PY1,
	       (double (*)(double)) PY2, 
	       (double (*)(double)) PY3,
	       (double (*)(double)) PY4);
  }
  fclose(fp);
  return (0);
}

void initializetheta(double *theta)
{
  int i;

  for (i=0; i<32; i++)
    theta[i] = 0.0;
  theta[9] = a2;
  theta[10] = a3;
  theta[11] = a4;
  theta[27] = b2;
}

void adapt(double e1, double nu, double Lf2he, double LgLfhe, double *theta)
{
  int i;
  double w[32];

  for (i=0; i<32; i++)
    w[i] = 0.0;
  w[9] = -yi(2);
  w[10] = -sin(yi(3));
  w[11] = -yi(4);
  w[27] = -(-Lf2he + nu)/LgLfhe;

  for (i=0; i<32; i++)
       /* steepest descent gradient */
/*    theta[i] += T*(-lambda*e1*w[i]);*/
       /* normalized gradient */
    theta[i] += T*(-lambda*e1*w[i]/(1+lambda2*w[i]*w[i]));
}

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