Brazo_robotico
luis marin.
3
replies
Brazo-robotico
#include "mbed.h"
#include "scolor_TCS3200.h"
/***
Este archivo es una modificacion al ejercicio propuesta por el ingeniero Ferney alberto Beltran
molina.
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 | Tipo_Comando | Parametro_1 | Funcion |
| 0xff | 0x01 | 0x01 | Pos. Home |
| 0xff | 0x01 | 0x02 | Pos. Material |
| 0xff | 0x01 | 0x03 | Pos. Celda 1 |
| 0xff | 0x01 | 0x04 | Pos. Celda 2 |
| 0xff | 0x01 | 0x05 | Pos. Celda 3 |
| 0xff | 0x01 | 0x06 | Pos. Celda 4 |
| 0xff | 0x02 | 0x01 | Abre Pinza |
| 0xff | 0x02 | 0x02 | Cierra Pinza |
| 0xff | 0x03 | 0x00 - 0xb4 | Mueve Motor 1 |
| 0xff | 0x04 | 0x00 - 0xb4 | Mueve Motor 2 |
| 0xff | 0x05 | 0x00 - 0xb4 | Mueve Motor 3 |
| 0xff | 0x06 | 0x00 - 0xb4 | Mueve Motor 4 |
| 0xff | 0x07 | 0x00 - 0x03 | Mueve Motor 1 |
| 0xff | 0x08 | 0x00 - 0xb4 | Mueve Motor 2 |
| 0xff | 0x09 | 0x00 - 0xb4 | Mueve Motor 3 |
para enviar los comandos usar el programa Coolterm http://freeware.the-meiers.org/
# para el servo motor se debe modificar el ciclo util del PWM SEGUN:
# _ _
# | || |__
# <-width->
# <-------period 20ms--->
# period = 20 ms
# width = 1000us y 2000us
# 1000us para 0 grados
# 2000us para 180 grados
___
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 |
***/
// configuracion comunicación serial
Serial command(USBTX, USBRX);
// seleccion de los pines a usar en la stm32f411re
PwmOut myservo1(PB_4);
PwmOut myservo2(PB_3);
PwmOut myservo3(PB_10);
PwmOut myservo4(PC_7);
scolor_TCS3200 scolor(PA_8, PA_9, PB_6, PA_7, PA_6);
AnalogIn analog_value1(A1);
AnalogIn analog_value2(A2);
AnalogIn analog_value3(A3);
InterruptIn abrir(PA_10);
InterruptIn cerrar(PB_5);
Ticker tk1;
Timer timer2;
#define INITCMD 0xFF
#define INITELE 0xFE
#define DEGREES_MAX 180
#define CMD 0x01
// definición de las variables globales
uint8_t Tipo_Comando; // varable que almacena el numero de seleccion de comando
uint8_t Parametro_1; // varable que almacena el parametro a ejecutar
uint8_t velociraptor =10 ;
uint32_t dpulse=0;
// definición de las funciones
void setup_uart();
void setup_servo();
void leer_datos();
void leer_color();
void servo(uint8_t T_C, uint8_t Par_1);
uint32_t degrees2usec(uint8_t grados);
uint8_t cmd;
void cerrarpinza();
void abrirpinza();
int main() {
setup_servo();
setup_uart();
abrir.mode(PullUp);
cerrar.mode(PullUp);
timer2.start();
abrir.fall(&abrirpinza);
cerrar.fall(&cerrarpinza);
tk1.attach(&leer_color,0.2);
command.printf("inicio de programa");
while(1){
if (timer2.read_ms()>=2000) // read time in ms
{
leer_datos();
servo(Tipo_Comando, Parametro_1);
timer2.reset(); // reset timer
}
}
}
void leer_color() {
long red;
long green;
long blue;
long clear;
red = scolor.ReadRed();
green = scolor.ReadGreen();
blue = scolor.ReadBlue();
clear = scolor.ReadClear();
clear=100/clear ;
red=(10000/red)/clear ;
green=(10000/green)/clear ;
blue=(10000/blue)/clear ;
clear=100*clear/clear ;
//printf("RED: %5d GREEN: %5d BLUE: %5d CLEAR: %5d \n ", red, green, blue, clear);
if (red < 40 && red > 30 && green < 45 && green > 35 && blue < 35 && blue > 23)
{
printf("0xFE 0x01 \n");
//printf("VERDE ");
}
else
{
if (red < 60 && red > 32 && green < 40 && green > 25 && blue < 23 && blue > 10)
{
printf("0xFE 0x02 \n");
//printf("AMARILLO ");
}
else {
if (red < 80 && red > 60 && green < 38 && green > 15 && blue < 28 && blue > 20)
{
printf("0xFE 0x03 \n");
//printf("ROJO ");
}
else{
if (red < 30 && red > 13 && green < 32 && green > 20 && blue < 67 && blue > 32)
{
printf("0xFE 0x04 \n");
//printf("AZUL ");
}
else
{
printf("0xFE 0x00 \n");
//printf("NINGUNO ");
}
}
}
}
}
void cerrarpinza() {
myservo4.pulsewidth_us(1900);
wait(0.1);
}
void abrirpinza(){
myservo4.pulsewidth_us(1550);
wait(0.1);
}
void setup_uart(){
command.baud(115200);
}
void setup_servo(){
myservo1.period_ms(20);
myservo1.pulsewidth_us(1350);
myservo2.period_ms(20);
myservo2.pulsewidth_us(900);
myservo3.period_ms(20);
myservo3.pulsewidth_us(1900);
myservo4.period_ms(20);
myservo4.pulsewidth_us(1550);
}
// funcion leer_datos obtiene los comandos enviados desde cool Terminal
void leer_datos(){
while(command.getc()!= INITCMD);
Tipo_Comando=command.getc();
Parametro_1=command.getc();
}
//Esta funcion convierte de grados a microsengudos, segun el ancho de pulso del servomotor
uint32_t degrees2usec(uint8_t grados){
if(grados <= DEGREES_MAX)
return float(440+grados*17.33/2);// u6
return 440;
}
void servo(uint8_t T_C, uint8_t Par_1){
dpulse=degrees2usec(Par_1);
switch(T_C){
case 1:
if (Par_1==1){//posicion home
myservo3.pulsewidth_us(degrees2usec(180));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(110));
command.printf("posicion home");
}
if (Par_1==2){//posicion material
myservo3.pulsewidth_us(degrees2usec(180));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(90));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(190));
wait(1);
myservo3.pulsewidth_us(degrees2usec(70));
wait(1);
myservo2.pulsewidth_us(degrees2usec(90));
wait(1);
command.printf("pos material");
}
if (Par_1==3){//posicion celda 1
myservo3.pulsewidth_us(degrees2usec(180));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(90));
wait(1);
myservo1.pulsewidth_us(degrees2usec(64));
wait(1);
myservo2.pulsewidth_us(degrees2usec(70));
wait(1);
myservo3.pulsewidth_us(degrees2usec(150));
wait(1);
}
if (Par_1==4){//posicion celda 2
myservo3.pulsewidth_us(degrees2usec(180));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(90));
wait(1);
myservo1.pulsewidth_us(degrees2usec(80));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo3.pulsewidth_us(degrees2usec(150));
wait(1);
}
if (Par_1==5){//posicion celda 3
myservo3.pulsewidth_us(degrees2usec(180));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(90));
wait(1);
myservo1.pulsewidth_us(degrees2usec(140));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo3.pulsewidth_us(degrees2usec(150));
wait(1);
}
if (Par_1==6){//posicion celda 4
myservo3.pulsewidth_us(degrees2usec(180));
wait(1);
myservo2.pulsewidth_us(degrees2usec(50));
wait(1);
myservo1.pulsewidth_us(degrees2usec(90));
wait(1);
myservo1.pulsewidth_us(degrees2usec(162));
wait(1);
myservo2.pulsewidth_us(degrees2usec(70));
wait(1);
myservo3.pulsewidth_us(degrees2usec(150));
wait(1);
}
break;
case 2:
if (Par_1==1){//abrir pinza
myservo4.pulsewidth_us(degrees2usec(100));
}
if (Par_1==2){//cerrar pinza
myservo4.pulsewidth_us(degrees2usec(170));
}
break;
case 3:
myservo1.pulsewidth_us(dpulse);
command.printf("M1");
break;
case 4:
myservo2.pulsewidth_us(dpulse);
command.printf("M2");
break;
case 5:
myservo3.pulsewidth_us(dpulse);
command.printf("M3");
break;
case 6:
if(Par_1>=120)
myservo4.pulsewidth_us(dpulse);
command.printf("M4");
break;
case 7:
if(Par_1==1){//velocida baja
myservo1.period_ms(40);
myservo1.pulsewidth_us(1350);
myservo2.period_ms(40);
myservo2.pulsewidth_us(900);
myservo3.period_ms(40);
myservo3.pulsewidth_us(1900);
myservo4.period_ms(40);
myservo4.pulsewidth_us(1550);
}
else if(Par_1==2){//velocidad media
myservo1.period_ms(20);
myservo1.pulsewidth_us(1350);
myservo2.period_ms(20);
myservo2.pulsewidth_us(900);
myservo3.period_ms(20);
myservo3.pulsewidth_us(1900);
myservo4.period_ms(20);
myservo4.pulsewidth_us(1550);
}
else{ //velocidad alta
myservo1.period_ms(10);
myservo1.pulsewidth_us(1350);
myservo2.period_ms(10);
myservo2.pulsewidth_us(900);
myservo3.period_ms(10);
myservo3.pulsewidth_us(1900);
myservo4.period_ms(10);
myservo4.pulsewidth_us(1550);
}
break;
case 8:
float meas_r1,meas_r2,meas_r3;
float meas_m1,meas_m2, meas_m3;
float grado1, grado2 , grado3;
bool x=0;
command.printf("entro");
while(x==0){
meas_r1 = analog_value1.read(); // Read the analog input value (value from 0.0 to 1.0 = full ADC conversion range)
meas_r2 = analog_value2.read();
meas_r3 = analog_value3.read();
meas_m1 = (meas_r1 * 180) ; // Converts value in the 0V-3.3V range
meas_m2 = (meas_r2 * 180) ;
meas_m3 = (meas_r3 * 180) ;
if(meas_m1<= 180){
grado1=(440+(meas_m1+20)*20.33/2);// u6
myservo1.pulsewidth_us(grado1);}
else{ myservo1.pulsewidth_us(440);}
if(meas_m2>55){
grado2=(440+(meas_m2-40)*20.33/2);// u6
command.printf("%f",grado2);
myservo2.pulsewidth_us(grado2);
}
else{ myservo2.pulsewidth_us(1300);}
if(meas_m3<= 55){
grado3=(440+(meas_m3+30)*20.33/2);// u6
command.printf("%f",grado3);
myservo3.pulsewidth_us(grado3);}
else{ myservo3.pulsewidth_us(1900);}
if(!abrir){myservo4.pulsewidth_us(degrees2usec(120));}
if(!cerrar){myservo4.pulsewidth_us(degrees2usec(170));}
x=command.readable();
wait(0.05); // 1 second
}
break;
}
}
3 comments on Brazo_robotico:
Manual del Usuario
La finalidad de este proyecto es realizar un brazo robótico que pueda organizar cajas o piezas en 4 secciones definidas, además de que también esta la opción de un control manual sobre los 4 motores que el brazo robótico posee.
Para la elaboración de este proyecto se es necesario obtener el diseño del brazo robot DOF 4, se realizan los respectivos cortea en en el material de preferencia para tener las piezas necesarias para poder armar el brazo, le elección de los motores dependerá de la capacidad(Kg) que quiera que tenga su brazo robótico. Ya teniendo las piezas cortadas y seleccionado el tipo de motor se procede a armar el brazo con las piezas anteriormente cortadas y los motores, para el acople de esto se realizó con tornillos (4mm grosor) y usando el video “Tutorial Barzo Robótico 2.0” como guía para el ensamble, el cual esta disponible en
Para poder enviar las instrucciones se utiliza el software Cool Terminal.
Pines de Coneccion
Servomotores
| servo1 | servo2 | servo3 | servo4 |
|---|---|---|---|
| PB_4 | PB_3 | PB_10 | PC_7 |
Joystick
| joystick1_X | joystick1_Y | joystick2_X | joystick2_Y | joystick1_SW | joystick2_SW |
|---|---|---|---|---|---|
| A1 | A2 | A3 | NC | PA_10 | PB_5 |
Sensor de color
| S0 | S1 | S2 | S3 | S_IN |
|---|---|---|---|---|
| PA_8 | PA_9 | PB_6 | PA_7 | PA_6 |
Sensor Color
Se definen las librerías tanto de los comandos básicos como el del sensor de color; en el caso de las librerías creadas para el sensor de color, estas ya tienen unas funciones predefinidas tal como el uso de los pines que van conectados al sensor de color:
S0 y S1 son la escala de la medición de la frecuencia, en este caso usamos la escala al 100% donde S1 y S0 están en estado lógico 1 para la configuración de medición.
S2 y S3 es el tipo de color a medir:
S2 S3 Tipo de Photodiodo
0 0 ROJO
0 1 AZUL
1 0 CLEAR (sin filtro)
1 1 VERDE
Una vez entendido esto, dentro de la librería “scolor_TCS3200.h” también se realiza el proceso de la medición del valor de tiempo Tao (T); haciendo una medición del tiempo de ciclo útil; dicho proceso se repite en cada una de las configuraciones del sensor.
Se necesita el valor de periodo para sacar el valor del porcentaje RGB (RED-GREEN-BLUE) donde el periodo es :
P = 1 / T
Luego se realiza una regla de 3, sabiendo que el valor del periodo del clear es el mas alto de todos (la sumatoria de los periodos de los otros valores), entonces la formula para obtener el valor del porcentaje RGB se da por:
% COLOR= (PERIODO COLOR x 100)/(PERIODO CLEAR)
Servomotores
En el caso de los servomotores, estos giran con base al tiempo del ciclo útil, en el programa se introduce el valor en milisegundos según el ángulo en el que se desee que cada motor quede.
Main
Ya dentro de la rutina principal(main) , una subrutina inicial (antes del while(1) ) que da un valor inicial al periodo de los motores, en otras palabras disminuye o agranda el ciclo útil, lo cual deja predefinida una velocidad de los motores; además envía valores en milisegundos para dejar los motores en la posición inicial home.
Existe otra subrutina inicial “setup_uart()” la cual tiene como finalidad asignar la velocidad de transmisión de información entre el sistema embebido y el programa del computador.
Entrando en el “while(1)”, que es un ciclo que se repite indefinidamente, primero se entra a una subrutina “leer_datos()” la cual tiene un sistema de seguridad con while; ya que si el parámetro es diferente de 0xff el programa no hara absolutamente nada; en caso de que el valor del parámetro sea correcto lo que hace es pedir los otros comandos : Tipo Comando y Parametro.
Luego entra en la subrutina ” servo” se realizan diferentes acciones según los comandos que se le envíen:
Desde el tipo de comando 0x01 hasta el 0x02 se pone un valor pre establecido a los motores para generar un movimiento.
Desde el tipo de comando 0x03 hasta el 0x06 que a diferencia del anterior es para que el usuario le asigne un valor de giro a los motores, para ello se usa la formula de la pendiente, sabiendo que 1ms es el valor de 0° , y que 2ms es el valor de 180° ; quedando la fórmula de tal manera:
VALOR EN MILISEGUNDOS = 440+(grados *17.33/2)
Entonces la anterior formula pasa el valor ingresado en grados a milisegundos, para que el servomotor escogido tenga dicho ciclo útil.
En los comandos 0x07 se varia la velocidad de los motores, por lo que se varia con el periodo de los motores para asignarle su respectiva velocidad.
En tc =0x02 y Parametro1 =0x01 ; lo que se hace es usar dos joystick, los cuales son una serie de potenciómetros, el valor analógico recibido esta entre 1 y 0 , además que el puerto analógico del embebido soporta hasta 3.3v, dicho voltaje es el usado en este caso. Deben estar configurados de tal manera que arriba y derecha son los máximos valores en grados, mientras que abajo e izquierda son los menores valores en grados.
Se vuelve a aplicar las formulas de la pendiente, donde la formula de transformación a milisegundos queda de la siguiente manera:
VALOR EN MILISEGUNDOS=( 440+ ( ( (VALOR MEDIDO * 180)+20)* 20.33) /2)
Finalmente en tc=0x09 se activa el sensor de color, cuyo funcionamiento y programación ya fueron explicados al principio del programa. En este, es el único caso donde en la interfaz grafica se debe imprimir un valor, según el color leído:
ROJO=0x01
AZUL=0x02
VERDE=0x03
AMARILLO (adicional)= 0x04
DIFERENTE A LOS ANTERIORES = 0X00
Quote:
Manual Tecnico
VOLTAJES DE OPERACION
| TARJETA DE DESARROLLO F411RE | SENSOR DE COLOR TCS320 | SERVOMOTOR SG90 | JOYSTICK |
|---|---|---|---|
| 3.6V | 3.3V | 5V | 3.3V |
La terjeta de desarrollo F411RE se alimenta desde el puerto usb del computador, el sensor TCS320 de color se alimenta desde la tarjeta de desarrollo, los 4 servomotores se conectan a una fuente externa de 5V a 2A, los joysticks se alimenta de la tarjeta de desarrollo a la salidad de 3.3V
CODIGO
setup_servo()
Configura el periodo y el ancho de pulso de los servomotores.
leer_datos()
Lee los datos enviados por serial.
leer_color()
Esta funcion es la encargada de leer los colores.
servo(T_C, Par_1)
Esta funcion esta compuesta por
degrees2usec(grados)
Convierte un valor expresado en grados a un valor en tiempo para el movimiento de los servomotores.
FALLOS COMUNES
Mal posicionamiento usando joystik
Puede deberse a la mala alimentacion de los joystick,la lectura de los puertos analogos de la tarjeta de desarrollo F411RE tiene como referencia 3.3V a si que si se alimentan lso joysticks con un valor diferente al mecionado anteriormente llevara a un mal funcionamiento.
Rebotes en los joystick
Debido a que aparentemente si se preciona el boton del joystick el conmuta inmediatamente, pero en realida el pulsador no hace un contacto inmediato, causando que la pinza se abra o cierre sola.
Mala lectura del color
Las mediciones realizadas sobre el sensor de color fueron tomadas bajo un ambiente de poca luminosidad, asi que las lecturas pueden variar segun las condiciones en las que se encuentre el sensor.
Elevada temperatura de servomotor
suelen recalentarse por un mal uso, excediendo la carga maxima de estos.
RECOMENDACIONES
- Realizar un constante ajustes de la tornilleria del brazo.
- Revisar las conexiones realizadas antes de encender el brazo, podria estar conectando mal y esto llegaria a generar un daño irreparable.
- Realizar constantes inspecciones visuales para constatar el funcionamiento correcto del brazo
Please log in to post comments.
Diagrama de Flujo