#pragma  platform(NXT)
#include "writeFile.c"

const string  sFileName = "datos.txt";

task main()
{
   long i = 0;
     float tfinal=30.0;
     long niteraciones=0;
     float posruedaAnue=0;      //Posicion instante actual rueda derecha (motorA)
     float posruedaAant=0;      //Posicion instante anterior rueda derecha (motorA)
     float posruedaBnue=0;      //Posicion instante actual rueda izquierda (motorB)
     float posruedaBant=0;      //Posicion instante anterior rueda izquierda (motorB)
     float acontrolA,acontrolB;             //Accion de control motor rueda derecha e izquierda instante k
     float acontrolA1=0,acontrolB1=0;       //Accion de control motor rueda derecha e izquierda instante k-1
     float vel_ang_d, vel_ang_i;
     float vel_lin_d, vel_lin_i;
     float vref_d, vref_i;
     float wref_d,wref_i;
     float error_vel_d, error_vel_i;         //Error de velocidad rueda derecha e izquierda instante k
     float error_vel_d1=0, error_vel_i1=0;   //Error de velocidad rueda derecha e izquierda instante k-1
     float error_vel_d2=0, error_vel_i2=0;   //Error de velocidad rueda derecha e izquierda instante k-2
     float velLin_robot, velAng_robot;
     float refx,refy;
     float xpunto=0.0;
     float ypunto=0.0;
     float velxref=0.0;
     float velyref=0.0;
   float errorcua=0.0;        //indice integral error cuadrado
   float ex=0.0, ey=0.0;

     float kx=1.0;        //Parametro de ponderacion para el control cinematico con punto descentralizado en x
     float ky=1.0;        //Parametro de ponderacion para el control cinematico con punto descentralizado en y

     float theta=0;          // Orientacion inicial del robot (rad)
     float x=500.0;             // Coordenada X inicial del robot (mm)
     float y=0.0;               // Coordenada Y inicial del robot (mm)

     float b=56;                // (separación entre las ruedas)/2 (mm)
     float e=56;                // punto descentrado para cálculo la velocidad y posición (mm)

     float pul2rad=0.0174533;   // pul2rad=2*pi/360=0.0174533: cad vuelta: 360 pulsos de encoder
     float radiorueda=28.0;     // radio de la rueda (mm)

     float Ts=0.05;             // periodo de muestreo utilizado para el control

   nMotorEncoder[motorA] = 0; // reseteamos el encoder de la rueda derecha
   nMotorEncoder[motorB] = 0; // reseteamos el encoder de la rueda izquierda
   Delete(sFileName,nIoResult);
   createTextFile(sFileName, 30000);

   float q0,q1,q2;       // parametros del controlador

   //Parametros P,Pi
     float Kp=7.5;
   float Kpi=7.5;
   float T=0.05;
   float Ti=0.1707;

   //incialización parámetros del controlador
   //Controles P

   q0=Kp;
   q1=0.0;
   q2=0.0;

   //Controles PI
   /*
   q0=Kpi;
   q1=Kpi*((T-Ti)/Ti);
   q2=0.0;
   */

   i=0;
   niteraciones=(int)((tfinal)/0.05);

   while(i < niteraciones)    //bucle de control
      {
        // reseteamos el temporizador para empezar el bucle de control
              ClearTimer(T1);

        //---------------Trayectoria circular-------------------------------------
        // posicion y orientacion inicial: x=500.0, y=0.0, theta=pi/2;
        /*
        refx=500*cos(2*3.14159*0.04*0.05*i);
        refy=500*sin(2*3.14159*0.04*0.05*i);
        velxref=-500*2*3.14159*0.04*sin(2*3.14159*0.04*0.05*i);
        velyref=500*2*3.14159*0.04*cos(2*3.14159*0.04*0.05*i);
        */
        //---------------Trayectoria infinito--------------------------------------
        // posicion y orientacion inicial: x=500.0, y=0.0, theta=pi/4;
        /*
        refx=500*sin(2*3.14159*0.04*0.05*i)+500;
        refy=250*sin(4*3.14159*0.04*0.05*i);
        velxref=500*2*3.14159*0.04*cos(2*3.14159*0.04*0.05*i);
        velyref=250*4*3.14159*0.04*cos(4*3.14159*0.04*0.05*i);
        */
        //---------------Trayectoria cuadrada--------------------------------------
        // posicion y orientacion inicial: x=500.0, y=0.0, theta=0;

            if (i < niteraciones/4)
          {
            refx=500+(1500-500)*i/(niteraciones/4);
            refy=0;
            velxref=(1500-500)/(niteraciones/4);
            velyref=0;
          }
        else if (i < niteraciones/2)
          {
            refx=1500;
            refy=(1000-0)*(i-(niteraciones/4))/(niteraciones/4);
            velxref=0;
            velyref=(1000-0)/(niteraciones/4);
          }
        else if (i < niteraciones*3/4)
          {
            refx=1500-(1500-500)*(i-2*(niteraciones/4))/(niteraciones/4);
            refy=1000;
            velxref=-(1500-500)/(niteraciones/4);
            velyref=0;
          }
        else
          {
            refx=500;
            refy=1000-(1000-0)*(i-3*(niteraciones/4))/(niteraciones/4);
            velxref=0;
            velyref=-(1000-0)/(niteraciones/4);
          }


        //-------------------------------------------------------------------------

        // obtenemos el valor de los encoders (rad) para ver cuanto se ha movido
        posruedaAnue=nMotorEncoder[motorA]*pul2rad;
        posruedaBnue=nMotorEncoder[motorB]*pul2rad;

        // calculamos las velocidades angulares (rad/s) de las ruedas
            vel_ang_d=(posruedaAnue-posruedaAant)/Ts;
            vel_ang_i=(posruedaBnue-posruedaBant)/Ts;

            // calculamos las velocidades lineales (mm/s) de las ruedas: v = w*radio
            vel_lin_d=vel_ang_d*radiorueda;
            vel_lin_i=vel_ang_i*radiorueda;

            // calculamos la velocidad lineal del robot (mm/s)
            velLin_robot= (vel_lin_d+vel_lin_i)/2;

            // calculamos la velocidad angular del robot (rad/s)
            velAng_robot= (vel_lin_d-vel_lin_i)/(2*b);

            // calculamos la posicion X-Y (mm) y la orientacion del robot (rad)
              x=x+velLin_robot*Ts*cos(theta);
              y=y+velLin_robot*Ts*sin(theta);
              theta=theta+velAng_robot*Ts;

            // calculamos la expresion del control cinematico del robot (mm/s)
            xpunto=velxref+kx*(refx-(x+e*cos(theta)));
            ypunto=velyref+ky*(refy-(y+e*sin(theta)));

            // calculamos el modelo cinematico inverso del robot (mm/s)
            vref_d=((e*cos(theta)-b*sin(theta))*xpunto+(e*sin(theta)+b*cos(theta))*ypunto)/e;
            vref_i=((e*cos(theta)+b*sin(theta))*xpunto+(e*sin(theta)-b*cos(theta))*ypunto)/e;

            // calculamos las velocidades angulares de referencia para el control dinamico (rad/s)
            wref_d=vref_d/radiorueda;
            wref_i=vref_i/radiorueda;

            // calculamos los errores de la velocidad angular de las ruedas (rad/s)
            error_vel_d=wref_d-vel_ang_d;
            error_vel_i=wref_i-vel_ang_i;

            // calculamos las acciones de control a aplicar a cada rueda
            acontrolA= q0*error_vel_d; //Accion de control P
            acontrolB= q0*error_vel_i; //Accion de control P

            //acontrolA=(q0*error_vel_d+q1*error_vel_d1+acontrolA1); //Accion de control PI
            //acontrolB=(q0*error_vel_i+q1*error_vel_i1+acontrolB1); //Accion de control PI

            // saturamos las acciones de control
            if (acontrolA>100)
                {
                    acontrolA=100;
                }
            if (acontrolA<-100)
                {
                    acontrolA=-100;
                }
            if (acontrolB>100)
                {
                    acontrolB=100;
                }
            if (acontrolB<-100)
                {
                    acontrolB=-100;
                }

              // aplicamos las acciones de control a los motores
            motor[motorA] = acontrolA;
            motor[motorB] = acontrolB;

        // almacenamos los valores de los encoder para la proxima iteracion
            posruedaAant=posruedaAnue;
            posruedaBant=posruedaBnue;

            // actualizacion de los errores y acciones anteriores
        acontrolA1=acontrolA;
        acontrolB1=acontrolB;
        error_vel_d2=error_vel_d1;
        error_vel_i2=error_vel_i1;
        error_vel_d1=error_vel_d;
        error_vel_i1=error_vel_i;

            // calculo indice integral error cuadratico
                ex=refx-x;
                ey=refy-y;
                errorcua=errorcua+sqrt(ex*ex+ey*ey);

        // escribimos los valores mas importantes en el fichero de datos
            writeFloatNumber(refx);
            writeFloatNumber(refy);
            writeFloatNumber(x);
            writeFloatNumber(y);
            writeFloatNumber(errorcua);
            writeFloatNumber(vel_ang_d);
            writeFloatnumber(vel_ang_i);
            writeNewLine();

            i++;

        //Esperar el tiempo restante hasta el siguiente periodo
            wait1Msec(50-(time1(T1)));
  }
  closeWriteTextFile();
}