Diff: main.cpp

Revision:

0:4b1e8862ad53

Child:

1:9430a3a15f7a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 14 20:37:28 2018 +0000
@@ -0,0 +1,503 @@
+#include "mbed.h"
+Serial pc(USBTX, USBRX);    // leer comnunicacion serial
+Serial pcBT(D8, D2);        // tx, rx
+AnalogIn analog_value(A0);  // definimos puerto para leer analogamente el joystick
+PwmOut myServo1(PC_7);      // definir puertos de los servos
+PwmOut myServo2(PB_4);
+PwmOut myServo3(PB_5);
+PwmOut myServo4(PB_8);
+PwmOut myServo5(PA_6);
+PwmOut myServo6(PB_9);
+PwmOut myServo7(PA_8);
+PwmOut myServo8(PA_7);
+DigitalOut led(LED1,0);     //led indicador de interrupcion
+DigitalOut S0(PA_13);       //definimos los puertos del sensor
+DigitalOut S1(PA_14);
+DigitalOut S2(PA_15);
+DigitalOut S3(PC_8);
+DigitalIn  leer(PC_9);      // puerto donde leemos la frecuencia que envia el sensor
+Timer tiempo;               // timer para uso del sensor
+Timeout timeout;            //creamos el objeto timeout
+unsigned int lectura();     //respuesta de lectura de sensor
+unsigned int inicio=0, final=0, resultado=0, rojo=0,verde=0,azul=0;// variables que se usan en los procesos
+float meas_r;               //variables del joystick
+float meas_v;
+void moving();
+void grupomover();
+void mover_ser(uint8_t nmotor, uint8_t pos);
+void timer_interrupt();
+void read_command();        // definimos los 4 numeros hexadecimales enviados
+void read_commandbt();      // definimos los 4 numeros hexadecimales enviados
+void adelante();
+void atras();
+void init_servo();
+char init_c;
+int read_cc;
+uint32_t grados;
+uint32_t nmotor;
+int endc;
+int main()
+{
+    pc.baud(9600);                              // leer en el monitor serial
+    pc.printf("inicio\n");
+    timeout.attach(&timer_interrupt,0.5);       // llamamos la interrupcion del timeout cada 0.5 segundos
+    pc.attach(&read_command, Serial::RxIrq);    //llamamos la interrupcion serialmente
+    pcBT.attach(&read_commandbt, Serial::RxIrq);//llamamos la interrupcion serialmente
+    S0=1,S1=0;                                  //calibracion de lectura del sensor
+    S2=0,S3=0;
+    init_servo();
+    while(1) {
+        wait(1);
+        pc.printf("modo joystick\n");
+        meas_r = analog_value.read();            // se lee la entrada analoga del joystick
+        meas_v = meas_r * 3300;                  // se convierte la variable a 3.3v
+        pc.printf("medida = %f = %.0f mV\n", meas_r, meas_v); //muestra en el monitor lectura del joystick
+        if (meas_v < 1000) {
+            pc.printf("adelante\n");
+            adelante();
+
+        }
+        if (meas_v > 3000) {
+            pc.printf("atras\n");
+            atras();
+        }
+    }
+}
+unsigned int lectura()      // lectura del sensor
+{
+    tiempo.start();
+    while(leer) {}
+    while(!leer) {}         // inicia desde el inicio de cambio de flanco
+    //pc.printf("sale");
+    while(leer) {}
+    tiempo.reset();
+    inicio=tiempo.read_us();
+    while(!leer) {}
+    final=tiempo.read_us();
+    resultado=(final-inicio);
+    return resultado;
+}
+void init_servo()                   //inicializamos servomotores
+{
+    myServo1.period_ms(20);         // periodo de cada servo
+    myServo2.period_ms(20);
+    myServo3.period_ms(20);
+    myServo4.period_ms(20);
+    myServo5.period_ms(20);
+    myServo6.period_ms(20);
+    myServo7.period_ms(20);
+    myServo8.period_ms(20);
+
+}
+int coord2us(float coord)
+{
+    if(0 <= coord <= 180)
+        return int(750+coord*1750/180);// cambiar de numero de entrada serial a grados
+    return 750;
+}
+void read_command()
+{
+    init_c=pc.getc();
+    if( init_c==0xFF) {
+        read_cc=pc.getc();
+        nmotor=pc.getc();
+        grados=pc.getc();
+        endc=pc.getc();
+
+        if(endc==0xFD) {
+            switch (read_cc) {
+                case  0x01:
+                    moving();
+                    break;
+                case  0x02:
+                    grupomover();
+                    break;
+                default:
+                    pc.printf("error \nSe espera  0xFEF0 o 0xFFF0 \n");
+                    break ;
+            }
+        } else
+            pc.printf("Error de codigo de cierre \n");
+
+    } else
+        pc.printf("Error de codigo de inicio. \n");
+}
+void read_commandbt()
+{
+    init_c=pc.getc();
+    if( init_c==0xFF) {
+        read_cc=pc.getc();
+        nmotor=pc.getc();
+        grados=pc.getc();
+        endc=pc.getc();
+
+        if(endc==0xFD) {
+            switch (read_cc) {
+                case  0x01:
+                    moving();
+                    break;
+                case  0x02:
+                    grupomover();
+                    break;
+                default:
+                    pc.printf("error \nSe espera  0xFEF0 o 0xFFF0 \n");
+                    break ;
+            }
+        } else
+            pc.printf("Error de codigo de cierre \n");
+
+    } else
+        pc.printf("Error de codigo de inicio. \n");
+
+}
+void moving()
+{
+    pc.printf("mueve\n");
+    mover_ser(nmotor,grados);       // llama el void para mover servo con grados
+    pc.printf("fin mueve\n");
+
+}
+void mover_ser(uint8_t nmotor, uint8_t grados)  // definimos que leen los 8 servos y le asiganamos a cada variable un servo
+{
+    int pulsea = coord2us(grados);
+    switch (nmotor) {
+        case  0x01:
+            myServo1.pulsewidth_us(pulsea);
+            break;                              // caso para cada servo
+        case  0x02:
+            myServo2.pulsewidth_us(pulsea);
+            break;
+        case  0x03:
+            myServo3.pulsewidth_us(pulsea);
+            break;
+        case  0x04:
+            myServo4.pulsewidth_us(pulsea);
+            break;
+        case  0x05:
+            myServo5.pulsewidth_us(pulsea);
+            break;
+        case  0x06:
+            myServo6.pulsewidth_us(pulsea);
+            break;
+        case  0x07:
+            myServo7.pulsewidth_us(pulsea);
+            break;
+        case  0x08:
+            myServo8.pulsewidth_us(pulsea);
+            break;
+        default:
+            pc.printf("no hay mas motores error de comando\n");
+            break ;
+    }
+}
+
+void timer_interrupt()   //la respuesta de cada interrupcion externamente por timer
+{
+    led = !led;
+    S2=0,S3=0;
+    rojo=lectura();         //pc.printf("rojo --%d\n",rojo);
+    S2=1,S3=1;
+    verde=lectura();        //pc.printf("verde --%d\n",verde);
+    S2=0,S3=1;
+    azul=lectura();         //pc.printf("azul --%d\n",azul);
+   
+    if (rojo>=100 and rojo <=400) {          // lee el rojo------------
+        if (verde>=450 and verde <=700) {
+            if (azul>=300 and azul <=600) {
+
+                
+               //POSICION ORIGINAL
+               pc.printf("POSICION ORIGINAL --%d\n",rojo);
+                 myServo2.pulsewidth_us(1800);
+                 wait(0.5);
+                 myServo8.pulsewidth_us(1600);
+                 wait(0.5);
+                 myServo4.pulsewidth_us(1400);
+                 wait(0.5);
+                 myServo6.pulsewidth_us(1600);
+                 wait(0.5);
+
+               /*  myServo1.pulsewidth_us(1200);
+                 wait(0.5);
+                 myServo3.pulsewidth_us(1200);
+                 wait(0.5);
+                 myServo5.pulsewidth_us(1200);
+                 wait(0.5);
+                 myServo7.pulsewidth_us(1200);
+                 wait(1);*/
+                 
+                 pc.printf("rojo --%d\n",rojo);
+
+             }
+        }
+    }
+
+    if (rojo>=200 and rojo <=450) {             // lee el verde --------------------------------------
+        if (verde>=200 and verde <=450) {
+            if (azul>=200 and azul <=450) {
+
+                
+            // GIRO A LA DERECHA
+                pc.printf("GIRO A LA DERECHA --%d\n",verde);             
+                myServo1.pulsewidth_us(1200);
+                myServo3.pulsewidth_us(1200);
+                myServo5.pulsewidth_us(1200);
+                myServo7.pulsewidth_us(1200);
+                wait(0.5); 
+     
+                myServo1.pulsewidth_us(2000);
+                wait(0.5);
+                myServo2.pulsewidth_us(1000);
+                wait(0.5); 
+                myServo1.pulsewidth_us(1200);
+                wait(0.5); 
+    
+                myServo7.pulsewidth_us(2000);
+                wait(0.5);
+                myServo8.pulsewidth_us(1000);
+                wait(0.5); 
+                myServo7.pulsewidth_us(1200);
+                wait(0.5); 
+    
+                myServo5.pulsewidth_us(2000);
+                wait(0.5);
+                myServo6.pulsewidth_us(1000);
+                wait(0.5); 
+                myServo5.pulsewidth_us(1200);
+                wait(0.5); 
+    
+                myServo3.pulsewidth_us(2000);
+                wait(0.5);
+                myServo4.pulsewidth_us(1000);
+                wait(0.5); 
+                myServo3.pulsewidth_us(1200);
+                wait(0.5);
+    
+   
+                myServo2.pulsewidth_us(2200);
+                myServo6.pulsewidth_us(2200);
+                wait(0.5); 
+                myServo4.pulsewidth_us(2200);
+                myServo8.pulsewidth_us(2200);
+                wait(1); 
+                
+                pc.printf("verde --%d\n",verde);
+
+            }
+        }
+    }
+
+    if (rojo>=400 and rojo <=800) {                     // lee el azul------------------------------------
+        if (verde>=200 and verde <=500) {
+            if (azul>=1 and azul <=400) {
+
+                 
+               
+             // GIRO A LA IZQUIERDA
+             pc.printf("GIRO A LA IZQUIERDA --%d\n",azul);
+             
+             myServo1.pulsewidth_us(1200);
+             myServo3.pulsewidth_us(1200);
+             myServo5.pulsewidth_us(1200);
+             myServo7.pulsewidth_us(1200);
+             wait(1); 
+ 
+             myServo1.pulsewidth_us(2000);
+             wait(0.5);
+             myServo2.pulsewidth_us(2200);
+             wait(0.5); 
+             myServo1.pulsewidth_us(1200);
+             wait(0.5); 
+    
+             myServo7.pulsewidth_us(2000);
+             wait(0.5);
+             myServo8.pulsewidth_us(2200);
+             wait(0.5); 
+             myServo7.pulsewidth_us(1200);
+             wait(0.5); 
+    
+             myServo5.pulsewidth_us(2000);
+             wait(0.5);
+             myServo6.pulsewidth_us(2200);
+             wait(0.5); 
+             myServo5.pulsewidth_us(1200);
+             wait(0.5); 
+    
+             myServo3.pulsewidth_us(2000);
+             wait(0.5);
+             myServo4.pulsewidth_us(2200);
+             wait(0.5); 
+             myServo3.pulsewidth_us(1200);
+             wait(0.5);
+    
+             myServo2.pulsewidth_us(1200);
+             myServo6.pulsewidth_us(1200);
+             wait(0.5); 
+             myServo4.pulsewidth_us(1200);
+             myServo8.pulsewidth_us(1200);
+             wait(1); 
+  
+             pc.printf("azul --%d\n",azul);
+            }
+        }
+    }
+    timeout.attach(&timer_interrupt, 0.5); // reiniciamos el timeout para la siguiente lectura
+}
+void grupomover()
+{
+    pc.printf("mueve grupo \n");
+    switch (nmotor) {
+        case 0x01: {
+            myServo1.pulsewidth_us(550);
+            myServo2.pulsewidth_us(1800);
+            pc.printf("primer caso\n");
+            break;
+        }
+
+        case 0x02: {
+            myServo3.pulsewidth_us(550);
+            myServo4.pulsewidth_us(1800);
+            pc.printf("segundo caso \n");
+            break ;
+        }
+
+        case 0x03: {
+            myServo5.pulsewidth_us(550);
+            myServo6.pulsewidth_us(1800);
+            pc.printf("tercer caso \n");
+            break;
+        }
+
+        case 0x04: {
+            myServo7.pulsewidth_us(500);
+            myServo8.pulsewidth_us(1800);
+            pc.printf("cuarto caso \n");
+            break;
+            default:
+                pc.printf("solo 4 casos error en comando\n");
+                break ;
+            }
+    }
+    pc.printf("fin mueve grupo\n");
+
+}
+void adelante()
+{
+//  PASO DERECHA-DELANTERA 
+   
+    myServo8.pulsewidth_us(1000);
+    wait(0.5);
+    myServo7.pulsewidth_us(2000);
+    wait(0.5);
+    myServo8.pulsewidth_us(2000);
+    wait(0.5);
+    myServo7.pulsewidth_us(1200);
+    wait(0.5); 
+    myServo8.pulsewidth_us(1000);
+    wait(0.5);   
+
+//  PASO IZQUIERDA-TRASERA
+    
+    myServo4.pulsewidth_us(1600);
+    wait(0.5); 
+    myServo3.pulsewidth_us(2000);
+    wait(0.5);
+    myServo4.pulsewidth_us(800);
+    wait(0.5);
+    myServo3.pulsewidth_us(1200);
+    wait(0.5); 
+    myServo4.pulsewidth_us(1600);
+    wait(0.5); 
+
+//  PASO IZQUIERDA-DELANTERA  
+    
+    myServo2.pulsewidth_us(2400);
+    wait(0.5); 
+    myServo1.pulsewidth_us(2000);
+    wait(0.5);
+    myServo2.pulsewidth_us(1200);
+    wait(0.5);
+    myServo1.pulsewidth_us(1200);
+    wait(0.5); 
+    myServo2.pulsewidth_us(2400);
+    wait(0.5);   
+
+//  PASO DERECHA-TRASERA
+    
+    myServo6.pulsewidth_us(1500);
+    wait(0.5);  
+    myServo5.pulsewidth_us(2000);
+    wait(0.5);
+    myServo6.pulsewidth_us(2300);
+    wait(0.5);
+    myServo5.pulsewidth_us(1200);
+    wait(0.5); 
+    myServo6.pulsewidth_us(1500);
+    wait(0.5);    
+
+}
+
+void atras()
+{
+//  PASO DERECHA-DELANTERA 
+   
+    myServo8.pulsewidth_us(1000);
+    wait(0.5);
+    myServo7.pulsewidth_us(1200);
+    wait(0.5);
+    myServo8.pulsewidth_us(2000);
+    wait(0.5);
+    myServo7.pulsewidth_us(2000);
+    wait(0.5); 
+    myServo8.pulsewidth_us(1000);
+    wait(0.5);  
+    myServo7.pulsewidth_us(1200);
+    wait(0.5);
+
+//  PASO IZQUIERDA-TRASERA
+    
+    myServo4.pulsewidth_us(1600);
+    wait(0.5); 
+    myServo3.pulsewidth_us(1200);
+    wait(0.5);
+    myServo4.pulsewidth_us(800);
+    wait(0.5);
+    myServo3.pulsewidth_us(2000);
+    wait(0.5); 
+    myServo4.pulsewidth_us(1600);
+    wait(0.5); 
+    myServo3.pulsewidth_us(1200);
+    wait(0.5);
+
+//  PASO IZQUIERDA-DELANTERA  
+    
+    myServo2.pulsewidth_us(2400);
+    wait(0.5); 
+    myServo1.pulsewidth_us(1200);
+    wait(0.5);
+    myServo2.pulsewidth_us(1200);
+    wait(0.5);
+    myServo1.pulsewidth_us(2000);
+    wait(0.5); 
+    myServo2.pulsewidth_us(2400);
+    wait(0.5);   
+    myServo1.pulsewidth_us(1200);
+    wait(0.5); 
+
+//  PASO DERECHA-TRASERA
+    
+    myServo6.pulsewidth_us(1500);
+    wait(0.5);  
+    myServo5.pulsewidth_us(1200);
+    wait(0.5);
+    myServo6.pulsewidth_us(2300);
+    wait(0.5);
+    myServo5.pulsewidth_us(2000);
+    wait(0.5); 
+    myServo6.pulsewidth_us(1500);
+    wait(0.5);   
+    myServo5.pulsewidth_us(1200);
+    wait(0.5);
+
+   
+}