Diff: main.cpp

Revision:

8:79701df1b502

Parent:

7:cb59c58cab78

Child:

9:2c4bf2c1ccdc

--- a/main.cpp	Fri Jun 09 01:14:42 2017 +0000
+++ b/main.cpp	Mon Jun 12 11:02:13 2017 +0000
@@ -31,127 +31,145 @@
 Serial bt(PA_2, PA_3);          // (RX, TX) (puerto serial 2)
 
 // declaramos otras variables que utilizaremos
-Ticker ticker1, ticker2;        // iniciamos dos tickers
+Ticker ticker1, tiker2;         // iniciamos dos tickers
 DigitalOut led1 (LED1);         // declaramos el LED1 
 DigitalOut led2 (LED2);         // declaramos el LED2 
 DigitalOut led3 (LED3);         // declaramos el LED3 
 int pulsos[6]={0,0,0,0,0,0};    // iniciamos el array pulsos
 int posiciones[10][6]={0};      // declaramos los arrays de posiciones 
 char dato=0;
+float V=1;
 
 // declaramos el prototipo de las funciones que utilizaremos
 void enviar_bt();               // prototipo de la funcion que ejecutara el ticker1
 void encenderLEDs();            // prototipo de la funcion que ejecutara el ticker1
 void sethome();                 // prototipo de la funcion que nos calibrara la posicion hard home de referencia
-void posicion();                // prototipo de la funcion que nos llevara hasta la posicion especificada
+void posicion(int);             // prototipo de la funcion que nos llevara hasta la posicion especificada
 void mover(int,int,float);      // prototipo de la funcion que movera cada motor
 void frenar(int);               // prototipo de la funcion que frenara cada motor
 void pinza(int);                // prototipo de la funcion que abrira (-1) o cerrara (1) la pinza
 void movCinta();                // prototipo de la funcion que avanzara la cinta hasta detectar un objeto
+float map(long,long,long,float,long);   // prototipo de la funcion que hace una redimension de los parametros
 
 int main()
 {
-    m1.period(0.0001);      // establece el periodo del PWM, equivalente a una frecuencia de 10KHz.
-    m2.period(0.0001);
-    m3.period(0.0001);
-    m4.period(0.0001);
-    m5.period(0.0001);
-    m6.period(0.0001);
+    m1.period(0.001);      // establece el periodo del PWM, equivalente a una frecuencia de 1KHz.
+    m2.period(0.001);
+    m3.period(0.001);
+    m4.period(0.001);
+    m5.period(0.001);
+    m6.period(0.001);
     
     bt.baud(9600);          // velocidad de transmision del bluetooth a 9600 baudios
     
-    ticker1.attach(&enviar_bt, 0.5);        // asignamos la funcion "enviar_bt" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.5s
-    ticker2.attach(&encenderLEDs, 0.05);    // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s
+    ticker1.attach(&enviar_bt, 0.2);    // asignamos la funcion "enviar_bt" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.2s
+    ticker2.attach(&encenderLEDs, 0.1);    // asignamos la funcion "encenderLEDs" al protocolo de interrupcion del "ticker1" y le damos un intervalo de 0.2s
+
 
     sethome();              // establecemos la posicion de referencia
     
     while(1)
     {
-        if(bt.readable()>0)
+        if(bt.readable()>0)             // si hay un dato disponible:
         {
-            dato = bt.getc();
+            dato = bt.getc();           // leemos un caracter
+            if (dato<=10 && dato>=1)    // si tiene un valor entre 1 y 10 lo asignamos como velocidad
+                V=map(dato,1,10,0.6,1);
             switch(dato)
             {   
-                case 'A':
-                mover(1,1,1);
+                case 'A':       // si el caracter es A se mueve el motor 1 al derecho
+                mover(1,1,V);
                 wait(0.2);
                 frenar(1);
                 break;
-                case 'B':
-                mover(1,-1,1);
+                case 'B':       // si el caracter es B se mueve el motor 1 al inverso
+                mover(1,-1,V);
                 wait(0.2);
                 frenar(1);
                 break;
-                case 'C':
-                mover(2,1,1);
+                case 'C':       // si el caracter es C se mueve el motor 2 al derecho
+                mover(2,1,V);
                 wait(0.2);
                 frenar(2);
                 break;
-                case 'D':
-                mover(2,-1,1);
+                case 'D':       // si el caracter es D se mueve el motor 2 al inverso
+                mover(2,-1,V);
                 wait(0.2);
                 frenar(2);
                 break;
-                case 'E':
-                mover(3,1,1);
+                case 'E':       // si el caracter es E se mueve el motor 3 al derecho
+                mover(3,1,V);
                 wait(0.2);
                 frenar(3);
                 break;
-                case 'F':
-                mover(3,-1,1);
+                case 'F':       // si el caracter es F se mueve el motor 3 al inverso
+                mover(3,-1,V);
                 wait(0.2);
                 frenar(3);
                 break;
-                case 'G':
-                mover(4,1,1);
+                case 'G':       // si el caracter es G se mueve el motor 4 Y 5 al derecho
+                mover(14,1,V);
                 wait(0.2);
-                frenar(4);
+                frenar(14);
                 break;
-                case 'H':
-                mover(4,-1,1);
+                case 'H':       // si el caracter es H se mueve el motor 4 Y 5 al inverso
+                mover(14,-1,V);
                 wait(0.2);
-                frenar(4);
+                frenar(14);
                 break;
-                case 'I':
-                mover(5,1,1);
+                case 'I':       // si el caracter es I se mueve el motor 4 Y 5 al derecho
+                mover(15,1,V);
                 wait(0.2);
-                frenar(5);
+                frenar(15);
                 break;
-                case 'J':
-                mover(5,-1,1);
+                case 'J':       // si el caracter es J se mueve el motor 4 Y 5 al inverso
+                mover(15,-1,V);
                 wait(0.2);
-                frenar(5);
+                frenar(15);
                 break;
-                case 'K':
+                case 'K':       // si el caracter es K se abre la pinza
                 pinza(-1);
                 break;
-                case 'L':
+                case 'L':       // si el caracter es L se cierra la pinza
                 pinza(1);
                 break;
-                case 'M':
+                case 'M':       // si el caracter es M se avanza la cinta
                 movCinta();
                 break;
+                case 'N':       // si el caracter es N se mueve a la posicion home
+                posicion(0);
+                pinza(-1);
+                break;
             }
         }
     }
 }
 
+float map(long x, long in_min, long in_max, float out_min, long out_max)    // funcion que redimensiona los parametros
+{
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
 void enviar_bt()            // definimos la funcion que ejecutara el ticker
 {
-    pulsos[1]=enc1.getPulses();
+    pulsos[1]=enc1.getPulses();     // cargamos los contadores de los encoders
     pulsos[2]=enc2.getPulses();
     pulsos[3]=enc3.getPulses();
     pulsos[4]=enc4.getPulses();
     pulsos[5]=enc5.getPulses();
-    bt.printf(" pulsos encoder 1:%d\n pulsos encoder 2:%d\n pulsos encoder 3:%d\n pulsos encoder 4:%d\n pulsos encoder 5:%d\n", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5]); // envia el valor de "pulsos" por bluetooth
+    bt.printf(" pulsos encoder 1:%d\n pulsos encoder 2:%d\n pulsos encoder 3:%d\n pulsos encoder 4:%d\n pulsos encoder 5:%d\n", pulsos[1],pulsos[2],pulsos[3],pulsos[4],pulsos[5]); // envia los valores del array "pulsos" por bluetooth
 }
 
 void encenderLEDs()
 {
-    if (mp5==0) led1=1;     // si se pulsa el mp1 se enciende el LED1
-    else led1=0;            // si no se apaga
-    if (mp4==0) led3=1;     // si se pulsa el mp1 se enciende el LED1
-    else led3=0;            // si no se apaga
+if(mp1==0)
+    led1=1;
+else if (mp1==1)
+    led1==1;
+if(mp2==0)
+    led3=1;
+else if (mp2==1)
+    led3==1;
 }
 
 void sethome()
@@ -164,7 +182,7 @@
     {
         enc=enc1.getPulses();   // leemos el contador del encoder
         mover(1,dir,0.7);       // movemos el motor en la dirección inicial
-        wait(0.02);             // esperamos 0.01s
+        wait(0.01);             // esperamos 0.01s
         if (enc==enc1.getPulses())  // Si el contador no se ha incrementado, hemos llegado al extremo
             dir=1;              // se cambia la dirección del motor
     }
@@ -181,7 +199,7 @@
         {
             enc=enc2.getPulses();   // leemos el valor de los encoders
             mover(2,dir,0.7);   // movemos el motor
-            wait(0.02);         // esperamos 0.01s
+            wait(0.01);         // esperamos 0.01s
             if(enc==enc2.getPulses())   // Si el contador no ha cambiado
             {
                 if (dir==-1)
@@ -204,7 +222,7 @@
         {
             enc=enc3.getPulses();   // leemos el valor de los encoders
             mover(3,dir,0.7);   // movemos el motor
-            wait(0.02);         // esperamos 0.01s
+            wait(0.01);         // esperamos 0.01s
             if(enc==enc3.getPulses())   // Si el contador no ha cambiado
             {
                 if (dir==-1)
@@ -221,13 +239,13 @@
     while (dir==-1)              // Mientras que la dirección del motor sea 1:
     {
         while (mp4==0)          // Mientras el MP esté pulsado
-            mover(4,dir,0.7);   // movemos los motores 4 y 5
+            mover(14,dir,0.7);   // movemos los motores 4 y 5
         dir=1;
         while (mp4==1)          // Mientras el MP esté sin pulsar:
         {
             enc=enc5.getPulses();   // leemos el valor del los encoders
-            mover(4,dir,0.7);   // movemos los motores
-            wait(0.02);         // esperamos 0.01s
+            mover(14,dir,0.7);   // movemos los motores
+            wait(0.01);         // esperamos 0.01s
             if(enc==enc5.getPulses())   // Si el contador no ha cambiado
             {
                 if (dir==-1)
@@ -237,16 +255,19 @@
             }
         }
     }
-    frenar(4);                  // la muñeca ha llegado a la posicion adecuada y se frenan los motores
+    frenar(14);                  // la muñeca ha llegado a la posicion adecuada y se frenan los motores
     
     // posicionamiento de la rotacion de la muñeca
     while (mp5==1)              // Mientras no se pulse el MS
     {
-        mover(5,1,0.7);         // se mueven los motores 4 y 5 
+        mover(15,1,0.7);         // se mueven los motores 4 y 5 
         wait(0.02);             // esperamos 0.01s
     }
-    frenar(5);                  // frenamos los motores
+    frenar(15);                  // frenamos los motores
+    
     pinza(-1);                  // abrimos la pinza
+    
+    // ponemos el contador de los encoders a 0
     frenar(0);                  // frenamos todos los motoes
     enc1.reset();               // reseteamos los encoders
     enc2.reset();
@@ -259,7 +280,51 @@
 
 void posicion(int pos)
 {
-    
+    frenar(0);                      // nos aseguramos de que los motores estan parados
+    pulsos[1]=enc1.getPulses();     // leemos los valores de los conadores de los encoders
+    pulsos[2]=enc2.getPulses();
+    pulsos[3]=enc3.getPulses();
+    pulsos[4]=enc4.getPulses();
+    pulsos[5]=enc5.getPulses();
+    int dir[6]={0,0,0,0,0,0};       // declaramos un array para las direcciones
+    int flag[6]={1,1,1,1,1,1};      // declaramos un array para banderas
+    int i;
+    for (i=1;i<6;i++)               // establecemos la direccion a la que tiene que ir cada motor 
+    {
+        if (posiciones[pos][i]<pulsos[i])
+            dir[i]=-1;
+        else if (posiciones[pos][i]>pulsos[i])
+            dir[i]=1;
+        else if (posiciones[pos][i]==pulsos[i])
+            flag[i]=0;
+    }
+    while (flag[1]+flag[2]+flag[3]+flag[4]+flag[5]) // mientras que alguna de las banderas este activada:
+    {
+        for (i=1;i<6;i++)       // repetimos para cada uno de los motores
+        {
+            if (flag[i]==1)     // si la bandera del motor i esta activa
+            {
+                mover(i,dir[i],V);  // movemos el motor en la direccion establecida 
+                if (dir[i]==1 && pulsos[i]>=posiciones[pos][i]) // si ha llegado a la posicion deseada
+                {
+                    frenar(i);      // frenamos el motor i
+                    flag[i]=0;      // desactivamos su bandera
+                }
+                if (dir[i]==-1 && pulsos[i]<=posiciones[pos][i]) // si ha llegado a la posicion deseada
+                {
+                    frenar(i);      // frenamos el motor i
+                    flag[i]=0;      // desactivamos su bandera
+                }
+            }
+        }
+        wait(0.001);                     // esperamos 0.001s
+        pulsos[1]=enc1.getPulses();     // leemos la nueva posicion de los encoders
+        pulsos[2]=enc2.getPulses();
+        pulsos[3]=enc3.getPulses();
+        pulsos[4]=enc4.getPulses();
+        pulsos[5]=enc5.getPulses();
+    }
+wait(0.1);              // esperamos 0.1s
 }
 
 void mover(int m, int dir, float v)
@@ -284,17 +349,22 @@
             case 3:             // en caso de que el valor sea 3 movemos el motor 3 (codo)
             m3.speed(dir*v);
             break;
-            case 4:             // en caso de que el valor sea 4 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
-            m4.speed((-1)*dir*v);
-            m5.speed(dir*v);
+            case 4:             // en caso de que el valor sea 4 movemos el motor 4 
+            m4.speed(dir*v);
             break;
-            case 5:             // en caso de que el valor sea 5 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
-            m4.speed(dir*v);
+            case 5:             // en caso de que el valor sea 5 movemos el motor 5
             m5.speed(dir*v);
             break;
             case 6:             // en caso de que el valor sea 6 movemos el motor 6 (pinza)
             m6.speed(dir*v);
             break;
+            case 14:             // en caso de que el valor sea 14 movemos el motor 4 en un sentido y el 5 en el otro (girar muñeca)
+            m4.speed((-1)*dir*v);
+            m5.speed(dir*v);
+            break;
+            case 15:             // en caso de que el valor sea 15 movemos los motores 4 y 5 en el mismo sentido (rotar muñeca)
+            m4.speed(dir*v);
+            m5.speed(dir*v);
         }
     }
     else        // si los parametros no son correctos encendemos el LED3
@@ -304,7 +374,7 @@
 void pinza(int a)
 {
     int enc=0, encOld=0;
-    mover(6,a,1);             // movemos la pinza
+    mover(6,a,1);               // movemos la pinza
     encOld=enc6.getPulses();    // leemos la posicion del encoder
     wait(0.1);                  // espera 0.1s
     enc=enc6.getPulses();       // leemos la posicion nueva del encoder
@@ -338,27 +408,28 @@
         case 3:         // en caso de que el valor sea 3 frenamos el motor 3
         m3.brake();
         break;
-        case 4:         // en caso de que el valor sea 4 frenamos los motores 4 y 5
+        case 4:         // en caso de que el valor sea 4 frenamos el motor 4 
         m4.brake();
-        m5.brake();
         break;
-        case 5:         // en caso de que el valor sea 5 frenamos los motores 4 y 5
-        m4.brake();
+        case 5:         // en caso de que el valor sea 5 frenamos el motor 5
         m5.brake();
         break;
         case 6:         // en caso de que el valor sea 6 frenamos el motor 6
         m6.brake();
         break;
+        case 14:         // en caso de que el valor sea 4 frenamos los motores 4 y 5
+        m4.brake();
+        m5.brake();
+        break;
+        case 15:         // en caso de que el valor sea 5 frenamos los motores 4 y 5
+        m4.brake();
+        m5.brake();
     }
 }
 void movCinta()
 {
-    while (sensor==1)
-        cinta.speed(1);
-    cinta.brake();
+    while (sensor==1)   // mientras el sensor no se active:
+        cinta.speed(1); // avanza la cinta
+    cinta.brake();      // cuando se ectiva la cinta se para el sensor
 }
 
-
-
-
-