Diff: main.cpp

Revision:

6:644b77b2111e

Parent:

5:053c61912254

Child:

7:cb59c58cab78

--- a/main.cpp	Thu Jun 08 19:08:52 2017 +0000
+++ b/main.cpp	Fri Jun 09 01:06:34 2017 +0000
@@ -31,40 +31,42 @@
 Serial bt(PA_2, PA_3);          // (RX, TX) (puerto serial 2)
 
 // declaramos otras variables que utilizaremos
-Ticker ticker1, ticker2, ticker3;        // iniciamos dos tickers
+Ticker ticker1, ticker2;        // 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];             // declaramos el array de posiciones 
+int posiciones[10][6];          // declaramos los arrays de posiciones 
+for (int i=0; i<10; i++)
+    for (int j=0; j<10; j++)
+        posiciones[i][j];
 char dato=0;
 
 // declaramos el prototipo de las funciones que utilizaremos
 void enviar_bt();               // prototipo de la funcion que ejecutara el ticker1
-void encenderLED1();            // prototipo de la funcion que ejecutara el ticker1
-void encenderLED3();            // 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 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
 
 int main()
 {
-    m1.period(0.001);      // establece el periodo del PWM, equivalente a una frecuencia de 10KHz.
-    m2.period(0.001);
-    m3.period(0.001);
-    m4.period(0.001);
-    m5.period(0.001);
-    m6.period(0.001);
+    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);
     
-    bt.baud(9600);              // velocidad de transmision del bluetooth a 9600 baudios
+    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(&encenderLED1, 0.05);    // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s
-    ticker3.attach(&encenderLED3, 0.05);    // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s
+    ticker2.attach(&encenderLEDs, 0.05);    // asignamos la funcion "encenderLED1" al protocolo de interrupcion del "ticker2" y le damos un intervalo de 0.05s
 
-    sethome();                  // establecemos la posicion de referencia
+    sethome();              // establecemos la posicion de referencia
     
     while(1)
     {
@@ -129,6 +131,9 @@
                 case 'L':
                 pinza(1);
                 break;
+                case 'M':
+                movCinta();
+                break;
             }
         }
     }
@@ -141,17 +146,13 @@
     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 el valor de "pulsos" por bluetooth
 }
 
-void encenderLED1()
+void encenderLEDs()
 {
     if (mp5==0) led1=1;     // si se pulsa el mp1 se enciende el LED1
     else led1=0;            // si no se apaga
-}
-
-void encenderLED3()
-{
     if (mp4==0) led3=1;     // si se pulsa el mp1 se enciende el LED1
     else led3=0;            // si no se apaga
 }
@@ -161,31 +162,29 @@
     led2=1;
     // posicionar base
     int enc=0;
-    int dir=1;                  // mover primero hacia delante
+    int dir=-1;                 // mover primero hacia delante
     while (mp1==1)              // Mientras no se pulse el microswitch
     {
         enc=enc1.getPulses();   // leemos el contador del encoder
         mover(1,dir,0.7);       // movemos el motor en la dirección inicial
-        wait(0.01);             // esperamos 0.01s
+        wait(0.02);             // 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
+            dir=1;              // se cambia la dirección del motor
     }
     frenar(1);                  // el motor ha llegado a la posicion adecuada y se frena
     
     // posicionar brazo
-    dir=1;                      // mover primero hacia delante
-    while (dir==1)              // mientras la direccion sea hacia delante:
+    dir=-1;                     // mover primero hacia delante
+    while (dir==-1)             // mientras la direccion sea hacia delante:
     {
         while (mp2==0)          // Mientras se mantenga pulsado el boton
-        {
             mover(2,dir,0.7);   // se seguira moviendo el motor en la direccion inicial
-        }
-        dir=-1;                 // se cambia a direccion
+        dir=1;                  // se cambia a direccion
         while (mp2==1)          // Mientras el MP esté sin pulsar:
         {
             enc=enc2.getPulses();   // leemos el valor de los encoders
             mover(2,dir,0.7);   // movemos el motor
-            wait(0.01);         // esperamos 0.01s
+            wait(0.02);         // esperamos 0.01s
             if(enc==enc2.getPulses())   // Si el contador no ha cambiado
             {
                 if (dir==-1)
@@ -198,17 +197,17 @@
     frenar(2);                  // el motor ha llegado a la posicion adecuada y se frena
     
     // posicionar antebrazo
-    dir=1;                      // mover primero hacia delante
-    while (dir==1)              // mientras la direccion sea hacia delante:
+    dir=-1;                      // mover primero hacia delante
+    while (dir==-1)              // mientras la direccion sea hacia delante:
     {
         while (mp3==0)          // Mientras se mantenga pulsado el boton
             mover(3,dir,0.7);   // se seguira moviendo el motor en la direccion inicial
-        dir=-1;                 // se cambia a direccion
+        dir=1;                 // se cambia a direccion
         while (mp3==1)          // Mientras el MP esté sin pulsar:
         {
             enc=enc3.getPulses();   // leemos el valor de los encoders
             mover(3,dir,0.7);   // movemos el motor
-            wait(0.01);         // esperamos 0.01s
+            wait(0.02);         // esperamos 0.01s
             if(enc==enc3.getPulses())   // Si el contador no ha cambiado
             {
                 if (dir==-1)
@@ -221,17 +220,17 @@
     frenar(3);                  // el motor ha llegado a la posicion adecuada y se frena
     
     // posicionamiento del giro de la muñeca
-    dir=1;                      // mover primero hacia delante
-    while (dir==1)              // Mientras que la dirección del motor sea 1:
+    dir=-1;                      // mover primero hacia delante
+    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
-        dir=-1;
+        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.01);         // esperamos 0.01s
+            wait(0.02);         // esperamos 0.01s
             if(enc==enc5.getPulses())   // Si el contador no ha cambiado
             {
                 if (dir==-1)
@@ -247,7 +246,7 @@
     while (mp5==1)              // Mientras no se pulse el MS
     {
         mover(5,1,0.7);         // se mueven los motores 4 y 5 
-        wait(0.01);             // esperamos 0.01s
+        wait(0.02);             // esperamos 0.01s
     }
     frenar(5);                  // frenamos los motores
     pinza(-1);                  // abrimos la pinza
@@ -354,9 +353,15 @@
         m6.brake();
         break;
     }
-    
+}
+void movCinta()
+{
+    while (sensor==1)
+        cinta.speed(1);
+    cinta.brake();
 }
 
 
 
 
+