File content as of revision 0:bf91ab2afeaf:

#include "mbed.h"
#include <Serial.h>
#include "Color.h"

//*****************************************************************************//
// Universidad ECCI - Ingenieria mecatronica
// Sistemas embebidos - Control por telecomandos
// Mario Esteban Galeano Rodriguez
//*****************************************************************************//

//*****************************************************************************//
//Declaración de variables
    
    uint8_t data [3];
    int color, ciclos;
    char status = 0;
    Serial pc(SERIAL_TX, SERIAL_RX, 9600);

//*****************************************************************************//
// Timer para lectura del sensor

    Timer timeRead;
    Timer timeSerial;
    AnalogIn value_y (A0);
    AnalogIn value_x(A1);
    
    float valor_y;
    float valor_x;
    
//*****************************************************************************//
// Posiciones movimientos
    
    int arriba = 5;
    int abajo = 25;

//*****************************************************************************//
// Otras variables

    int tiempo = 200;
    
//*****************************************************************************//
// Distribución pines servomotores

// Brazo derecho delante 
    PwmOut servo_1 (PA_10);     //D0
    PwmOut servo_2 (PB_3);      //D1

// Brazo derecho atras
    PwmOut servo_3 (PB_5);      //D8
    PwmOut servo_4 (PB_4);      //D9

// Brazo izquierdo adelante
    PwmOut servo_5 (PB_10);     //D10
    PwmOut servo_6 (PA_8);      //D11

// Brazo izquierdo atras
    PwmOut servo_7 (PA_9);      //D12
    PwmOut servo_8 (PB_6);      //D13

//*****************************************************************************//
// Función para el control de servomotores

void controlServo(int n_servo, int degrees)
{        
    int p_width = ((degrees*2000)/180)+500;
     
    switch (n_servo)
    {
        case 1:
            servo_1.pulsewidth_us(p_width);
            break;
            
        case 2:
            servo_2.pulsewidth_us(p_width);
            break;
        
        case 3:
            servo_3.pulsewidth_us(p_width);
            break;
            
        case 4:
            servo_4.pulsewidth_us(p_width);
            break;
        
        case 5:
            servo_5.pulsewidth_us(p_width);
            break;
            
        case 6:
            servo_6.pulsewidth_us(p_width);
            break;
        
        case 7:
            servo_7.pulsewidth_us(p_width);
            break;
            
        case 8:
            servo_8.pulsewidth_us(p_width);
            break;
        
        default:
            break;
    }
}

//*****************************************************************************//
// Funcion para movimiento de los brazos

void controlBrazo (int Brazo, int movimiento)
{
    if (Brazo == 0x01)
    {
        switch (movimiento)
        {
            case 1:// handRightUp
                controlServo(2, arriba-5);
                break;
            
            case 2:// handRightDown
                controlServo(2, abajo-5);
                break;
            
            case 3:// handRightForward
                controlServo(1, 70);
                break;
                
            case 4:// handRightReverse
                controlServo(1, 15);
                break;
            
            default:
                break;
        }
    }
    
    else if (Brazo == 0x02)
    {
        switch (movimiento)
        {
            case 1:// legRightUp
                controlServo(4, arriba-5);
                break;
            
            case 2:// legRightDown
                controlServo(4, abajo-10);
                break;
            
            case 3:// legRightForward
                controlServo(3, 150);
                break;
                
            case 4:// legRightReverse
                controlServo(3, 95);
                break;
            
            default:
                break;
        }
    }
    
    else if (Brazo == 0x03)
    {
        switch (movimiento)
        {
            case 1:// handLeftUp
                controlServo(6, arriba);
                break;
            
            case 2:// handLeftDown
                controlServo(6, abajo);
                break;
            
            case 3://handLeftForward
                controlServo(5, 60);
                break;
                
            case 4:// handLeftReverse
                controlServo(5, 118);
                break;
            
            default:
                break;
        }
    }
    
    else if (Brazo == 0x04)
    {
        switch (movimiento) 
        {
            case 1:// legLeftUp
                controlServo(8, arriba);
                break;
            
            case 2:// legLeftDown
                controlServo(8, abajo);
                break;
            
            case 3:// legLeftForward
                controlServo(7, 10);
                break;
                
            case 4:// legLeftReverse
                controlServo(7, 65);
                break;
            
            default:
                break;
        }
    }    
}

//*****************************************************************************//
//Funciones movimientos

void inicialPosition ()
{   
    controlServo(2,arriba);
    controlServo(1,50);
    wait_ms(80);
    controlServo(2,abajo-12);
    
    controlServo(4,arriba);
    controlServo(3,105);
    wait_ms(80);
    controlServo(4,abajo-12);
    
    controlServo(6,arriba);
    controlServo(5,80);
    wait_ms(80);
    controlServo(6,abajo);
    
    controlServo(8,arriba);
    controlServo(7,45);
    wait_ms(80);
    controlServo(8,abajo);    
}

void forward()
{       
    //*************************************************************//
    // POSICION INICIAL
                
    controlServo(5,118);    // Brazo 3 atras
    controlServo(7,5);      // Brazo 4 adelante
    wait_ms(tiempo);        // Tiempo en posicion
                
    //*************************************************************//
    // PRIMER PASO
                
    controlBrazo(3,1);      // brazo 3 arriba
    controlServo(7,50);     // Brazo 4 atras
    controlServo(5,50);     // Brazo 3 avanza
    controlServo(3,90);     // Brazo 2 atras 
    wait_ms(tiempo);        // tiempo en posicion arriba
                
    controlBrazo(3,2);      // Brazo 3 baja
    controlServo(1,25);     // Brazo 1 atras
    wait_ms(tiempo);
                
    //*************************************************************//
    // SEGUNDO PASO
                
    controlBrazo(2,1);      // Brazo 2 arriba
    controlServo(1,8);      // Brazo 1 atras
    controlServo(3,150);    // Brazo 2 adelante
    controlServo(7,50);     // Brazo 4 atras 
    wait_ms(tiempo);        // Tiempo en posicion arriba
                
    controlBrazo(2,2);      // brazo 2 abajo
    controlServo(5,100);    // Brazo 3 atras
                
    //*************************************************************//
    // TERCER PASO
                
    controlBrazo(1,1);      // Brazo 1 arriba
    controlServo(3,90);     // Brazo 2 atras
    controlServo(1,60);     // Brazo 1 adelante
    controlServo(7,65);     // Brazo 4 atras
    wait_ms(tiempo);
    controlBrazo(1,2);      // Brazo 1 abajo
    controlServo(5,118);
    
    //*************************************************************//
    // TERCER PASO            
    controlBrazo(4,1);
    controlServo(7,5);      // Brazo 4 adelante
    wait_ms(tiempo);
    controlBrazo(4,2);
}

void reverse()
{       
    //*************************************************************//
    // POSICION INICIAL
                
    controlServo(5,118);    // Brazo 3 atras
    controlServo(7,5);      // Brazo 4 adelante
    wait_ms(tiempo);        // Tiempo en posicion
                
    //*************************************************************//
    // PRIMER PASO
    
    controlBrazo(4,1);      // brazo 4 arriba
    controlServo(5,60);     // Brazo 3 atras
    controlServo(7,65);     // Brazo 4 avanza
    controlServo(1,70);     // Brazo 1 atras 
    wait_ms(tiempo);        // tiempo en posicion arriba
                
    controlBrazo(4,2);      // Brazo 4 abajo
    controlServo(3,120);    // Brazo 1 atras
    wait_ms(tiempo);
    
    //**************************************************************//
    // SEGUNDO PASO
    
    controlBrazo(1,1);      // Brazo 1 arriba
    controlServo(5,70);     // Brazo 3 atras
    controlServo(1,8);      // Brazo 1 adelante
    controlServo(3,150);    // Brazo 2 atras 
    wait_ms(tiempo);        // Tiempo en posicion arriba
                
    controlBrazo(1,2);      // brazo 1 abajo
    controlServo(7,35);     // Brazo 4 atras
                
    //*************************************************************//
    // TERCER PASO
    
    controlBrazo(2,1);      // Brazo 2 arriba
    controlServo(1,70);     //Brazo 1 adelante
    controlServo(3,95);     //Brazo 2 atras
    controlServo(5,60);     // Brazo 4 adelante
    wait_ms(tiempo);
    
    controlBrazo(2,2);      // Brazo 2 abajo
    controlServo(7,10);     // Brazo 4 adelante
    
    //*************************************************************//
    // TERCER PASO      
          
    controlBrazo(3,1);      //Brazo 3 arriba
    controlServo(5,118);    // Brazo 4 adelante
    wait_ms(tiempo);
    controlBrazo(3,2);
}

void right()
{
    //*************************************************************//
    // POSICION INICIAL
                
    controlServo(5,118);    // Brazo 3 atras
    controlServo(7,5);      // Brazo 4 adelante
    wait_ms(tiempo);        // Tiempo en posicion
    
    //*************************************************************//
    // PASO 1
    
    controlBrazo(3,1);
    controlServo(5,60);
    controlServo(1,70);
    controlServo(7,40);
    controlServo(3,130);
    wait_ms(tiempo);
    controlBrazo(3,2);
    
    // PASO 2
    controlBrazo(1,1);
    controlServo(1,8);
    controlServo(3,150);
    controlServo(5,80);
    controlServo(7,55);
    wait_ms(tiempo);
    controlBrazo(1,2);
    
    // PASO 3
    controlBrazo(2,1);
    controlServo(3,95);
    controlServo(1,30);
    controlServo(5,100);
    controlServo(7,65);
    wait_ms(tiempo);
    controlBrazo(2,2);
    
    //PASO 4
    controlBrazo(4,1);
    controlServo(7,10);
    controlServo(5,118);
    controlServo(1,45);
    controlServo(3,105);
    wait_ms(tiempo);
    controlBrazo(4,2);
}

void left()
{
    //*************************************************************//
    // POSICION INICIAL
                
    controlServo(1,8);      // Brazo 3 atras
    controlServo(3,150);    // Brazo 4 adelante
    wait_ms(tiempo);        // Tiempo en posicion
    
    //*************************************************************//
    // PASO 1
    
    controlBrazo(1,1);
    controlServo(3,105);
    controlServo(1,70);
    controlServo(5,60);
    controlServo(7,55);
    wait_ms(tiempo);
    controlBrazo(1,2);
    
    // PASO 2
    controlBrazo(3,1);
    controlServo(1,50);
    controlServo(5,118);
    controlServo(3,80);
    controlServo(7,15);
    wait_ms(tiempo);
    controlBrazo(3,2);
    
    // PASO 3
    controlBrazo(4,1);
    controlServo(5,100);
    controlServo(7,65);
    controlServo(3,95);
    controlServo(1,30);
    wait_ms(tiempo);
    controlBrazo(4,2);
    
    //PASO 4
    controlBrazo(2,1);
    controlServo(1,8);
    controlServo(3,150);
    controlServo(5,80);
    controlServo(7,45);
    wait_ms(tiempo);
    controlBrazo(2,2);
    controlBrazo(1,2);
    controlBrazo(3,2);
}

//*****************************************************************************//
// Funcion para la lectura de datos

uint8_t readComand()
{
    timeSerial.reset();
    timeSerial.start();
    
    while (timeSerial.read_ms() <= 120)
    {
        if(pc.readable())
        {
            char charRead = pc.getc();
            
            if (charRead == 0xFF)
            {            
                for (int i=0; i<3; i++)
                {
                    data[i] = pc.getc();
                }     
            }    
            
            charRead = pc.getc();
                
            if (charRead != 0xFD)
            {
                pc.printf("\nError al recibir comando, intente nuevamente\n");
                return 0;    
            }
                
            return 1;
        }
    }
    return 0;
}

void readSensor()
{
    valor_y = value_y.read();
    valor_y = valor_y*3300;
    //pc.printf("\nValor Y: %f\n",valor_y);
    
    valor_x = value_x.read();
    valor_x = valor_x*3300;
    //pc.printf("\nValor X: %f\n",valor_x);
}

void up (void)
{
    controlBrazo(1,2);
    controlBrazo(2,1);
    controlBrazo(3,2);
    controlBrazo(4,1);
}

// *********************************************************************************************************************************//
// Programa principal


int main ()
{
    inicialPosition ();
    wait(1);
        
    while (1)
    {
       // Lectura constante del color  
       color = readColor();
       
       // lectura del joystick cada 100 ms
       timeRead.start();
       
       if (timeRead.read_ms() >= 100)
       {
           readSensor();
           timeRead.reset();
       }
       
       while (readComand())
       {    
            // Codigo para comandos desde CoolTerm
       
            if (data[0] == 0x01)
            {
                // Codigo para movimiento independiente de los servomotores desde CoolTerm
                controlServo(data[1],data[2]);       
                pc.printf("\nNumero motor: %d", data[1]);
                pc.printf("\nGrados: %d\n", data[2]);
            }   
                   
            else if(data[0]==0x02)
            {
                // Codigo para movimiento de las extremidades desde CoolTerm
                pc.printf("\nNumero brazo: %d", data[1]);
                pc.printf("\nMovimiento: %d\n", data[2]);
                controlBrazo(data[1],data[2]);
            }
            
            else if (data[0] == 3)
            {
                // Codigo para llevar a posicion inicial desde CoolTerm
                inicialPosition ();
            }
            
            else if (data[0] == 4)
            {
                // Codigo para lectura de color desde CoolTerm
                color = readColor();
                pc.printf("\nColor: %d\n", color);
            } 
        }
               
        if (valor_y <= 1000)
        {
            // Codigo para movimiento adelante segun joystick
            
            if (readColor() == 1)
            {
                for (int i=0; i<2; i++)
                {
                    // Si el color es rojo gira a la derecha
                    right();
                }
            }
            
            else if(readColor() == 2)
            {
                for (int i=0; i<2; i++)
                {
                    // Si el color es azul gira a la izquierda
                    left();
                }
            }
            
            else
            {
                forward();
                status=1;
            }
        } 
                
        else if (valor_y >= 2000)
        {
            // Codigo para movimiento atras segun joystick
            reverse();
            status=1;
        }
                
        if (valor_x <= 1000)
        {
            // Codigo para giro derecho segun joystick
            
            if (readColor() == 3)
            {
                // Si el color es verde se "sienta"
                up();
            }
            
            else 
            {
                // Si el color es diferente de verde gira a la izquierda
                right();
                status=1;
            }
        } 
                
        else if (valor_x >= 2000)
        {
            // Codigo para giro izquierdo segun joystick
            
            if (readColor() == 3)
            {
                // Si el color es verde se "sienta"
                up();
            }
            
            else 
            {
                // Si el color es diferente de verde gira a la izquierda
                left();
                status=1;
            }
        }
                
        else if(status == 1)
        {
            inicialPosition ();
            status=0;
        }

    }
}