Diff: main.cpp

Revision:

0:af114e34a5ae

Child:

1:5650b51c59f7

diff -r 000000000000 -r af114e34a5ae main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 14 21:49:17 2018 +0000
@@ -0,0 +1,419 @@
+// Universidad ECCI
+// Sistemas embebidos
+// Entregable programacion Comando 2 y sensor de color
+
+#include "mbed.h"
+#include "Timer.h"
+#define MAX 20
+
+// Funciones
+void init_servo();                          // Se crea la funcion iniciarlizar servo, esta funcion tiene como objetivo definirle el periodo al pwm
+void mov_servo(int ser);
+void imp_val();
+void recibir_dat();
+void comando_1(float gra);
+void comando_2();
+void paso();
+int Medicion_tiempo();
+void Selector_color();
+void config_escala();
+void mediana();
+void sensor_color();
+void joystick();
+
+// Vectores
+int32_t vbuff[MAX];                         // creando vector que recibe la informacion
+int mbuff[MAX][MAX];                        // creando matriz para guardar la informacion validada
+float vgrados[8]={0,0,0,0,0,0,0,0};         // vector donde se guardan los grados en los cuales se encuetra cada servomotor
+float vgrados_ant[8]={0,0,0,0,0,0,0,0};
+int mpasos[2];
+int mcolor[3][2]={{0,0},{0,1},{1,1}};
+int mesc_freq[3][2]={{0,1},{1,0},{1,1}};
+int mediciones[3][9];
+
+
+// Variables
+int filas=1,colum=5;                        // numero de filas y columnas de las matrices
+int ms = 20;
+Timer tiempo;
+int tim=0;
+int escala;
+
+// Comunicacion serial
+Serial emisor(USBTX, USBRX);                // se crea la clase serial poder intercambiar informacion entre la tarjeta y el pc
+
+// Mototores y gpio 
+PwmOut servo1(PA_10);//d2
+PwmOut servo2(PB_3);//d3                        // se define la salida de proposito general a cada servomotr
+PwmOut servo3(PB_4);//d5
+PwmOut servo4(PA_9);//d8
+PwmOut servo5(PC_7);//d9
+PwmOut servo6(PB_6);//d10
+PwmOut servo7(PA_7);//d11
+PwmOut servo8(PA_8);//d12
+
+
+DigitalOut s0(PB_8);//d14
+DigitalOut s1(PB_9);//d15
+DigitalOut s2(PB_10);//d6
+DigitalOut s3(PA_5);//d13
+DigitalIn p_out(PB_5);//d4
+
+AnalogIn JY(A0);
+AnalogIn JX(A1);
+
+int main() {
+
+    init_servo();    
+    emisor.baud(9600);                      // sincronicroniza la velocidad de intercambio de datos
+    emisor.printf("start \n");              // imprime en coolterm la señal de inicio de comunicacion
+
+    while(1) 
+    {   
+        recibir_dat();
+        switch(mbuff[0][1]) 
+        {
+            case 1: 
+                comando_1(mbuff[0][3]);
+            break;
+            case 2:
+                comando_2();                
+            break; 
+            case 3:
+                paso();                
+            break; 
+            case 4:
+                sensor_color();               
+            break; 
+            case 5:
+                joystick();               
+            break; 
+            default: emisor.printf("tipo de comando erroneo");
+        }
+    }
+}
+
+void init_servo()                               // se define la duracion del periodo del pulso a cada servomotor
+{
+    servo1.period_ms(ms);
+    servo2.period_ms(ms);
+    servo3.period_ms(ms);
+    servo4.period_ms(ms);
+    servo5.period_ms(ms);
+    servo6.period_ms(ms);
+    servo7.period_ms(ms);
+    servo8.period_ms(ms);
+}
+
+uint8_t  ss_time=50;                            // tiempo  de espera para moverse 1 mm en microsegundos
+
+void put_sstime(uint8_t vtime)
+{
+    ss_time=vtime;}
+
+void mov_servo(int ser)
+{       
+            switch(ser)                                               // con esta funcion se le asgina a cada servomotor el valor
+        {   
+                                                                // de pulsos para que se mueva al angulo correspondiente
+            case 1:
+            servo1.pulsewidth_us(vgrados[0]);
+            break;
+            case 2:
+            servo2.pulsewidth_us(vgrados[1]);
+            break;
+            case 3:
+            servo3.pulsewidth_us(vgrados[2]);
+            break;
+            case 4:
+            servo4.pulsewidth_us(487+vgrados[3]);
+            break;
+            case 5:
+            servo5.pulsewidth_us(vgrados[4]);
+            break;        
+            case 6:
+            servo6.pulsewidth_us(3250 - (487+vgrados[5]));
+            break;        
+            case 7:
+            servo7.pulsewidth_us(vgrados[6]);
+            break;
+            case 8:
+            servo8.pulsewidth_us(1625+(1625-vgrados[7]));
+            break;
+        }
+}
+
+void imp_val()
+{
+            // imprimir informacion
+        for(int x = 0; x < filas; x++)                          // En esta parte del codigo se imprime la informacion que
+        {for(int y = 0; y < colum; y++)                         // recibio el programa, con esto el usuario puede confirmar 
+        {                                                       // que el programa esta trabajando con los datos correctos
+        emisor.printf("%x",mbuff[x][y]);                        // se imprimen en formato Hexadecimal
+        }
+        emisor.printf("\n");
+        }
+        
+        for(int w = 0; w < 8; w++)                              // Se imprime el vector de pulsos asignado para cada servo
+        {emisor.printf(" %.1f",vgrados[w]);}                    // este datos se entrega en numeros decimales
+        emisor.printf("\n");
+        emisor.printf("%d \n",mbuff[0][3]);                     // se imprime en decimal el valor de los grados ingresados
+
+}
+
+void recibir_dat()
+{
+        //diligenciar vector
+        for(int a = 0;a < colum;a++)        // Este vector recibe y valida los datos recibidos
+        {                                   // las validaciones que hace son si el telecomando inicia con el comando
+            vbuff[a]=emisor.getc();         // correcto(ff), el numero de servomotor se encuentra entre 1 a  
+            if(vbuff[0]!=0xff)              // 8 y si los grados se encuebtran entre 0 y 180, si no cumple estos parametros
+            {a--;}
+                                      // descarta la informacion
+            if(vbuff[4]!=0xfd)
+            {if(vbuff[4]!=0x00)
+                {emisor.printf("Corregir comando final servo %x\n",vbuff[2]);
+                    vbuff[4]=0;
+                    a = a - 5;}}
+                                            
+            if(vbuff[2]>0x08)
+            {   emisor.printf("Corregir el numero del servomotor %x\n",vbuff[2]);
+                vbuff[2]=0;
+                a = a - 3;}
+            
+            if(vbuff[2]<0x00)
+            {   emisor.printf("Corregir el numero del servomotor %x\n",vbuff[2]);
+                vbuff[2]=0;
+                a = a - 3;}
+            
+            if(vbuff[3]>0xb4)
+            {   emisor.printf("Corregir los grados asignados al servomotor %x\n",vbuff[2]);
+                vbuff[3]=0;
+                a = a - 4;}
+            
+            if(vbuff[3]<0x00)
+            {   emisor.printf("Corregir los grados asignados al servomotor %x\n",vbuff[2]);
+                vbuff[3]=0;
+                a = a - 4;}
+        }
+        
+        //pasar vector a matriz
+        for(int i = 0;i<filas;i++)          // Esta matriz se encarga de almacenar los datos despues de que se realizaron
+        {for(int j =0; j<colum;j++)         // las validaciones, con esto aseguramos que le entregamos datos correctos para 
+        {                                   // el que el programa pueda trabajar sin inconvenientes
+            mbuff[i][j]=vbuff[j];
+        }
+        }
+    
+}
+
+void comando_1(float gra)
+{
+        vgrados[(mbuff[0][2])- 1]= (((2600-650)*gra)/180)+650;    // el vector convierte los grados (hexadecimales) a pulsos
+        imp_val();
+        mov_servo(mbuff[0][2]);
+
+}                                                                // y se le asigna a la casilla del servomotor correspondiente
+                                                                        // en hexadecimal
+void comando_2()
+{
+    int mbrazos[4][2]={{0,1},{2,3},{4,5},{6,7}};
+    float mmov[4][2]={{0x1e,0x5a},{0x46,0x5a},{0x1e,0x5a},{0x46,0x2d}};
+    int vnew[2];
+    int vnew2[2];
+    int q=mbuff[0][3];
+    if (q>0x02)
+    {
+    vnew[0]=mbrazos[mbuff[0][2] - 1][1];
+    vnew[1]=mmov[mbuff[0][3] - 1][1];
+    }                
+    vnew2[0]=mbrazos[mbuff[0][2] - 1][0];
+    vnew2[1]=mmov[mbuff[0][3] - 1][0];
+    
+    if (q>0x02)
+    {
+    mbuff[0][1]=0x01;
+    mbuff[0][2]=vnew[0]+1;
+    mbuff[0][3]=vnew[1];
+    comando_1(mbuff[0][3]);
+    }                
+    
+    mbuff[0][2]=vnew2[0]+1;
+    mbuff[0][3]=vnew2[1];
+    comando_1(mbuff[0][3]);
+}
+
+void paso()
+{
+
+    mpasos[0]= mbuff[0][2];
+    mpasos[1]= mbuff[0][3];
+    int cant=0;
+    emisor.printf("pasos %d ",mpasos[1]);
+while(cant<mpasos[1])
+{
+    for (int aa=0;aa<4;aa++)
+    {
+        mbuff[0][1]=2;
+        mbuff[0][2]=aa+1;
+        mbuff[0][3]=3;
+        comando_2();
+        //wait_ms(200);
+        mbuff[0][1]=2;
+        mbuff[0][2]=aa+1;
+        mbuff[0][3]=2;        
+        comando_2();
+        //wait_ms(200);        
+    }
+    //wait_ms(100);
+    for (int bb=0;bb<4;bb++)
+    {
+        mbuff[0][1]=2;
+        mbuff[0][2]=bb+1;
+        mbuff[0][3]=4;
+        comando_2();
+    }
+        //wait_ms(200);
+        cant++;
+}    
+    
+}
+
+void sensor_color()
+{
+        emisor.printf("ingrese el tipo de escala de frecuencia \n");
+        escala=emisor.getc();
+        config_escala();
+        Selector_color();
+        
+        for(int e=0;e<3;e++)
+        {
+        float a = mediciones[e][5];
+        emisor.printf(" %.1f",a);
+        }
+        emisor.printf("\n");
+        if(mediciones[0][5]>mediciones[1][5])
+        {
+            if (mediciones[0][5]>mediciones[2][5])
+            {
+                emisor.printf("azul TRES PASOS \n");
+                wait(1);
+                
+                mbuff[0][1]=3;
+                mbuff[0][2]=1;
+                mbuff[0][3]=3;                
+                paso();                
+            }
+        }
+        if  (mediciones[2][5]>mediciones[0][5])
+        {
+            if (mediciones[2][5]>mediciones[1][5])
+            {
+                emisor.printf("rojo DOS PASOS \n");
+                wait(1);
+                
+                mbuff[0][1]=3;
+                mbuff[0][2]=1;
+                mbuff[0][3]=2;                
+                paso();               }
+        }
+        if  (mediciones[1][5]>mediciones[0][5])
+        {
+            if (mediciones[1][5]>mediciones[2][5])
+            {
+                emisor.printf("verde UN PASO \n");        
+                wait(1);
+                
+                mbuff[0][1]=3;
+                mbuff[0][2]=1;
+                mbuff[0][3]=1;                
+                paso();               }
+        }
+    
+}
+int Medicion_tiempo()
+{
+    while(p_out){}
+    tiempo.start();
+    while(!p_out){}
+    tiempo.stop();
+    tim=tiempo.read_us();
+    tiempo.reset();    
+
+    return tim;
+}
+
+void Selector_color()
+{
+    for(int j =0;j<9;j++)
+    {
+        for(int i =0;i<3;i++)
+        {
+            s2.write(mcolor[i][0]);
+            s3.write(mcolor[i][1]);
+        
+            switch(i)
+            {
+                case 0:
+                    mediciones[i][j]= Medicion_tiempo();
+                    break;
+                case 1:
+                    mediciones[i][j]= Medicion_tiempo();
+                    break;
+                case 2:
+                    mediciones[i][j]= Medicion_tiempo();
+                    break;
+            }
+        }
+    }
+    
+    mediana();
+}
+
+void config_escala()
+{
+    s0.write(mesc_freq[escala - 1][0]);
+    s1.write(mesc_freq[escala - 1][1]);
+}
+
+void mediana()
+{   
+    int aux;
+    
+    for(int w=0;w<3;w++)
+    {
+    for(int u=0;u<9;u++)
+    {
+    for(int o=0;o<9;o++)
+    {
+        if (mediciones[w][o]>mediciones[w][o + 1])
+        {
+            aux=mediciones[w][o];
+            mediciones[w][o]=mediciones[w][o + 1];
+            mediciones[w][o + 1]=aux;
+        }
+    }
+    }
+    }
+}
+void joystick()
+{
+    while(JY.read()*1000<600)
+        {while(JY.read()*1000>300)
+            {    if(JX.read()*1000>600)
+                {
+                    mbuff[0][1]=3;
+                    mbuff[0][2]=1;
+                    mbuff[0][3]=1;                
+                    paso();  
+                }
+                else
+                {emisor.printf("Quieta\n");}        
+                wait(0.5);
+                }
+                break;
+            }
+            {emisor.printf("Saliendo de mando con Joystick\n");
+        }      
+    
+}