#include<stdio.h>
#include<math.h>
#include<conio.h>


double Kp,Ti,Td,Ts;
double E[100],U[100],P[100];
double e_2,e_1,e, u,u_1;
double sp,y,c,fb;
double Emax;
double feedback();
int i;

FILE *proc;

/*double comparator()
 {
  double er;
  er=sp-fb;
  if      (er>Emax)  er=Emax;
  else if (er<-Emax) er=-Emax;
  return er;
 }*/

double pid()
 {
  u  = u_1 +Kp*((e-e_1) + (Ts/Ti)*e + (Td/Ts)*(e - (2*e_1) + e_2));
  e_2=e_1;
  e_1=e;
  u_1=u;
  return u;

 }


double process()
 {
  double cc;
  cc=u*exp(-5*i*Ts);
  return cc;
 }

/*double feedback()
 {
  double f;
  f=15*c;
  return f;
 }*/





int main()
{

 Kp=1.2;Ti=.02;Td=.01;Ts=0.001;Emax=60000.0;
 clrscr();
 //initialize data
 sp=0;e=0.0; y=0.0;c=0.0;fb=0.0;
 e_2=0;e_1=0;u=0;u_1=0;

 if ((proc = fopen("e:\\haptics\\mycodes\\testpi.dat", "wt")) == NULL)
  {
    fprintf(stderr, "Cannot open output file.\n");
    return 1;
  }

sp=0;

 for(i=0;i<20;i++)

 {
  //e=comparator();
    e=0;
    y=pid();
  //c=process();
  //fb=0;
  //feedback();
  //fprintf(proc,"%f\t%f\t%f\t%g\t%f\n",sp,e,y,c-1,fb);
    fprintf(proc,"%f\n",y);
  //printf("%f\t%f\t%f\t%f\t%f\n",sp,e,y,c,fb);

  }
sp=2;
 for(i=20;i<50;i++)

 {
  //e=comparator();
    e=1;
    y=pid();
  //c=process();
  //fb=0;
  //feedback();
  //fprintf(proc,"%f\t%f\t%f\t%g\t%f\n",sp,e,y,c,fb);
    fprintf(proc,"%f\n",y);
  //printf("%f\t%f\t%f\t%f\t%f\n",sp,e,y,c,fb);

 }

fclose(proc);
return 1;

}