Revision 0:4b10df072322, committed 2019-04-26
- Comitter:
- andriuxone
- Date:
- Fri Apr 26 16:36:40 2019 +0000
- Commit message:
- Entrega Corte 2 - Sistemas embebidos - Universidad ECCI - Colombia
Changed in this revision
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Fri Apr 26 16:36:40 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/scolor_TCS3200.cpp Fri Apr 26 16:36:40 2019 +0000
@@ -0,0 +1,40 @@
+#include "mbed.h"
+#include "scolor_TCS3200.h"
+
+
+scolor_TCS3200::scolor_TCS3200(PinName s0, PinName s1, PinName s2, PinName s3, PinName s_in) :
+_s0(s0), _s1(s1), _s2(s2), _s3(s3), _s_in(s_in)
+{
+ SetMode(SCALE_100);
+
+};
+
+
+long scolor_TCS3200::ReadRed() { _s2=0; _s3=0; return pulsewidth();}
+long scolor_TCS3200::ReadBlue() { _s2=0; _s3=1; return pulsewidth();}
+long scolor_TCS3200::ReadClear() { _s2=1; _s3=0; return pulsewidth();}
+long scolor_TCS3200::ReadGreen() { _s2=1; _s3=1; return pulsewidth();}
+
+void scolor_TCS3200::SetMode(uint8_t mode) {
+ switch (mode){
+ case SCALE_100: _s0= 1; _s1=1; break;
+ case SCALE_20: _s0=1 ; _s1=0; break;
+ case SCALE_2: _s0=0 ; _s1=1; break;
+ case POWER_DOWN: _s0=0 ; _s1=0; break;
+ }
+};
+
+long scolor_TCS3200::pulsewidth() {
+ while(_s_in);
+ while(!_s_in);
+ timer.start();
+ while(_s_in);
+ while(!_s_in);
+ timer.stop();
+ float pulsewidth_v = timer.read_us();
+ timer.reset();
+ return pulsewidth_v;
+};
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/scolor_TCS3200.h Fri Apr 26 16:36:40 2019 +0000
@@ -0,0 +1,63 @@
+#ifndef SCOLOR_TCS3200_H
+#define SCOLOR_TCS3200_H
+#include "mbed.h"
+ /* **************************************************************************
+
+@fabeltranm 2019
+fbeltranm@ecci.edu.co
+
+
+ datasheet https://www.mouser.com/catalog/specsheets/TCS3200-E11.pdf
+
+
+ S0 Frequency scaling
+ S1 Frequency scaling
+ S2 Photo diode selection
+ S3 Photo diode selection
+ OutFreq Frequency
+
+ -----------------------------------
+ | ____________ ____________ |
+----> | | | | | | ___ ___
+Light | | Photodiode | | Current |--|---OUTPUT_FREQ | |___| |___
+----> | | Array |---| to | |
+ | | | | Frequency | |
+ | |____________| |____________| |
+ | ^ ^ ^ ^ |
+ -------|--|-------------|--|-------
+ | | | |
+ S2 S3 S0 S1
+
+SO | S1 | OUTPUT FREQUENCY SCALING | | S2 | S3 | PHOTODIODE TYPE |
+ 0 | 0 | power down | | 0 | 0 | Red |
+ 0 | 1 | 2% | | 0 | 1 | Blue |
+ 1 | 0 | 20% | | 1 | 0 | Clear (no filter) |
+ 1 | 1 | 100% | | 1 | 1 | Green |
+
+******************************************************************************/
+
+
+#define SCALE_100 1
+#define SCALE_20 2
+#define SCALE_2 3
+#define POWER_DOWN 4
+
+class scolor_TCS3200 {
+ public:
+ scolor_TCS3200(PinName s0, PinName s1, PinName s2, PinName s3, PinName s_in);
+ long ReadRed(); // retorno el tiempo en alto de OutFreq para Rojo en ns
+ long ReadGreen(); // retorno el tiempo en alto de OutFreq para verde en ns
+ long ReadBlue(); // retorno el tiempo en alto de OutFreq color azul en ns
+ long ReadClear(); // retorno el tiempo en alto de OutFreq sin filtro en ns
+ void SetMode(uint8_t mode);
+ private:
+ DigitalOut _s0;
+ DigitalOut _s1;
+ DigitalOut _s2;
+ DigitalOut _s3;
+ DigitalIn _s_in;
+ Timer timer;
+ long pulsewidth();
+
+};
+#endif
\ No newline at end of file