Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Línea_de_Clasificación_de_Materiales
Línea de Clasificación de Materiales por Colores¶
Materiales¶
- Tarjeta de Programación Núcleo STM32F446RE
- Software CoolTerm
- Software MBED
- Sensor de color TCS3200
- Joystick
- Servomotores de piñonería metálica
- Jumpers, cable UTP
- Fuente de voltaje externa (5v)
- Acrílico de 3mm
- Tornillos (Para el ensamble del brazo robótico)
Acelerómetro MPU-6050¶
El sensor MPU6050 y contiene todo lo necesario medir movimiento en 6 grados de libertad, combinando un giroscopio de 3 ejes y un acelerómetro de 3 ejes en un mismo chip. Integra un DMP (Procesador digital de movimiento) capaz de realizar complejos algoritmos de captura de movimiento de 9 ejes.
Se comunica a través de una interfaz I2C y posee una librería muy difundida para su uso inmediato. Este sensor puede entregar 6 grados de libertad e incorpora un regulador de tensión a 3.3V y resistencias pull-up para su uso directo por I2C. Para su uso con arduino se emplea la librería i2cdevlib. Su conexión es sencilla a través de su interfaz I2C master, permitiendo así controlar sensores externos adicionales como magnetómetros o barómetros, entre otros, sin intervención del procesador principal (economizar recursos).
Características del Sensor¶
- Sensor : MPU6050
- Voltaje de operación: 3V/3.3V5V DC
- Regulador de voltaje en placa
- Grados de libertad (DoF): 6
- Rango Acelerómetro: 2g/4g/8g/16g
- Rango Giroscopio: 250Grad/Seg, 500Grad/Seg, 1000Grad/Seg, 2000Grad/Seg
- Sensibilidad Giroscopio: 131 LSBs/dps
- Interfaz: I2C
- Conversor AD: 16 Bits (salida digital)
- Tamaño: 2.0cm x 1.6cm x 0.3cm
Para una captura precisa de movimiento rápido y lento, posee un rango de escala programable de 250/500/1000/2000 grados/seg para el giroscopio y de 2g/4g/8g/16g para el acelerómetro.
Conexión en la Tarjeta¶
A continuación se presenta el esquema respectivo de conexiones, para su mejor entendimiento:
Protocolo I2C¶
Abreviatura de Inter-IC (inter integrated circuits), un tipo de bus diseñado por Philips Semiconductors a principios de los 80s, que se utiliza para conectar circuitos integrados (ICs). El I2C es un bus con múltiples maestros, lo que significa que se pueden conectar varios chips al mismo bus y que todos ellos pueden actuar como maestro, sólo con iniciar la transferencia de datos. Este bus se utiliza dentro de una misma placa de un dispositivo.
El bus I2C, un estándar que facilita la comunicación entre microcontroladores, memorias y otros dispositivos con cierto nivel de «inteligencia», sólo requiere de dos líneas de señal y un común o masa. Fue diseñado a este efecto por Philips y permite el intercambio de información entre muchos dispositivos a una velocidad aceptable, de unos 100 Kbits por segundo, aunque hay casos especiales en los que el reloj llega hasta los 3,4 MHz.
Descripción de Señales¶
- SCL (System Clock) es la línea de los pulsos de reloj que sincronizan el sistema.
- SDA (System Data) es la línea por la que se mueven los datos entre los dispositivos.
- GND (Masa) común de la interconexión entre todos los dispositivos «enganchados» al bus.
Conexión y Características del sensor de Color TCS3200¶
El sensor de color TCS3200 es un convertidor de luz a frecuencia que combina fotodiodos de silicio reconfigurables y una corriente de frecuencia en un solo circuito integrado. La salida es una onda cuadrada (ciclo de trabajo 50%) con una frecuencia directamente proporcional a la intensidad de luz. Las entradas y salidas digitales permiten una interfaz directa con un microcontrolador u otro conjunto de circuitos lógicos, por esta razón el sensor TCS3200 es ideal para líneas de producción, domótica, robótica, etc. El sensor es un convertidor de luz a frecuencia que lee una matriz de 8×8 fotodiodos , de tal manera que 16 fotodiodos tienen filtro azul, 16 fotodiodos tienen filtro verde, 16 fotodiodos tienen filtro rojo y 16 fotodiodos son sin filtro.
Conexión en la tarjeta¶
A continuación se presenta el esquema respectivo de conexiones, para su mejor entendimiento:
Conexión y Características del Joystick¶
Un joystick analógico es un controlador que tiene la ventaja de proporcionar una cantidad de información superior a la obtenida con pulsadores. Internamente los joystick están formados por un sistema de dos ejes ortogonalesacoplados a dos potenciómetros. Estos potenciómetros realizan la medición de laposición de la palanca en ambos ejes. Por otro lado, uno de los ejes está apoyado en un microrruptor, lo que permite detectar la pulsación de la palanca.
Por tanto, los joystick facilitan una señal analógica para la posición de cada eje, más una señal digital para la detección de la pulsación del mando. Al disponer de la entrada en los ejes X e Y en forma analógica.
Conexión en la tarjeta¶
A continuación se presenta el esquema respectivo de conexiones, para su mejor entendimiento:
NOTA wrote:
Para tener en cuenta: Los pines VRx y VRy del primer joystick van conectados a los pines A0 y A1 respectivamente, estos corresponden a las puertos o pines análogos de la tarjeta y el pin SW del joystick, irá conectado al pin PA_9 (D8) de la tarjeta, ya que este correspnde a una entrada digital. Para el segundo joystick sólo se conectará el puerto VRx al pin A2.
Conexión y Características de los Motores¶
Los motores usados para la programación y el montaje del brazo robótico son servomotores MG995 de piñonerí metálica 180°
Características:¶
- Modulación: Digital
- Torque (kg)(4.8v): 8.5
- Stall Torque: 4.8V: 130.54 oz-in (9.40 kg-cm) 6.0V: 152.76 oz-in (13.00 kg-cm)
- Velocidad: 4.8V: 0.20 sec/60° 6.0V: 0.16 sec/60°
- Periodo: 20 ms
- Ancho pulso: 1.0 ~ 2.5 ms
- Peso: 1.94 oz (55.0 g)
- Dimensiones: Largo:1.60 in (40.7 mm) Ancho:0.78 in (19.7 mm) Alto:1.69 in (42.9 mm)
Conexión de los motores en la tarjeta¶
Se ubica en el motor y en la tarjeta el GND y el positivo de los mismos (en el motor: cable café=GND, Cable rojo=Positivo, Cable amarillo=Control PWM), y se procede a realizar la respectiva conexión.
Se ubica, dependiendo el pinout de la tarjeta de programacíon un periferico que maneje PWM y así mismo, especificado en el programa prinicipal, para conectar el cable de control del motor
A continuación se presenta el esquema respectivo de conexiones, para su mejor entendimiento:
NOTA wrote:
Para tener en cuenta: Los puertos de control de los motores (cable amarillo del mismo), estarán conectados a los puertos: servo #1 PA_10 (D2), servo #2 PB_3(D3), servo #3 PB_5(D4), servo #4 PB_4(D5), los cables cafe y rojo de cada servomotor corresponden a la alimentación, cable rojo irá a 5v y cable cafe a GND, es importante tener conmutadas las tierras.
Selección de la Tarjeta de Programación¶
En esta sección se explica la forma de escoger o de cargar al MBED, la tarjeta de programación que se usará, en este caso, será la STMNucleo-F411RET6.
PASO 1¶
Primeramente, ingresamos a nuestra cuenta MBED
PASO 2¶
Para seleccionar la tarjeta correspondiente, en la esquina superior derecha, encontramos el nombre de la tarjeta, damos click en el recuadro.
PASO 3¶
Nos aparece un recuadro en cuál nos aparece la tarjeta ya cargada en la plataforma MBED, sólo la seleccionamos y damos click en "Select Platform"
PASO 4¶
En caso de no tenerla cargada en la plataforma, damos click en "Add Board", lo que nos redigirá a una nueva pestaña, en donde encontarremos un gran número de componentes y tarjetas de programación
PASO 5¶
Buscamos, con ayuda de CNTRL+F, el nombre de la tarjeta Nucleo-F411RE
PASO 6¶
Damos click en el recuadro de la tarjeta, esto nos abre una pesataña en donde encontramos toda la información con respecto a la tarjeta, las características, el pinout de la misma, y demás.
PASO 7¶
En los recuadros de la parte inferior derecha, aparecerá un recuadro que dice: "Add to your MBED compiler", damos click en el y automáticamente quedará cargada en nuestra cuenta de MBED y se procede a seguir el paso 3, anteriormente mencionado.
PASO 8¶
Para crear un nuevo programa, en la parte superior izquierda, encontramos el ícono de "NEW", damos click en el
PASO 9¶
Inmediatamente después, nos aparecerá un recuadro en el que podemos ver la tarjeta o la plataforma de programación, nos permite escoger una plantilla y obviamente, ponerle un nombre al programa.
PASO 10¶
Nos abrirá una página en el que aparece un archivo main.cpp, damos click en el y nos llevara a la página para crear el código
PASO 11¶
Y listo, es todo lo que se necesita para poder empezar a programar en la plataforma.
Descarga del Programa CoolTerm¶
El programa CoolTerm, es una aplicación o programa que se usa para comunicar a través del puerto serial a la tarjeta con el PC y enviar dichos valores en hexadecimal.
Primero se abre el CoolTerm y se da click en "Connect", para realizar la conexión entre la tarjeta y el pc, esto por medio de un cable de datos
Inmediatamente, se despliega un recuadro en el cuál debemos establecer el puerto de la tarjeta, la velocidad que se estableció previamente en el programa para la comunicación entre la tarjeta y el pc (115.200), dejamos las demás opciones por defecto y damos click en "OK"
Nos dirigimos a la barra de herramientas, ubicamos la pestaña que dice "connection" y clickeamos en la opción "send string", pues desde allí se enviarán los datos.
Se abre el recuadro y ubicamos la opción "HEX", para empezar a enviar los parámetros y los comandos en hexadecimal
Se envía un parámetro FF para inicializar el programa, seguidamente se envía un 01 o un 02, para determinar el tipo de comando y por último, se envía el valor del respectivo Parámetro
Descripción del programa¶
Este programa se trabajó en la plataforma online MBED, por medio de un lenguaje de programación c++ orientado a objetos. Se pretende mover un brazo robótico que consta de 4 servomotores, el cuál se explica con detalle a continuación.
Programa Proyecto
#include "mbed.h" #include "scolor_TCS3200.h" #include "MPU6050.h" I2C i2c(PB_9, PB_8); Serial command(USBTX, USBRX); //Es la comunicación serial entre la tarjeta y el PC PwmOut myservo1(PA_10); //Asigno PWM al pin PA_10 (D2)para el servomotor #1 PwmOut myservo2(PB_3); //Asigno PWM al pin PB_3 (D3)para el servomotor #2 PwmOut myservo3(PB_5); //Asigno PWM al pin PB_5 (D4)para el servomotor #3 PwmOut myservo4(PB_4); //Asigno PWM al pin PB_4 (D5)para el servomotor #4 AnalogIn analog_valueX1(A0); //Asigno una entreada analogica en A0 para el Joystick Eje X1 AnalogIn analog_valueY1(A1); //Asigno una entreada analogica en A1 para el Joystick Eje Y1 AnalogIn analog_valueX2(A2); //Asigno una entreada analogica en A2 para el Joystick Eje X2 InterruptIn switchMS(PA_9,PullUp); //Asigno una entrada digital para switch del Joystick InterruptIn switchMS2(PC_7,PullUp); //Asigno una entrada digital para switch del Joystick Ticker mostrar_color; MPU6050 acc(&i2c); MPU6050 gyro(&i2c); // S0, S1, S2, S3, OUT scolor_TCS3200 scolor(PA_8, PB_10, PB_9, PB_6, PB_8); //Asigno los pines de entrada y salida para el sensor de color
En estas líneas de código, se asignan las respectivas librerías, seguidamente, se establece la comunicación serial entre la tarjeta de programación y el pc, asigno los respectivos PWM a los pines de la tarjeta, al igual que las entradas analógicas del joystick, la entrada digital del switch del mismo y por último, los pines de entrada del sensor de color
#define INITCMD 0xFF //Es una varíable global para inicializar el programa #define INITTELE 0xFE //Variable global de telemetria #define DEGREES_MAX 180 //Variable que determina el numero de grados maximos a usar #define CMD 0x09 //Tipo de comando para el sensor de color #define ROJO 0x01 //Telemetria para el color rojo #define AZUL 0x02 //Telemetria para el color azul #define VERDE 0x03 //Telemetria para el color verde #define AMARILLO 0x04 //Telemetria para el color amarillo #define NINGUNO 0x00 //Telemetria para cuando no halla color correspondiente
Se procede a definir las variables globales de los telecomandos y la telemetría correspondiente, teniendo en cuenta los datos de la tabla de telecomandos, anexada posteriormente en el documento
uint8_t N_parametro; //Asigno variable de 8 bits, al parametro que voy a leer de los comandos uint8_t N_tipocomando; //Asigno variable de 8 bits, al comando que voy a leer de los comandos long frec_clear; //Asigno variable para la frecuencia de clear long frec_red; //Asigno variable para la frecuencia de rojo long frec_blue; //Asigno variable para la frecuencia de azul long frec_green; //Asigno variable para la frecuencia de verde float Eje_X1; //Asigno variable para la lectura del conversor en el eje x1 float Eje_Y1; //Asigno variable para la lectura del conversor en el eje y1 float Eje_X2; //Asigno variable para la lectura del conversor en el eje x2 int MS_x1; //Asigno variable para los grados en decimales del eje x1 int MS_y1; //Asigno variable para los grados en decimales del eje y1 int MS_x2; //Asigno variable para los grados en decimales del eje x2
Asigno cada una de las variables que necesito a lo largo del programa, y de igual manera el tipo de dato que recibirá.
void setup_uart(); //Funcion para determinar la velocidad de comunicacion del PC a la tarjeta void setup_servo(int velocidad); //Funcion que me permitira modificar la velocidad de los servomotores void leer_datos(); //Funcion que se usa para leer los comandos enviados desde el PC void posicion(uint8_t tipocomando, uint8_t parametro); //Funcion principal que permite realizar todos los tipos de movimientos con los servos void leer_color(); //Funcion que determina la lectura del sensor de color void joystick_ADC( float meas_x1, float meas_y1, float meas_x2); //Funcion que convierte los valores del conversor en grados decimales void leer_joystick(); //Funcion que asigna los valores obtenidos en el conversor a una variable de tipo float void mover_servojoystick(int grados_x1, int grados_y1, int grados_x2); //Funcion que permite mover los servos dependiendo de los grados que se lean void def_color(long rojo, long azul, long verde, long clear); void mover_color(long rojo, long azul, long verde, long clear);
Determino cada una de las funciones que se usaran para el movimiento de los servomotores, para el sensor de color y el joystick, respectivamente.
void config_servos(uint32_t dpulse){
myservo1.period_ms(30); //Se modifica la frecuencia con la que se envian los ciclos de trabajo a un estado estandar
myservo1.pulsewidth_us(dpulse); //Establesco mi ancho de pulso para inicializar los servomotores
myservo2.period_ms(30); //
myservo2.pulsewidth_us(dpulse); // |Uso la misma configuracion en los servomotores 1,2 y 3 debido a que son los que |
myservo3.period_ms(30); // | sufriran el cambio de velocidad segun el usuario lo requiera |
myservo3.pulsewidth_us(dpulse); //
myservo4.period_ms(20); //Asigno los estados standar a el servomotor de la pinza ya que no requiere de un cambio
myservo4.pulsewidth_us(1000); // en su velocidad
}
Programo la función config_servos, a la cuál se le asignará una variable dpulse (uint32_t: es una variable de tipo enteros normales sin signo), posteriormente establecemos el periodo de los servomotores en 30 ms y el ancho de pulso de los motorers, será el valor ingresado, que se almacenara en la variable dpulse
void open_pinza()
{
myservo4.pulsewidth_us(1776); //Asigno los us para mover el servo de la pinza
}
void close_pinza()
{
myservo4.pulsewidth_us(2352); //Asigno los us para mover el servo de la pinza
}
Programamos la función abrir y cerrar pinza; después de transformar de grados a us, determinamos un valor de 1776(72°) y 2352(120°), para la apertura y el cierre de la pinza, respectivamente
void mover_manual()
{
uint8_t ciclo =1;
while(ciclo == 1){
leer_joystick(); //Llamo a la funcion que me indica los valores del conversor
joystick_ADC(Eje_X1, Eje_Y1, Eje_X2); //Llamo a la funcion que me da los valores en grado y le asigno los valores del convertidor que se usaran dentro de la funcion
mover_servojoystick(MS_x1, MS_y1, MS_x2); //Llamo a la funcion que usa los valores en grados que se estan asignando para mover los servos
while (switchMS == 0) //Verifico que se este o no oprimiendo el switch del joystick
{
printf("en boton");
close_pinza();
}
open_pinza();
if (command.readable())
{
printf("dato leido");
leer_datos();
if (N_tipocomando == 0x08 && N_parametro== 0x02)
{
ciclo = 2;
}
}
}
}
Se programa la función mover_manual, lo que nos permitirá manejar o mover el brazo robótico a través de los Joystick, primeramente, se especifica una variable ciclo (uint8_t: número entero sin signo de 1 bit), esta se activará con el telecomando 08 y con el parámetro 01; mientras que la variable ciclo sea igual a 01, se llamará a la función leer_joystick, luego llamo a la función joystick_ADC y por último, llamo a la función mover_servojoystick, en seguida, nos enfocamos en la pinza, mientras que el switch reciba o tenga un valor de 1, la pinza se abrirá hasta que se envíe un 0, es decir, hasta que se deje de presionar el switch; por otro lado, si el tipo de comando enviado es 08 y el parámetro es 02, se le asignará un valor entero de 2 a la variable ciclo y por consiguiente, se saldrá del bucle while y los joystick quedarán desactivados.
void leer_acelerometro()
{
float dato [3];
acc.getAccelero(dato);
printf("aceleracion_x = %f, aceleracion_y = %f, aceleracion_z = %f \n",dato[0], dato[1], dato[2]);
}
void leer_giroscopio()
{
float dato [3];
gyro.getGyro(dato);
printf("giro_x = %f, giro_y = %f, giro_z = %f \n",dato[0], dato[1], dato[2]);
}
Se declaran las funciones leer_acelerometro y leer giroscopio, las cuales se encargaran de almacenar y mostrar los datos obtenidos por el acelerometro MPU 6050
int main() //Funcion Principal de Configuracion
{
setup_uart(); //Llamo a la Funcion para iniciar la configuracion de la comunicacion
uint32_t dpulse=0;
dpulse=(700+90*1700/180);
config_servos(dpulse);
uint32_t dpulsep=0; //
dpulsep=(700+110*1700/180); //
myservo4.pulsewidth_us(dpulsep); //Inicializo con la pinza cerrada
wait(0.5); //
while(1) //Ciclo infinito de las funciones que va a ejecutar la tarjeta
{
leer_datos(); //Llama la funcion que me permite leer datos constantemente de los comandos recibidos
Dentro del programa principal, se llama a la función setup_uart, para iniciar la comunicación entre la tarjeta y el PC, se inicializa la variable dpulse en 0, seguidamente se cargará dicha variable con 90° (1550 us), esto hará que todos los servomotores se muevan o se pongan en 90°, se hará lo mismo con el servomotor de la pinza, se inicializa con la pinza cerrada, asignandole un valor de 110° (1738 us), se realiza una espera de 0.5 ms y se abre un while(1), lo que nos permite entrar en un ciclo infinito de las funciones que realizará el programa en adelante, por ende, se llama la función leer_datos(), para estar leyendo constantemente los telecomandos recibidos
if (N_tipocomando<=6)
posicion(N_tipocomando, N_parametro);
else
switch (N_tipocomando){
case(7): setup_servo(N_parametro);break;
case(8): mover_manual(); break;
case(9):
leer_color(); //Invoco la funcion que se encarga de determinar el color
//def_color (frec_red, frec_blue, frec_green, frec_clear); //Llamo a la funcion que me indica los colores, ademas le asigno las variables de frecuencia obtenidas en la anterior funcion
mover_color(frec_red, frec_blue, frec_green, frec_clear);
break;
case(10):
leer_acelerometro();
leer_giroscopio();
break;
break;
}
}
}
En esta parte del código, se crea un if, para preguntar si la variable N_tipocomando es menor o igual a 6, si es así, se llamará la función posición y se asiganarán los valores a las variables N_tipocomando y N_parámetro, de lo contrario, se entrará a un switch case, en el que dependiendo el valor asignado en la variable N_tipocomando, entrará a cada uno de los case, en caso de que el valor sea 7, se llamará a la función setup_servo, si el valor asignado es 8; se llamará a la función mover_manual, si el valor asignado es 9, se llamará a la función leer_color, seguidamente, se llama la función def_color y a su vez, mover_color; la instrucción break, nos permite salirnos del case, en caso de que el valor cambie y si el valor asignado es 10, se llamaran las funciones leer_acelerometro y leer_gyroscopio.
void leer_datos() //Aqui hago la configuracion de la funcion que va a leer los datos de los comandos
{
while(command.getc()!= INITCMD); //Aqui se monitorea y se confirma si el programa esta inicializado con el comando FF
N_tipocomando=command.getc(); //Asigna el valor introducido en tipocomando a la variable N_tipocomando
N_parametro=command.getc(); //Asigna el valor introducido en parametro a la variable N_parametro
}
Se programa la función leer_datos, dentro del ciclo while, nos aseguramos de que el programa se esté inicializando con el comando FF, después, se asigna el valor introducido de las variables N_tipocomando y N_tipoparámetro; la instrucción command.getc(), es el que se encarga de asignar o de poner los datos o valores introducidos en estas variables
void setup_uart() //Aqui configuro la velocidad de la comunicacion
{
command.baud(115200); //Establesco el valor que quiero comunicar la PC y la Tarjeta
}
Se programa la función setup_uart, en la que se determina la velocidad de comunicación entre la tarjeta y el PC, es enste case la velocidad es de (115200)
void setup_servo(int velocidad) //Aqui configuro la velocidad que requiera el usuario a la hora de mover los servos
{
if (velocidad == 1) //Si el paramatro ingresado es 0x01 configuro a una velocidad menor
{
uint32_t dpulse=0;
dpulse=(700+90*1700/180);
myservo1.period_ms(127); //Establesco un valor de 127 ms en el periodo para lograr una velocidad menor
myservo1.pulsewidth_us(dpulse);
myservo2.period_ms(127); //Establesco un valor de 127 ms en el periodo para lograr una velocidad menor
myservo2.pulsewidth_us(dpulse);
myservo3.period_ms(127); //Establesco un valor de 127 ms en el periodo para lograr una velocidad menor
myservo3.pulsewidth_us(dpulse);
}
if (velocidad == 2) //Si el paramatro ingresado es 0x02 configuro a una velocidad media
{
uint32_t dpulse=0;
dpulse=(700+90*1700/180);
myservo1.period_ms(76); //Establesco un valor de 76 ms en el periodo para lograr una velocidad media
myservo1.pulsewidth_us(dpulse);
myservo2.period_ms(76); //Establesco un valor de 76 ms en el periodo para lograr una velocidad media
myservo2.pulsewidth_us(dpulse);
myservo3.period_ms(76); //Establesco un valor de 76 ms en el periodo para lograr una velocidad media
myservo3.pulsewidth_us(dpulse);
}
if (velocidad == 3) //Si el paramatro ingresado es 0x03 configuro a una velocidad mayor
{
uint32_t dpulse=0;
dpulse=(700+90*1700/180);
myservo1.period_ms(20); //Establesco un valor de 20 ms en el periodo para lograr una velocidad mayor
myservo1.pulsewidth_us(dpulse);
myservo2.period_ms(20); //Establesco un valor de 20 ms en el periodo para lograr una velocidad mayor
myservo2.pulsewidth_us(dpulse);
myservo3.period_ms(20); //Establesco un valor de 20 ms en el periodo para lograr una velocidad mayor
myservo3.pulsewidth_us(dpulse);
}
}
Se procede a programar o configurar la función setup_servo, a la cuál se le asignará una variable velocidad, cuando a esta se le asigne un parámetro de 01, la velocidad de los servomotores será lenta, inicializamos la vairable depulse en 0, luego, establecemos esta para que inicie en una posición de 90°, en seguida, se establece el período en ms del motor 127(vel. lenta), 76(vel.media) y 20(vel.rápida) y al ancho de pulso, se le asigna la variable dpulse.
void leer_color() //Configuro la funcion que lee el color que este detectando el sensor
{
long red ; //variable
long green ; //variable
long blue ; //variable
long clear ; //variable
red = scolor.ReadRed(); //Asigno mi lectura de rojo en una variable
green = scolor.ReadGreen(); //Asigno mi lectura de verde en una variable
blue = scolor.ReadBlue(); //Asigno mi lectura de azul en una variable
clear = scolor.ReadClear(); //Asigno mi lectura de clear en una variable
frec_red= 100*clear/red; //Paso la lectura de tiempo a frecuencia del rojo
frec_blue= 100*clear/blue; //Paso la lectura de tiempo a frecuencia del azul
frec_green= 100*clear/green; //Paso la lectura de tiempo a frecuencia del verde
frec_clear= clear; //Paso la lectura de tiempo a frecuencia del clear
//printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", frec_red, frec_green, frec_blue, frec_clear);
}
Se configura la función leer_color(), se determinan los colores red, green, blue y el clear, el cuál será un valor de referencia, como variables de tipo long (entero largo), luego asigno la lectura de los colores, mostradas en tiempo o período, en estas variables, una vez se leen estos valores, se pasa del tiempo a la frecuencia, teniendo en cuenta o como referencia el valor del clear, por medio de la inversa, estos valores se almacenaran en las variables frec_red, frec_blue, frec_green y frec_clear.
void def_color (long rojo, long azul, long verde, long clear) //Configuro las definiciones de los coleres respecto a los valores
{
if (rojo > azul and azul > verde) //Determino el color mediante comparaciones
{
command.putc(INITTELE); //Envio la telemetria de inicio
command.putc(ROJO); //Envio la telemetria del color rojo
}
if (rojo < azul and azul >= 33 and azul <= 37) //Determino el color mediante comparaciones
{
command.putc(INITTELE); //Envio la telemetria de inicio
command.putc(VERDE); //Envio la telemetria del color verde
}
if (rojo < azul and azul >= 40 and azul <= 57) //Determino el color mediante comparaciones
{
command.putc(INITTELE); //Envio la telemetria de inicio
command.putc(AZUL); //Envio la telemetria del color azul
}
if (rojo > verde and verde > azul) //Determino el color mediante comparaciones
{
command.putc(INITTELE); //Envio la telemetria de inicio
command.putc(AMARILLO); //Envio la telemetria del color amarillo
}
}
Configuro la función def_color(), teniendo en cuenta los valores leídos por las variables red, blue, green y clear, se crea una instrucción if, que me permita comparar estos valores y así deerminar el color que está leyendo el sensor, la instrucción command.putc(INITTELE), me permite visualizar el telecomando FE en el coolterm y la isntrucción command.putc(ROJO), me permite visualizar el parámetro 01, definidas como variables globales en la parte inicial del programa, este proceso, será el mismo para los otros 3 colores: (AZUL)-02, (VERDE)-03 y (AMARILLO)-04.
void leer_joystick () //Configuro la funcion que me permite obtener los valores del ADC y os asigno en una variable
{
wait(0.1);
Eje_X1 = analog_valueX1.read(); //Asigno el valor del eje X1 en una variable
Eje_Y1 = analog_valueY1.read(); //Asigno el valor del eje y1 en una variable
Eje_X2 = analog_valueX2.read(); //Asigno el valor del eje X2 en una variable
}
Programamos la función leer_joystick(), que me permite leer los valores enviados del ADC y los asigno en las variables Eje_X1, Eje_Y1 y Eje_X2
void joystick_ADC( float meas_x1, float meas_y1, float meas_x2)//Configuro la funcion de donde hago la conversion de valores del ADC a grados
{
MS_x1=(180*meas_x1); //Opero para obtener los grados del eje X1
MS_y1=(180*meas_y1); //Opero para obtener los grados del eje Y1
MS_x2=(180*meas_x2); //Opero para obtener los grados del eje X2
}
Programamos la función joystick_ADC, en la cuál, teniendo en cuenta los valores recibidos, son asignados en unas variables locales meas_x1, meas_y1 y meas_x2, lo que nos permitirá transformar de voltios (0v-3.3v) a grados(0°-180°) por medio de la inversa, una vez hecho esto, el valor, será almacenado en una variable MS_x1, MS_y1 y MS_x2, respectivamente.
void mover_servojoystick (int grados_x1, int grados_y1, int grados_x2) //Configuro la funcion en la que realizo los moviemientos de los servos con el joystick
{
uint32_t dpulse1; //Variable para el servo 1
dpulse1=(700+grados_x1*1700/180); //Adquiero los grados y los convierto en us
myservo1.pulsewidth_us(dpulse1); //Asigno los us en el ancho de pulso del servo 1
uint32_t dpulse2; //Variable para el servo 2
dpulse2=(700+grados_y1*1700/180); //Adquiero los grados y los convierto en us
myservo2.pulsewidth_us(dpulse2); //Asigno los us en el ancho de pulso del servo 2
uint32_t dpulse3; //Variable para el servo 3
dpulse3=(700+grados_x2*1700/180); //Adquiero los grados y los convierto en us
myservo3.pulsewidth_us(dpulse3); //Asigno los us en el ancho de pulso del servo 3
//printf("%i %i %i \n", grados_x1, grados_y1, grados_x2);
}
Programamos la función mover_servojoystick(), asigno los valores obtenidos a las variables grados_x1, grados_y1 y grados_x2, asigno una variable dpulse1, para el servomotor 1, dpulse2 para el servomotor 2 y dpulse 3 para el servomotor 3, cada una de estas variables, se cargarán con los datos de las variables locales grados_x1, grados_y1 y grados_x2 y se realizará la conversión de grados a us, para configurar el ancho de pulso de cada motor.
uint32_t gradosamover(uint8_t parametro) //Aqui se realiza la conversion de grados a us para cuando se requiera el movimiento individual de los motores
{
if(parametro <= DEGREES_MAX) //Se asegura que el limite de grados maximos sean de 180
return int(700+parametro*1700/180); //Con esta operacion de realiza el cambio de grados a us
return 700; //Retornamos la variable a un valor de 700 us
}
Esta variable gradosamover realiza la conversión de grados a us cuando se requiera mover los servomotores individualmente, es decir uno por uno, la variable parámetro, se cargará con un valor de 0° hasta 180° (00-b4) en hexadecimal, se crea una instrucción if, en el que se asegura que el valor asignado a la función parametro sea menor de 180°, si es así, se convierte dicho valor a us y seguidamente, retornamos la variable a un valor de 700 us
void posicion(uint8_t tipocomando,uint8_t parametro) //Se crea la función posición y se asignan los valores leídos a las variables tipocomando y parametro
{
if (tipocomando == 1) //Se crea un if para saber que tipo de comando se desea ingresar
{
if (parametro == 1) //Posicion Home
{
uint32_t dpulse=0;
dpulse=(700+90*1700/180);
myservo2.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+90*1700/180);
myservo3.pulsewidth_us(dpulse);
wait(0.5);
dpulse=(700+120*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
if (parametro == 2) //mover a posición material
{
uint32_t dpulse=0;
dpulse=(700+20*1700/180);
myservo1.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+130*1700/180);
myservo2.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+40*1700/180);
myservo3.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+70*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
if (parametro == 3) //mover a posición celda 1
{
uint32_t dpulse=0;
dpulse=(700+90*1700/180);
myservo1.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo2.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo3.pulsewidth_us(dpulse);
wait(0.5);
dpulse=(700+120*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
if (parametro == 4) //mover a posición celda 2
{
uint32_t dpulse=0;
dpulse=(700+110*1700/180);
myservo1.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo2.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo3.pulsewidth_us(dpulse);
wait(0.5);
dpulse=(700+120*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
if (parametro == 5) //mover a posición celda 3
{
uint32_t dpulse=0;
dpulse=(700+130*1700/180);
myservo1.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo2.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo3.pulsewidth_us(dpulse);
wait(0.5);
dpulse=(700+120*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
if (parametro == 6) //mover a posición celda 4
{
uint32_t dpulse=0;
dpulse=(700+150*1700/180);
myservo1.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo2.pulsewidth_us(dpulse);
wait(0.5);
dpulse=0;
dpulse=(700+110*1700/180);
myservo3.pulsewidth_us(dpulse);
wait(0.5);
dpulse=(700+120*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
}
La función posición, almacena los datos enviados en las variables tipocomando y tipo parámetro, teniendo en cuenta el comando y el parámetro enviado, entrará a cada uno de los if y realizará la acción establecida por defecto, es decir, en caso de que el comando sea 01 y el parámetro sea 01, inicializamos la variable dpulse con 0, establecemos los grados de los motores a mover, en este caso posición home, se realiza una espera de 0.5 ms y se espera a que se envíe el próximo comando o parámetro
if(tipocomando==2) //if del comando abrir y cerrar pinza
{
if (parametro==1) //abrir pinza
{
uint32_t dpulse=0;
dpulse=(700+70*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
if (parametro==2) //cerrar pinza
{
uint32_t dpulse=0;
dpulse=(700+120*1700/180);
myservo4.pulsewidth_us(dpulse);
wait(0.5);
}
}
if(tipocomando==3)
{
uint32_t dpulse=0;
dpulse=gradosamover(parametro);
myservo1.pulsewidth_us(dpulse);
}
if(tipocomando==4)
{
uint32_t dpulse=0;
dpulse=gradosamover(parametro);
myservo2.pulsewidth_us(dpulse);
}
if(tipocomando==5)
{
uint32_t dpulse=0;
dpulse=gradosamover(parametro);
myservo3.pulsewidth_us(dpulse);
}
if(tipocomando==6)
{
uint32_t dpulse=0;
dpulse=gradosamover(parametro);
myservo4.pulsewidth_us(dpulse);
}
}
Tabla de Telecomandos para la operación del Brazo Robótico¶
A continuación, se anexa una tabla, con el fin de guiar al ususario para poder diferenciar los diferentes comandos y parámetros, ya que de esta forma, pueda hacer un uso correcto o eficiente del brazo robótico y de todas sus funciones.
Quote:
El sensor de color estará leyendo y recibiendo datos indefinidamente, en el coolterm se mostrara el telecomando FE, lo que significa que el sensor está testeando junto con el parámetro que se le fue asignado a cada color:
- Rojo = 01;
- Azul = 02;
- Verde = 03;
- Amarillo = 04;
Nota:En los telecomandos para mover los motores independientemente, al ingresar el parámetro, se puede ingresar valores en hexadecimal desde 0°(0x00), hasta 180° (0xb4)
Planos del Brazo Robótico¶
A continuación, se anexan los planos del brazo robótico, usados para este proyecto. /media/uploads/barrero_99/corte_brazo_robotico.pdf