/* ---------------------------------------------------------------------------
|
|
| Autor : Felix San  Martin   (felixsan@pantuflo.escet.urjc.es)
|         Jose Maria Cañas Plaza      
|         https://gsyc.escet.urjc.es/robotica/pfc-siguebola.html
| Descripcion:
5.24 (Félix)
 En esta version funciona el control en posicion con la pelota parada
 tiene que funcionar en velocidad
 ahora el centro es el centro de la ventana
 funciona control en velocidad
 vel nominal y estable alta
 vel nominal x estable + vel produce giros muy rapidos

----------------------------------------------------------------------------*/
# include "eyebot.h"
# include <math.h>

#define HILOS 3
#define SSIZE  160000

/** pointer to thread control blocks */
struct tcb  *pfinal, *pcaptura, *pmotores;

/** thread control blocks */
struct tcb final_tcb, captura_tcb, motores_tcb;


typedef BYTE image_XXL [64 * 16];
image_XXL imagen_XXL;

#define ancho 62
#define largo 82
int imagen[ancho*largo];
colimage imagen_col;


/* Variables para almacenar los histogramas */
int HistX[largo];
int HistY[ancho];


int  t1,t2, horas, mins, secs, ticks;
float frames=0.0;
int contador=0;


int i,col,fila,color;
int Md,Mi,Ma,Mb,X,Y,R,MX,MY,MR;


const int rojo=0;
const int verde=1;
const int azul=2;

/* array que va a contener los min1,max1,min2,max2,....
   en x y en y respectivamnente */
int ptosx[10];
int indicex;
int ptosy[10];
int indicey;

/* tipo ventana que va a contener los puntos de cada ventana
   vent [1][1]..[1][4] asi sucesivamente hasta el numero max de ventanas */
int Pelotas[40];
int indPelotas;
/* varaibles para representar el centro de la imagen */
int OrigenX=41,OrigenY=31;
int dx;

int Nvent=0;

/* variables para los motores */
VWHandle vw;
float controlx;
float controly;
int errorx;
int errory;
float const grados = 0.03; /* 0.035 radianes = 2 grados */
float const distancia = 0.01; 

/* Variable global para pintar o no la imagen */
int mostrar;
int limpiar;

/* variable semaforo */
struct sem *sem;
struct sem *semcontrol;
int TipoControl=0;/* variable para decir si es control en velocidad(=0) o en posicion(=1) */
int gira,avanza;

/* variable global para depurar avance y giro */
int depuravel = 0;

int SacaBordesVentanas(int *histograma, int limite, int *bordes, int *counter)
{
  int i;
  const int umbral1=1;
  const int umbral2=4;
  int estado=0;
  int comienzo=0;

  for (i=1; i<=limite; i++)
    {
      if (estado==0)
	{
	  if (histograma[i]>umbral1) estado=1;
	  if (histograma[i]>umbral2) estado=2;
	  comienzo=i;
	}
      else if (estado==1)
	{
	  if (histograma[i]>umbral2) estado=2;
	  else if (histograma[i]<umbral1) {estado=0; comienzo=i;}
	}
      else if (estado==2)
	{
	  if (histograma[i]<umbral1) 
	    {
	    bordes[*counter]=comienzo;
	    bordes[*counter+1]=i-1;
	    *counter+=2;
	    estado=0; 
	    comienzo=i;
	    }
	}
      histograma[i]=0;/* necesario para no tener que actualizar los histogramaspor separado*/
    }

  if (estado==2) 
    {bordes[*counter]=comienzo;
    bordes[*counter+1]=limite;
    *counter+=2;
    }
  
  return 0;
}

/* funcion que saca los ptos maximos de cada ventana */
int sacarmax(int *imgbin,int *Md,int *Mi,int *Ma,int *Mb,int p1,int p4){
  int cont=0;
  int i=p1;
  int f,c;
  int Xmin,Xmax,Ymin,Ymax;
  int tam=0;  
  
  Xmin=p1%82;
  Xmax=p4%largo;
  Ymin=(int)p1/largo;
  Ymax=(int)p4/largo;
  
 
  for(f=Ymin;f<=Ymax;f++)
    for(c=0;c<=Xmax-Xmin;c++){
      if(imgbin[f*largo+Xmin+c]==255){
	tam++;
	if (cont==0){
	  *Md=*Mi=*Ma=*Mb=f*largo+Xmin+c;
	  cont++;
	}
	else{	  
	  i=f*largo+Xmin+c;
	
	  if(f<(int)*Ma/largo)
	    *Ma=i;
	  if(f>(int)*Mb/largo)
	    *Mb=i;
	  if(Xmin+c<*Mi%largo)
	    *Mi=i;
	  if(Xmin+c>*Md%largo)
	    *Md=i;
	}	  
      }
    }
 
  return tam;
}
 

int Circulo(int Md,int Mi,int Mb,int *X,int *Y,int *R){
  /* funcion que a partir de unos puntos calcula el centro (x,y)
     del circulo y su radio */
  
  int X1,X2,X3,Y1,Y2,Y3;
  int Xaux, Yaux;

  /* tengo que descartar un punto */
  /* por ahora descarto el Ma(arriba) luego uno cada vez */
  X1=Md%largo;
  X2=Mi%largo;
  X3=Mb%largo;
  Y1=((int)Md/largo);
  Y2=((int)Mi/largo);
  Y3=((int)Mb/largo);

  Xaux= (2*(X1*Y2-X1*Y3+X2*Y3-X3*Y2-Y1*X2+Y1*X3));
  if (Xaux!=0)
    *X=(int)((-Y2*(Y3*Y3)-Y2*(X3*X3)+Y1*(Y3*Y3)+Y1*(X3*X3)-Y1*(X2*X2)-
	      Y1*(Y2*Y2)+Y2*(Y1*Y1)+Y2*(X1*X1)+Y3*(Y2*Y2)+Y3*(X2*X2)-Y3*(Y1*Y1)-Y3*(X1*X1))/
	     Xaux);
  
  Yaux=(2*(X1*Y2-X1*Y3+X2*Y3-X2*Y1-X3*Y2+Y1*X3));
  if (Yaux!=0)
    *Y=(int)((-X2*(Y1*Y1)-X2*(X1*X1)-X3*(Y2*Y2)-X3*(X2*X2)+X3*(Y1*Y1)+X3*(X1*X1)-X1*(Y3*Y3)-
	      X1*(X3*X3)+X1*(X2*X2)+X1*(Y2*Y2)+X2*(Y3*Y3)+X2*(X3*X3))/
	     Yaux);
  
  /* elijo un punto cualquiera y calculo el radio*/
  *R=(int)(((X1-(*X))*(X1-(*X))+((Y1-(*Y))*(Y1-(*Y)))));
  /* Falta calcular el radio, porque en mi casa no funciona.
     Probar en dept.*/
  *R=(int)pow((double)*R,(double)1/2);
  return 1;

}


/*----------------------------------------------------------------------*/
/* Solo para el control en posicion */

/* Refina las ventanas calculadas anteriormente. En la misma pasada halla los puntos extremos necesarios para calcular las coordenadas de la circunferencia pelota */
float  ExtraePelotas(int *imagen,int *ptosx,int *ptosy,int indicex,int indicey,int *Pelotas,int * eX, int *eY){
  /* A partir de unos puntos saca las esquinas de las ventanas
     1---2
     |   |
     |   |
     3---4
     en ese orden */
  /* primera ventana es la 0 */

  int contx=1,conty=1,j;
  int P1=0,P2=0,P3=0,P4=0;
  int aux2=1,Ma,Mb,Mi,Md;
  int X,Y,R,Pixels;
  int TAM,PIX;
  int pelotas_temp;
  int indice=0;

  pelotas_temp=0;
  
  while(conty<indicey){
    while(contx<indicex){      	
      P1=ptosy[conty]*largo+ptosx[contx];
      P2=ptosy[conty]*largo+ptosx[contx+1];
      P3=ptosy[conty+1]*largo+ptosx[contx];
      P4=ptosy[conty+1]*largo+ptosx[contx+1];
      /* pinto las lineas de las ventanas halladas*/
      for(j=P1;j<=P2;j++)
	imagen[j]=255;
      for(j=P3;j<=P4;j++)
	imagen[j]=255;
      for(j=P1+largo;j<=P3;j=j+largo)
	imagen[j]=255;
      for(j=P2+largo;j<=P4;j=j+largo)
	imagen[j]=255;
      
      contx=contx+2;
    }
    
    
    Ma=Mb=Mi=Md=X=Y=R=0;
    /* Saco maximos de cada ventanas que he sacado */
    Pixels=sacarmax(&imagen[0],&Md,&Mi,&Ma,&Mb,P1,P4);
    /* Calculo los centros */
    if((Ma==0)&&(Mb==0)&&(Mi==0)&&(Md==0)){
      aux2=0;
    }
    else
      if(Ma==0)
	aux2=Circulo(Md,Mi,Mb,&X,&Y,&R);
      else
	if(Mb==0)
	  aux2=Circulo(Md,Mi,Ma,&X,&Y,&R);
	else
	  if(Mi==0)
	    aux2=Circulo(Md,Ma,Mb,&X,&Y,&R);
	  else
	    if(Md==0)
	      aux2= Circulo(Mi,Ma,Mb,&X,&Y,&R);
	    else
	      aux2=Circulo(Md,Mi,Mb,&X,&Y,&R);
    conty=conty+2;
    contx=1;
    if (aux2!=0){
      /* almaceno los centros */
      Pelotas[pelotas_temp+1]=X;
      Pelotas[pelotas_temp+2]=Y;
      Pelotas[pelotas_temp+3]=R;
      Pelotas[pelotas_temp+4]=Pixels;
      pelotas_temp= pelotas_temp+1; /* Numero de pelotas que salen */
    }
  }
  /* Aqui calculo el error*/
  if(pelotas_temp>=1){
    if(pelotas_temp==1){
      
      X=Pelotas[1];
      Y=Pelotas[2];
      TAM=Pelotas[3];      
    }
    else{
      /* hay mas de una pelota elijo la mas grande, la que tenga mas pixels naranjas */
      PIX=Pelotas[indice+3]; /* cojo el tamaño de la primer pelota */ 
      for(indice=2;indice<=pelotas_temp;indice++){
	if(Pelotas[indice*4]>PIX)
	  PIX=Pelotas[indice*4];
      }/* en indice tengo la pelota mas grande */
      X=Pelotas[indice*4-3];
      Y=Pelotas[indice*4-2];
      TAM=Pelotas[indice*4-1];
    }
     

    *eX=OrigenX-X;
    *eY=OrigenY-Y;
   
  }
  else{
    *eX=0;
    *eY=0;
  }

  return aux2;
}  
/*--------------------------------------------------------------------*/
/* Solo para el control en velocidad */

float SacaCentro(int *imagen,int *ptosx,int *ptosy,int indicex,int indicey,int *Pelotas,int * eX, int *eY){
  /* A partir de unos puntos saca las esquinas de las ventanas
     1---2
     |   |
     |   |
     3---4
     en ese orden */
  /* primera ventana es la 0 */

  int contx=1,conty=1,j;
  int P1=0,P2=0,P3=0,P4=0;
  int aux2=1;
  int X,Y,Pixels;
  int TAM,PIX;
  int pelotas_temp;
  int indice=0;
  int f,c;
  int Xmin=0,Xmax=0,Ymin=0,Ymax=0;

  pelotas_temp=0;
	
  while(conty<indicey){
    while(contx<indicex){      	
      P1=ptosy[conty]*largo+ptosx[contx];
      P2=ptosy[conty]*largo+ptosx[contx+1];
      P3=ptosy[conty+1]*largo+ptosx[contx];
      P4=ptosy[conty+1]*largo+ptosx[contx+1];
      /* pinto las lineas de las ventanas halladas*/
      for(j=P1;j<=P2;j++)
	imagen[j]=255;
      for(j=P3;j<=P4;j++)
	imagen[j]=255;
      for(j=P1+largo;j<=P3;j=j+largo)
	imagen[j]=255;
      for(j=P2+largo;j<=P4;j=j+largo)
	imagen[j]=255;
      
      contx=contx+2;
    }
    
    
    X=Y=Pixels=0;
    /* Saco numero pixels naranjas de cada ventanas que he sacado */
    Xmin=P1%largo;
    Xmax=P4%largo;
    Ymin=(int)P1/largo;
    Ymax=(int)P4/largo;
    
    for(f=Ymin;f<=Ymax;f++)
      for(c=0;c<=Xmax-Xmin;c++){
	if(imagen[f*largo+Xmin+c]==255){
	  Pixels++;
	}
      }
    /* Saco centro de la ventana correpondiente */
    if(Xmax>82)
      Xmax=82;
    if(Ymax>62)
      Ymax=62;
    X=(Xmax-Xmin)*0.5+Xmin;
    Y=(Ymax-Ymin)*0.5+Ymin;


    conty=conty+2;
    contx=1;
    if (aux2!=0){
      /* almaceno los centros */
      Pelotas[pelotas_temp+1]=X;
      Pelotas[pelotas_temp+2]=Y;
      Pelotas[pelotas_temp+3]=Pixels;
      pelotas_temp= pelotas_temp+1; /* Numero de pelotas que salen */
    }
  }
  /* Aqui calculo el error*/
  if(pelotas_temp>=1){
    if(pelotas_temp==1){
      
      X=Pelotas[1];
      Y=Pelotas[2];
      TAM=Pelotas[3];
    }
    else{
      /* hay mas de una pelota elijo la mas grande, la que tenga mas pixels naranjas */
      PIX=Pelotas[indice+3]; /* cojo el tamaño de la primer pelota */ 
      for(indice=2;indice<=pelotas_temp;indice++){
	if(Pelotas[indice*4]>PIX)
	  PIX=Pelotas[indice*4];
      }/* en indice tengo la pelota mas grande */
      X=Pelotas[indice*4-3];
      Y=Pelotas[indice*4-2];
      TAM=Pelotas[indice*4-1];
    }
     

    *eX=OrigenX-X;
    *eY=OrigenY-Y;
    
    
    LCDSetPos(2,3);
    LCDPutInt(Xmax);
    LCDSetPos(3,3);
    LCDPutInt(Xmin);
   
  }
  else{
    *eX=0;
    *eY=0;
  }

  return aux2;
} 


/*----------------------------------------------------------------------*/

void inicializa(int *Arr,int tam)
{
  int i;
  for (i=0; i<=tam; i++)
    Arr[i]=0;
}

void ConvierteVisualizacion(int *im, image_XXL img_XXL)
{
  int i,j,k,pos,offset;
  int umbral = 128;
  BYTE num;
 
       for(i = 0; i < ancho; i++) 
  	  for(j = 0; j < largo; j++)
	  {
	    pos = i * 128 + j;
            offset = pos % 8;
	    pos = pos / 8;
            num = 128;
            for (k = 0; k < offset; k++)
                num = num >> 1;

	    if (im[i*largo+j]>umbral)
	        img_XXL[pos] = img_XXL[pos] | num;
            else
                img_XXL[pos] = img_XXL[pos] & (255-num);
          } 
} 

int filtroNaranja(colimage imagen_col,int *imgbin, int *hayimagen,int *HistX, int *HistY){
  /* funcion para pasar de RGB a Y, solo nos interesa
     la componente Y (por ahora) */
  /* Tambien es en esta funcion donde guardo los puntos
     maximos izq, der, arb, abj. */
  /* Imagen [numero de filas][numero de columnas][componente] */

  int i,j,a,b,c;

  *hayimagen=0;

  for (i=0;i<ancho;i++){
    for (j=0;j<largo;j++){ 
      
      a=imagen_col[i][j][verde]*100;
      b=imagen_col[i][j][verde]*200;
      c=imagen_col[i][j][rojo]*100;
      
      if (/*(imagen_col[i][j][rojo]<=255) && */ (imagen_col[i][j][rojo]>=99) && 
	  (imagen_col[i][j][verde]<206) && (imagen_col[i][j][verde]>65) && 
	  (imagen_col[i][j][azul]<=184) && (imagen_col[i][j][azul]>=48) &&
	  (c>a) && (c<b)) {
	
	imgbin[i*largo+j]=255;
	*hayimagen=1;
	HistY[i+1]++;
	HistX[j+1]++;
      }
      else imgbin[i*largo+j]=0;
    }
  }  
  return 0;
}



void final(){
/* Tarea espera a que se pulse cualquier boton. Si es el 4 se acaba la ejecucion del pragrama*/

  float V_lin;  /* Componente proporcional de v.*/
  float T_lin;  /* Componente integral de v.*/
  float V_ang; /* Componente proporcional de w.*/
  float T_ang;  /* Componente integral de w.*/

  int k = 0;
  int cambio2=0;
  int caso;
  
  LCDMenuI(2,"VEL");


  while(1) {
    LCDMenuI(4,"FIN"); /*Muestra botón de fin */
    k = KEYGet();
    switch (k){
    case KEY1: 
      if (mostrar==0){
	mostrar=1;
	LCDMenuI(1,"DPY");
      }
      else{
	mostrar=0;
	LCDMenuI(1,"dpy");
	limpiar=1;
      }
      break;
    case KEY2:
      /* para controlar control en vel o en pos en el eje x */
      if (cambio2==0){
	LCDMenuI(2,"POS");
	OSSemP(semcontrol);
	TipoControl=1;
	OSSemV(semcontrol);
	V_lin=3;
	T_lin=1;
	V_ang=5;
	T_ang=1;
	cambio2=1;
      }
      else{
	LCDMenuI(2,"VEL");
	OSSemP(semcontrol);
	TipoControl=0;
	OSSemV(semcontrol);
	V_lin=5;
	T_lin=0.1;
	V_ang=5;
	T_ang=0.11;
	cambio2=0;
      }
      VWStopControl(vw);
      VWStartControl(vw,V_lin,T_lin,V_ang,T_ang);
      break;
    case KEY3:
      caso = depuravel;
      caso = (caso+1)%3;
      depuravel=caso;
      break;
    case KEY4: 
      /* si es la tecla de fin mato las tareas y a esta misma */
      CAMRelease(); /*Desactiva la cámara*/ 
      VWStopControl(vw);
      VWRelease(vw);
      OSKill(pcaptura);
      OSKill(pmotores);
      OSKill(0);
      break;
    }
  }  
} 



/* Captura una imagen la analiza y devuelve el centro y el radio */
void captura()
{ 
  int i;
  int hayimagen;
  int limpiarM=1;
  int eX,eY;
 

    CAMInit(NORMAL); /*Inicializa la cámara con zoom normal */ 
    CAMSet (FPS1_875, 0, 0); /* TB: set slower frame rate for driving */
    LCDClear();    /*Borra pantalla*/
    /* Pinto los botones */
    mostrar=1;

    indicex=1;
    indicey=1;
    limpiar=0;
    LCDMenuI(1,"DPY");
    LCDMenuI(4,"FIN"); /*Muestra botón de fin */
    for(i=1; i <= 16 * 64; i++) imagen_XXL[i]=0;
     
 
    while (1)  
      {
	/* Inicializo los histogramas 
	inicializa_hist(&HistX[0],largo);
	inicializa_hist(&HistY[0],ancho);*/
	
	CAMGetColFrame(&imagen_col,0);   
	filtroNaranja(imagen_col,&imagen[0],&hayimagen,&HistX[0],&HistY[0]);
	if(hayimagen==1){	 	    
	  indicex=1;
	  indicey=1;
	  Nvent=0;
	  limpiarM=1;
	  
	  SacaBordesVentanas(&HistX[0],largo,&ptosx[0],&indicex);
	  SacaBordesVentanas(&HistY[0],ancho,&ptosy[0],&indicey);
	  
	  /* para conseguir el control en velocidad creo una  nueva funcion
	     que a partir de las ventanas encontradas calculen cual es su centro
	     y tomare ese como el centro de la pelota */
	  SacaCentro(&imagen[0],&ptosx[0],&ptosy[0],indicex,indicey,&Pelotas[0],&eX,&eY);
	  /*ExtraePelotas(&imagen[0],&ptosx[0],&ptosy[0],indicex,indicey,&Pelotas[0],&eX,&eY);*/
	  /*pintar*/
	  
	  if (mostrar==1){
	    ConvierteVisualizacion(&imagen[0],imagen_XXL);
	    LCDPutImage(imagen_XXL);
	    LCDMenuI(1,"DPY");
	    LCDMenuI(4,"FIN");
	  } 
	  else if (limpiar==1)
	    {
	      LCDClear();
	      LCDMenuI(1,"dpy");
	      LCDMenuI(4,"FIN");
	      limpiar=0;
	    }
	  
	  contador++;
	  LCDSetPos(0,11);
	  LCDPutInt(contador);
	  
	}
	else{
	  if(limpiarM==1){
	    LCDClear();
	    LCDMenuI(1,"DPY");
	    LCDMenuI(4,"FIN");
	    /* deberia poner a cero el array de Pelotas */
	    inicializa(&Pelotas[0],indPelotas);
	    limpiarM=0;
	    eX=0;
	    eY=0;
	  }	
	}
	
	OSSemP(sem);
	errorx=eX;
	errory=eY;
	OSSemV(sem);
	
      }
    
    
    
}

/* ----------------------------------------------------------------------- */
/**Codigo para motores
   se ocupa del manejo de los motores dependiendo de los valores devueltos
   por Captura
*/
void motores()
{


  int aux=0;
  float gradosX,velX=0;
  float distanciaY,velY=0;
  int XXX,YYY;
  int Tipocontrol_tmp;
  int caso;
  /* variables que indican la velocidad */
  float velxn=3.6,velxpa=21;
  float velyn=0.8,velypa=1.22;
  float velresx, velresy;
  /*------------------------------------*/
  int ex,exaux,ey,eyaux;
  float valresi,valresd;
  int valord,valori,valordant,valoriant;
  int umbral=3;

  /* inicializo los motores */ 
  vw=VWInit(VW_DRIVE,1);

  /* Leo los encoders para tener una primera referencia */
  valoriant=QUADRead(0x0c02); /* encoder izquierdo */
  valordant=QUADRead(0x0304); /* encoder derecho */
    
  while(1){
    
    /* vuelvo a leer los encoders para compararlos con los anteriores
       y poner la velocidad adecuada */
    valori=QUADRead(0x0c02); /* encoder izquierdo */
    valord=QUADRead(0x0304); /* encoder derecho */
    
    valresi=valori-valoriant;
    valresd=valord-valordant;
    
    /* actualizo valores */
    valoriant=valori;
    valordant=valord;
    
    /* comparo esto con el umbral */
    OSSemP(sem);
    ex=errorx;
    ey=errory;
    OSSemV(sem);

    if ((valresi>=-umbral && valresi<=umbral) || (valresd>=-umbral && valresd<=umbral)){
      LCDSetPos(6,11);
      LCDPrintf("pa");
      velresx=velxpa;
      velresy=velypa;
    }
    else{
     
      /*LCDPrintf("ex");*/
      if (ex<0)
	exaux=-1*ex;
      else
	exaux=ex;
      if (ey<0)
	eyaux=-1*ey;
      else
	eyaux=ey;
     
      velresx=velxn*sqrt((float)exaux/40);
      velresy=velyn*sqrt((float)eyaux/30);
      LCDSetPos(6,11);
      LCDPutFloatS(velresx,2,2);
    }
    
    /* depues que tengo las coordenadas correctas procedo a mover el robot*/
    /*POR AHORA SOLO EN X*/
    OSSemP(semcontrol);
    Tipocontrol_tmp=TipoControl;
    OSSemV(semcontrol);  
    if (Tipocontrol_tmp==0){
      /* Control en velocidad */
      LCDMenuI(2,"VEL");
      caso=depuravel;
      switch(caso){
      case 0:
	/* normal */
	if(ey<=4&&ey>=-4)
	  velY=0;
	else
	  if (ey>4)
	    velY=velresy;
	  else
	    velY=-velresy;
       	if(ex<=4&&ex>=-4)
	  velX=0;
	else{
	  if ((velY==velypa)||(velY==-velypa))
	    velresx=1;
	  if (ex>4)
	    velX=velresx;
	  else
	    velX=-velresx;
	}
	
	LCDMenuI(3,"N");
	break;
      case 1:
	/* no gira */
	if(ey<=4&&ey>=-4)
	  velY=0;
	else
	  if (ey>4)
	    velY=velresy;
	  else
	    velY=-velresy;
	velX=0;
	LCDMenuI(3,"A");
	break;
      case 2:
	/* no avanza */
	if(ex<=4&&ex>=-4)
	  velX=0;
	else	 
	  if (ex>4)
	    velX=velresx;
	  else
	    velX=-1*velresx;
	velY=0.1;
	LCDMenuI(3,"G");
	break;
      }
      
      LCDSetPos(4,11);
      LCDPutFloatS(velX,1,2);
      LCDSetPos(5,11);
      LCDPutFloatS(velY,1,2);

      /*LCDSetPos(2,3);
      LCDPutInt(valresd);
      LCDSetPos(3,3);
      LCDPutInt(valresi);*/
    
      VWSetSpeed(vw,velY,velX);
    }
    else{
      /* control incremental en posicion */
      LCDMenuI(2,"POS");
      OSSemP(sem);
      XXX=errorx;
      YYY=errory;
      OSSemV(sem);

      if(XXX>4)
	gradosX=grados;
      else
	if(XXX<=4 && XXX>=-4)
	  gradosX=0;
	else
	  gradosX=-grados;
      
      if(YYY>2)
	distanciaY=distancia;
      else
	if(YYY<=2 && YYY>=-6)
	  distanciaY=0;
	else{
	  distanciaY=-distancia;
	}
      
      LCDSetPos(4,11);
      LCDPutFloatS(gradosX,1,2);
      LCDSetPos(5,11);
      LCDPutFloatS(distanciaY,1,2);
      

      VWDriveTurn(vw,gradosX,90);
      /*VWDriveWait(vw);*/
      while(!VWDriveDone(vw)){
      }
      VWDriveStraight(vw,distanciaY,60);
      VWDriveWait(vw);
    }
    
    OSSleep(10);

    aux++;

    LCDSetPos(2,11);
    LCDPutInt(aux);
  
  }
  
} 


/* ----------------------------------------------------------------------- */
/** main del programa */
int main()
{ 
 
  /** clear display and set display mode */
  LCDClear();
  LCDMode(SCROLLING|CURSOR);

  /** initialize multithreading */
  LCDPrintf("Kernel.. ");
  OSMTInit(PREEMPT);
  LCDPrintf("done\n");

 /** Creo una variable semaforo */
  OSSemInit(sem,1);
  OSSemInit(semcontrol,1);
  OSSemV(sem);/* la inicializo por si acaso*/
  OSSemP(semcontrol);
  TipoControl=0;
  OSSemV(semcontrol);
  
  /** Inicializo las hebras
      Spawn necesarios: punteros al bloque de control 
      del nombre de la hebra,un puntero a su pila
      un puntero a su codigo, el tamaño de su pila; su prioridad
      y devuelve: un puntero al bloque de control inicializado 
  */
  LCDPrintf("Spawn..");
  pfinal = OSSpawn("Final",final, SSIZE, MIN_PRI,1);
  if(!pfinal) OSPanic("spawn para final ha fallado");
  pcaptura = OSSpawn("Captura",captura, SSIZE, MIN_PRI,2);
  if(!pcaptura) OSPanic("spawn para captura ha fallado");
  pmotores = OSSpawn("Motores",motores, SSIZE, MIN_PRI,3);
  if(!pmotores) OSPanic("spawn para motores ha fallado");
  LCDPrintf("done\n");
  LCDClear();
  
  /** READY all threads and start multitasking */
  OSReady(pfinal);
  OSReady(pcaptura);
  OSReady(pmotores);
  OSPermit();  /* activate preemptive multitasking */
  OSWait(100); /* startup time */



/** processing will return HERE, when there is no READY thread left */ 
  return 0;
}