Entrega 2 Proyecto Sistemas Embebidos
Diff: main.cpp
- Revision:
- 0:4b10df072322
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 26 16:36:40 2019 +0000
@@ -0,0 +1,503 @@
+#include "mbed.h"
+#include "scolor_TCS3200.h"
+
+/*****************************************************************************
+generar un programa que controle por el puerto serial el grado de 4 servo motores.
+por medio de la comunicacion serial el comando es
+
+| | | |
+| INITCMD | N_motor | N_grados |
+| 0xff | 0x01- 0x04 | 0x00 - 0xb4 |
+
+
+*****************************************************************************/
+/*CONSTANTES*/
+#define INITCMD 0xFF
+#define DEGREES_MAX 180
+
+/*TELECOMANDO*/
+#define AUTOMATIC 0x01
+#define PINZA 0x02
+#define M1_MANUAL 0x03
+#define M2_MANUAL 0x04
+#define M3_MANUAL 0x05
+#define M4_MANUAL 0x06
+#define VELOCIDAD 0x07
+#define JOYSTICK 0x08
+#define COLOR 0x09
+
+/*PARAMETRO*/
+#define HOME 0x01
+#define MATERIAL 0x02
+#define CELDA1 0x03
+#define CELDA2 0x04
+#define CELDA3 0x05
+#define CELDA4 0x06
+#define ABRIR_PINZA 0x01
+#define CERRAR_PINZA 0x02
+#define VELOCIDAD_LENTA 0x01
+#define VELOCIDAD_MEDIA 0x02
+#define VELOCIDAD_ALTA 0x03
+#define INICIA_JOYSTICK 0x01
+#define PARA_JOYSTICK 0x02
+#define LEE_COLOR 0x00
+
+
+/*VARIABLES GLOBALES*/
+uint8_t N_motor; // variable almacena el número de motor en leer_datos()
+uint8_t N_grados; // varable almacena los grados que se mueve el motor en leer_datos()
+
+/*VARIABLE RECIBE COMANDOS*/
+/*Mueve Base y Eslabon Izquierdo*/
+Serial pc(USBTX, USBRX);
+unsigned char telecomando;
+unsigned char parametro;
+unsigned char teleJoys;
+unsigned char paramJoys;
+
+/*CONFIGURACIÓN SERVOS*/
+PwmOut myservo1(PB_4);//d5 -- BASE
+PwmOut myservo2(PB_10);//d6 -- IZQUIERDO
+PwmOut myservo3(PA_8);//d7 -- DERECHO
+PwmOut myservo4(PA_9);//d8 -- PINZA
+
+/*CONFIGURACIÓN JOYSTICK 1*/
+/*Mueve Base y Eslabon Izquierdo*/
+AnalogIn analog1_value_x(A0);//A0 -- POSICION X -- BASE
+AnalogIn analog1_value_y(A1);//A1 -- POSICION Y -- ESLABON INZQUIERDO
+/*Mueve Eslabon Derecho y Pinza*/
+AnalogIn analog2_value_x(A2);//A2 -- POSICION X -- ESLABON DERECHO
+AnalogIn analog2_value_y(A3);
+DigitalIn sw2(PB_6);//D10 -- BOTON JOYSTICK -- 0-ABRE PINZA , 1-CIERRA PINZA
+
+/*CONFIGURACION SENSOR DE COLOR*/
+// S0, S1, S2, S3, OUT
+//scolor_TCS3200 scolor(PA_8, PB_10, PB_9, PB_5, PB_8);
+// D2 D3 D14 D4 D15
+scolor_TCS3200 scolor(PA_10, PB_3, PB_9, PB_5, PB_8);
+
+/*DEFINICIÓN FUNCIONES*/
+void setup_uart();
+void setup_servo();
+void mover_servo(uint8_t motor, uint8_t gradss);
+uint32_t degrees2usec(uint8_t grados);
+void motor_velocidad(uint8_t motor, float valor);
+
+//MOVIMIENTO ESLABONES POR POSICION
+void mover_base(int posicion);
+void mover_izquierdo(int posicion);
+void mover_derecho(int posicion);
+void mover_pinza(int posicion);
+
+void mover_home();
+void mover_material();
+void mover_celda1();
+void mover_celda2();
+void mover_celda3();
+void mover_celda4();
+void abrir_pinza();
+void cerrar_pinza();
+
+void mover_servo(uint8_t motor, uint8_t pos);
+void mover_con_joystick();
+void leer_color();
+
+/*INICIO DE PROGRAMA*/
+int main()
+{
+ pc.printf("INICIO PROGRAMA...\n\r");
+ setup_uart();//Configuración Velocidad de Transmisión
+ setup_servo();//Configuración de Motores -> periodo = 20 ms
+
+ while(1){
+
+ pc.printf("\nEsperando Telecomando...\n");
+ while(pc.getc() != INITCMD);//0xFF
+ telecomando = pc.getc();
+ parametro= pc.getc();
+
+ pc.printf("Validando Telecomando: ");
+ pc.printf("%02x",telecomando);pc.printf("\n");
+ if(telecomando == AUTOMATIC) {
+ pc.printf("MOVIMIENTO AUTOMATICO\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+ switch (parametro) {
+ case HOME:
+ pc.printf("\nHOME..\n");
+ mover_home();
+ NVIC_SystemReset();
+ break;
+ case MATERIAL:
+ pc.printf("\nMATERIAL.\n");
+ mover_material();
+ NVIC_SystemReset();
+ break;
+ case CELDA1:
+ pc.printf("\nCELDA1.\n");
+ mover_celda1();
+ NVIC_SystemReset();
+ break;
+ case CELDA2:
+ pc.printf("\nCELDA2.\n");
+ mover_celda2();
+ NVIC_SystemReset();
+ break;
+ case CELDA3:
+ pc.printf("\nCELDA3.\n");
+ mover_celda3();
+ NVIC_SystemReset();
+ break;
+ case CELDA4:
+ pc.printf("\nCELDA4.\n");
+ mover_celda4();
+ NVIC_SystemReset();
+ break;
+ default:
+ pc.printf("No se encontro parametro valido.\n");
+ break ;
+ }//Fin Switch
+
+
+ }else if(telecomando == PINZA){
+ pc.printf("PINZA\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+ switch (parametro) {
+ case ABRIR_PINZA:
+ pc.printf("\nABRE PINZA..\n");
+ abrir_pinza();
+ break;
+ case CERRAR_PINZA:
+ pc.printf("\nCIERRA PINZA..\n");
+ cerrar_pinza();
+ break;
+ default:
+ pc.printf("No se encontro comando valido.\n");
+ break ;
+ }//Fin Switch
+
+ }else if(telecomando == M1_MANUAL){
+ pc.printf("M1_MANUAL\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+
+ mover_servo(telecomando, parametro);
+
+ }else if(telecomando == M2_MANUAL){
+ pc.printf("M2_MANUAL\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+
+ mover_servo(telecomando, parametro);
+ }else if(telecomando == M3_MANUAL){
+ pc.printf("M3_MANUAL\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+
+ mover_servo(telecomando, parametro);
+
+ }else if(telecomando == M4_MANUAL){
+ pc.printf("M4_MANUAL\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+
+ mover_servo(telecomando, parametro);
+
+ }else if(telecomando == VELOCIDAD){
+ pc.printf("VELOCIDAD\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+ switch (parametro) {
+ case VELOCIDAD_LENTA:
+ pc.printf("\nVELOCIDAD_LENTA..\n");
+ break;
+ case VELOCIDAD_MEDIA:
+ pc.printf("\nVELOCIDAD_MEDIA..\n");
+ break;
+ case VELOCIDAD_ALTA:
+ pc.printf("\nVELOCIDAD_ALTA..\n");
+ break;
+ default:
+ pc.printf("No se encontro comando valido.\n");
+ break ;
+ }//Fin Switch
+
+ }else if(telecomando == JOYSTICK){
+ pc.printf("JOYSTICK\n");
+
+ pc.printf("Validando Parametro: ");
+ pc.printf("%02x",parametro);
+ switch (parametro) {
+ case INICIA_JOYSTICK:
+ pc.printf("\nINICIA_JOYSTICK ..\n");
+ mover_con_joystick();
+ break;
+ case PARA_JOYSTICK:
+ pc.printf("\nPARA_JOYSTICK..\n");
+ break;
+ default:
+ pc.printf("No se encontro comando valido.\n");
+ break ;
+ }//Fin Switch
+
+
+ }else if(telecomando == COLOR){
+ pc.printf("COLOR\n");
+
+ leer_color();
+
+ }else {
+ pc.printf("Telecomando No Encontrado...\n");
+ }//Fin Validación Telecomando
+ }//Fin While
+}// Fin main
+
+/*CONFIGURA VELOCIDAD DE TRAMSMISIÓN*/
+void setup_uart()
+{
+ pc.baud(9600);
+}
+
+/*CONFIGURACIÓN DE SERVOS*/
+void setup_servo()
+{
+ myservo1.period_ms(20);
+ myservo2.period_ms(20);
+ myservo3.period_ms(20);
+ myservo4.period_ms(20);
+}
+
+
+//MOVER BASE
+void mover_base(int posicion)
+{
+ myservo1.pulsewidth_us(posicion);
+}
+//
+//MOVER IZQUIERDO
+void mover_izquierdo(int posicion)
+{
+ myservo2.pulsewidth_us(posicion);
+}
+
+//MOVER DERECHO
+void mover_derecho(int posicion)
+{
+ myservo3.pulsewidth_us(posicion);
+}
+
+//MOVER PINZA
+void mover_pinza(int posicion)
+{
+ myservo4.pulsewidth_us(posicion);
+}
+
+
+//MUEVE A POSICIÓN HOME
+void mover_home()
+{
+ myservo1.pulsewidth_us(600);
+ wait(1.0);
+ myservo2.pulsewidth_us(600);
+ wait(1.0);
+ myservo3.pulsewidth_us(600);
+ wait(1.0);
+}
+
+//MUEVE A POSICIÓN MATERIAL
+void mover_material()
+{
+ myservo1.pulsewidth_us(700);
+ wait(1.0);
+ myservo2.pulsewidth_us(650);
+ wait(1.0);
+ myservo3.pulsewidth_us(630);
+ wait(1.0);
+}
+
+//MUEVE A POSICIÓN CELDA1
+void mover_celda1()
+{
+ myservo1.pulsewidth_us(800);
+ wait(1.0);
+ myservo2.pulsewidth_us(700);
+ wait(1.0);
+ myservo3.pulsewidth_us(660);
+ wait(1.0);
+}
+
+//MUEVE A POSICIÓN CELDA2
+void mover_celda2()
+{
+ myservo1.pulsewidth_us(900);
+ wait(1.0);
+ myservo2.pulsewidth_us(750);
+ wait(1.0);
+ myservo3.pulsewidth_us(690);
+ wait(1.0);
+}
+
+//MUEVE A POSICIÓN CELDA3
+void mover_celda3()
+{
+ myservo1.pulsewidth_us(1000);
+ wait(1.0);
+ myservo2.pulsewidth_us(770);
+ wait(1.0);
+ myservo3.pulsewidth_us(720);
+ wait(1.0);
+}
+
+//MUEVE A POSICIÓN CELDA4
+void mover_celda4()
+{
+ myservo1.pulsewidth_us(1100);
+ wait(1.0);
+ myservo2.pulsewidth_us(780);
+ wait(1.0);
+ myservo3.pulsewidth_us(750);
+ wait(1.0);
+}
+
+//ABRE PINZA
+void abrir_pinza()
+{
+ myservo4.pulsewidth_us(600);
+}
+
+//CIERRA PINZA
+void cerrar_pinza()
+{
+ myservo4.pulsewidth_us(900);
+}
+
+// Mover Servo con posicion
+void mover_servo(uint8_t motor, uint8_t pos){
+
+ int position = degrees2usec(pos);
+
+ switch (motor) {
+
+ case 0x01:
+ myservo1.pulsewidth_us(position);
+ break;
+ case 0x02:
+ myservo2.pulsewidth_us(position);
+ break;
+ case 0x03:
+ myservo3.pulsewidth_us(position);
+ break;
+ case 0x04:
+ myservo4.pulsewidth_us(position);
+ break;
+ default: break;
+ }
+
+}
+
+/*CONVERTIR GRADOS A SEGUNDOS*/
+uint32_t degrees2usec(uint8_t grados)
+{
+// Retorna el valor en microsegundos, donde
+// y − y1 = m(x − x1 )
+// y= b + mx
+
+ if(grados <= DEGREES_MAX)
+ return int(750+grados*1900/180);// u6
+ return 750;
+
+}
+
+/*MOVIMIENTOS CON JOYSTICK*/
+void mover_con_joystick(){
+
+ double base = 0.0;
+ double izquierdo = 0.0;
+ double derecho = 0.0;
+
+ int posBase=600;
+ int posIzquierdo=600;
+ int posDerecho=600;
+
+ while(1){
+ base = analog1_value_x.read() * 3.3;
+ izquierdo = analog1_value_y.read() * 3.3;
+ derecho= analog2_value_x.read() * 3.3;
+
+ //Valida Joystick 1 en X - base
+ if (base < 2.0){
+ posBase= posBase+3;
+ mover_base(posBase);
+ } else if(base > 3.0){
+ posBase= posBase-3;
+ mover_base(posBase);
+ }
+
+ //Valida Joystick 1 en Y - Izquierdo
+ if (izquierdo < 2){
+ posIzquierdo= posIzquierdo+3;
+ mover_izquierdo(posIzquierdo);
+ } else if(izquierdo > 3){
+ posIzquierdo= posIzquierdo-13;
+ mover_izquierdo(posIzquierdo);
+ }
+
+ //Valida Joystic 2 en X - Derecho
+ if (derecho < 2){
+ posDerecho= posDerecho+3;
+ mover_derecho(posDerecho);
+ } else if(derecho > 3){
+ posDerecho= posDerecho-3;
+ mover_derecho(posDerecho);
+ }
+
+ if(sw2.read() == 0){
+ cerrar_pinza();
+ } else if(sw2.read() == 1){
+ abrir_pinza();
+ }
+
+ }
+}
+
+//LECTURA DE COLOR
+void leer_color(){
+
+ long red;
+ long green;
+ long blue;
+ long clear;
+
+ red = scolor.ReadRed();
+ green = scolor.ReadGreen();
+ blue = scolor.ReadBlue();
+ clear = scolor.ReadClear();
+
+ if (red < 100 && green<100 && blue<100){
+ pc.printf("COLOR LEIDO: CLARO\n");
+ pc.printf("%#04x","04");
+ pc.printf("\n");
+ }else if (blue<100){
+ pc.printf("COLOR LEIDO: AZUL\n");
+ pc.printf("%#04x","02");
+ pc.printf("\n");
+ }else if (red < 100){
+ pc.printf("COLOR LEIDO: ROJO\n");
+ pc.printf("%#04x","01");
+ pc.printf("\n");
+ }else if (red > 100 && green>100 && blue>100 && red < 140 && green<140 && blue<140){
+ pc.printf("COLOR LEIDO: VERDE\n");
+ pc.printf("%#04x","03");
+ pc.printf("\n");
+ }
+
+ pc.printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
+}
\ No newline at end of file