Diff: main.cpp

Revision:

0:17f865015626

diff -r 000000000000 -r 17f865015626 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 20 01:45:51 2018 +0000
@@ -0,0 +1,635 @@
+#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;
+        }
+
+    }
+}
\ No newline at end of file