Diff: recoge_manzanas.cpp

Revision:

3:81dc55b0a8d7

Parent:

2:484928b9a5b5

Child:

4:3426b2472f99

--- a/recoge_manzanas.cpp	Sun Nov 18 03:37:38 2018 +0000
+++ b/recoge_manzanas.cpp	Sun Nov 18 20:49:38 2018 +0000
@@ -22,6 +22,8 @@
 #define  MINC 128
 #define  MAXC 1
 #define  DEL 0.1
+#define  GIRO 2
+#define  G_DIAG 2
 
 int columna=1,manzanas=0;
 int fila=1, fil=1, colum=1;
@@ -122,6 +124,59 @@
 //--------------------------------------------------   
 void revisar_mov(uint8_t correr){
     switch (correr){
+        
+        case 2:
+        columna=columna<<1;
+        fila--;
+        if (columna>MINC)
+            columna=MAXC;
+        if (fila<MIN){
+            fila=MAX;
+            sendSPI(MIN,0);
+            }
+        sendSPI(fila+1,0);
+        break;
+        
+        case 4:
+        columna=columna<<1;
+        fila++;
+        if (fila>MAX){
+            fila=MIN;
+            sendSPI(MAX,0);
+            }
+        if (columna>MINC)
+            columna=MAXC;
+        sendSPI(fila-1,0);
+        break;
+        
+        case 1:
+        columna=columna>>1;
+        fila--;
+        if (columna<MAXC)
+            columna=MINC;
+        if (fila<MIN){
+            fila=MAX;
+            sendSPI(MIN,0);
+            }
+        sendSPI(fila+1,0);
+        //command.printf("\n case1 Col:%d",columna);
+        //command.printf("\n FIL:%d",fila);
+        break;
+        
+        case 3:
+        columna=columna>>1;
+        fila++;
+        if (columna<MAXC)
+            columna=MINC;
+        if (fila>MAX){
+            fila=MIN;
+            sendSPI(MAX,0);
+            }
+        sendSPI(fila-1,0);
+        //command.printf("\n case3 Col:%d",columna);
+        //command.printf("\n FIL:%d",fila);
+        break;
+        
         case 28:
         columna=columna<<1;
         if (columna>MINC)
@@ -136,18 +191,20 @@
         
         case 30:
         fila--;
+        if (fila<MIN){
+            fila=MAX;
+            sendSPI(MIN,0);
+            }
         sendSPI(fila+1,0);
-        if (fila<MIN)
-            fila=MAX;
-            sendSPI(fila+1,0);
         break;
         
         case 31:
         fila++;
+        if (fila>MAX){
+            fila=MIN;
+            sendSPI(MAX,0);
+            }
         sendSPI(fila-1,0);
-        if (fila>MAX)
-            fila=MIN;
-            sendSPI(fila-1,0);
         break;  
         
         case 0:
@@ -155,6 +212,35 @@
         }
     }
     
+//--------------------------------------------------
+//-------------------VELOCIDAD-----------------------
+//-------------------------------------------------- 
+int velocidad(float _acc[3])
+{
+    uint8_t vel;
+    int acc[3];
+        acc[0]=_acc[0];
+        acc[1]=_acc[1];
+       if((acc[0]>GIRO) || (acc[0]<-GIRO) || (acc[1]>GIRO) || (acc[1]<-GIRO))
+        {
+                if(acc[0]>GIRO)
+                    vel=acc[0];
+                else if(acc[0]<-GIRO)
+                    vel=-acc[0];
+                else if(acc[1]>GIRO)
+                    vel=acc[1];
+                else if(acc[1]<-GIRO)
+                    vel=-acc[1];
+                command.printf("---Velocidad if %d \n", vel);     
+         wait(3/vel);           
+        }
+    int v=1/vel;
+    
+    command.printf("Velocidad %d \n", v);    
+    command.printf("x %d \n", acc[1]);
+    command.printf("y %d \n", acc[0]);
+    return vel;
+}
     
 //--------------------------------------------------
 //-------------------BUSCADOR------------------------
@@ -166,20 +252,32 @@
     float acc[3]; Wire.getAccelero(acc);     
   //command.printf("Accelerometer: \t X= %f, \t Y= %f, \t Z=%f \n", acc[0],acc[1],acc[2]);
   //command.printf("Gyroscope: \t X= %f, \t Y= %f, \t Z=%f \n", gyro[0],gyro[1],gyro[2]);
- 
-    wait(DEL);
-
-    int giro=3;
-        if(acc[0]>giro)         //eje x derecha
-            correr=30;          
-        else if(acc[0]<-giro)   //eje x izquierda
-            correr=31;          
-        else if(acc[1]>giro)    //eje y arriba
-            correr=29;
-        else if(acc[1]<-giro)   //eje y abajo
-            correr=28;
-        else
-        correr=0; //
+        velocidad(acc);
+        //command.printf("Velocidad %d \n", vel);
+        if(acc[0]>GIRO)         //eje x derecha
+        {
+            if(acc[1]>G_DIAG)     //eje y arriba
+            correr=1;
+            else if(acc[1]<-G_DIAG)//eje y abajo
+            correr=2;
+            else
+            correr=30;
+            }          
+    else if(acc[0]<-GIRO)   //eje x izquierda
+        {
+            if(acc[1]>G_DIAG)     //eje y arriba
+                    correr=3;
+                    else if(acc[1]<-G_DIAG)//eje y abajo
+                    correr=4;
+                    else
+                    correr=31;
+                    }           
+    else if(acc[1]>GIRO)    //eje y arriba
+                    correr=29;
+    else if(acc[1]<-GIRO)   //eje y abajo
+                    correr=28;
+                else
+                correr=0; //
         revisar_mov(correr);
         //wait (DEL);
     //command.printf("\n correr:\n %d",correr);