File content as of revision 0:af114e34a5ae:

// 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");
        }      
    
}