Yulied Ahumada
/
ProyectoEmbebidos
Programa araña
Revision 0:bf91ab2afeaf, committed 2018-11-20
- Comitter:
- Yulied97
- Date:
- Tue Nov 20 01:53:38 2018 +0000
- Commit message:
- Programa ara?a;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Color.cpp Tue Nov 20 01:53:38 2018 +0000 @@ -0,0 +1,73 @@ +#include "mbed.h" + +//*****************************************************************************// +// Distribucion pines sensor de color + + //DigitalOut S0(PA_12); // D2 | 0 0 POWER DOWN | 0 1 2% | 1 0 20% | 1 1 100% + //DigitalOut S1(PB_3); // D3 | 0 1 20% + DigitalIn Out(PA_7); // D11 | SALIDA SENSOR + DigitalOut S2(PA_6); // D12 | 0 0 RED | 0 1 BLUE | 1 0 ALL | 1 1 GREEN + DigitalOut S3(PA_5); // D13 | 0 0 RED | 0 1 BLUE | 1 0 ALL | 1 1 GREEN + +//*****************************************************************************// +// FUNCION AUXILIAR PARA LEER EL COLOR + +int readAux() +{ + Timer Tiempo; + int inicial = 0, final = 0, resultado = 0; + + Tiempo.start(); + + while(Out == 1); + while(Out == 0); + while(Out == 1); + + inicial = Tiempo.read_us(); + + while (Out == 0); + + final = Tiempo.read_us(); + + resultado = (final - inicial); + return (resultado); +} + +//*****************************************************************************// +// FUNCION PARA LEER EL COLOR + +int readColor() +{ + int rojo = 0, azul = 0, verde = 0; + int color; + + //S0.write(1); + //S1.write(1); + + S2.write(0); + S3.write(0); + rojo = readAux(); + + S2.write(0); + S3.write(1); + azul = readAux(); + + S2.write(1); + S3.write(1); + verde = readAux(); + + if (rojo < azul && verde > azul && rojo < 35) + { color = 1; } + + else if (azul < rojo && azul < verde && verde < rojo) + { color = 2; } + + else if (rojo < azul && azul > verde ) + { color = 3; } + + else + { color = 4; } + +return (color); + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Color.h Tue Nov 20 01:53:38 2018 +0000 @@ -0,0 +1,3 @@ +#include "mbed.h" + +int readColor(); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Nov 20 01:53:38 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Nov 20 01:53:38 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file