Entrega segundo corte

Dependencies:   mbed

Brayan Paiba Diego Lopez Jonathan Perez Luis Herrera

Corte # 2 Robot Piccolo dirigido por comunicación Serial

El Robot Piccolo ejecuta unas instrucciones determinadas emitidas por el usuario mediante una comunicación serial.

Materiales

/media/uploads/luischs/22712688_1603771992978110_1295872255_o.jpg

Tarjeta nucleo STM32F446

Estructura del Piccolo:

· 3 servomotores

· Jumpers

· Protoboard

Software para la comunicación serial "coolterm"

/media/uploads/luischs/22766502_1603776982977611_485358740_o.jpg/media/uploads/luischs/asyco67.jpg

Marco teorico

Comunicación serial

Los puertos serie requieren un solo conector, o cable, para enviar el mismo carácter de datos al dispositivo. Para conseguirlo, los datos se convierten del formato paralelo (enviado por el sistema), a formato secuencial, en el que los bits se organizan uno tras otro en una serie. Los datos se transmiten entonces al dispositivo, enviando el bit menos significativo (o el bit cero) en primer lugar. Una vez que el dispositivo remoto recibe los datos, éstos se vuelven a convertir al formato paralelo.

/media/uploads/luischs/asyco65.jpg

Las transmisiones serie de un solo carácter son simples y directas; sin embargo, surgen complicaciones cuando se transmiten en series un gran número de caracteres tal como se muestra en la ilustración siguiente. El sistema receptor no sabe cuándo finaliza un carácter y empieza el otro. Para solucionar este problema, ambos extremos del enlace de comunicación deben estar sincronizados o temporizados.

/media/uploads/luischs/asyco67.jpg

/media/uploads/luischs/22712817_1603771919644784_767445203_o.jpg

La comunicación serial es un protocolo muy común (no hay que confundirlo con el Bus Serial de Comunicación, o USB) para comunicación entre dispositivos que se incluye de manera estándar en prácticamente cualquier computadora.

Se establecieron los siguientes telecomandos para nuestra comunicación serial:

<00 #X #Y #Z> --> Punto (xyz)

<01 #Xi #Yi #Xf #Yf> --> Linea

<02 #Xi #Yi #L> ---> Cuadrado

<03 #X #Y #Rad> --> Circulo

<04> --> Home

<05 #Rev> --> Resolución

<06 # T(ms)> --> Tiempo Pasos

<07> --> Stop

<08> --> Pausa

<09> --> Reanudar

<10 #Cu> --> Mover

  1. include "mbed.h"
  2. include "InterruptIn.h" Serial command(USBTX, USBRX); DigitalOut led(LED1); DigitalIn button(PC_13); InterruptIn mybutton(USER_BUTTON); InterruptIn K();
  3. define DEBUG 1 PwmOut myServoX(PB_3); PwmOut myServoY(PB_5); PwmOut myServoZ(PB_4); Con el comando de COMANDO MOVER MOTOR |POS 1|POS 2|POS 3|POS 4| POS 5| | < | #C | a | b | > | | 3C |0 al A| D1 | D2 | D3 | D4 | 3E | #C -> indica el comando. En este caso es de 0 a 10 (en hexadesimal para enviar comando desde techado de 0 a A) a,b,c,d parametros del comando <,> -> inicio, y fin de comando el inicio de comando no se almacena en el buffer *****************

VARIABLES PARA DEFINIR EL COMMANDO

  1. define BUFF_SIZE 6
  2. define COMM_N 0
  3. define INITPARAMETER 1
  4. define MAXPOS 50
  5. define POSDRAW 00
  6. define SS_TIME 500
  7. define PI 3.1415926 int RSTEP = 5; en milimetros

COMANDOS

  1. define LED_NC 0 ok
  2. define DOT_NC 1 ok
  3. define LINE_NC 2 ok
  4. define RECTANGLE_NC 3 ok
  5. define CICLE_NC 4 ok
  6. define HOME_NC 5 ok
  7. define RESOLUCION_NC 6 ok
  8. define TIEMPOPASOS_NC 10
  9. define STOP_NC 8
  10. define PAUSA_NC 9
  11. define REANUDAR_NC 7
  12. define MOVER_CN 11

int coord2pulsex(float coord) { if(0 <= coord <= MAXPOS) return int(800+coord*1850/50); u6 return 750;

} int coord2pulsey(float coord) { if(0 <= coord <= MAXPOS) return int(750+coord*1400/42); u6 return 750;

} void vertex2d(float x, float y){

int pulseX = coord2pulsex(x); int pulseY = coord2pulsey(y);

myServoX.pulsewidth_us(pulseX); myServoY.pulsewidth_us(pulseY+8); }

uint8_t posx_old=0; posición anterior del eje X uint8_t posy_old=0; posición anterior del eje Y uint8_t ss_time=100; tiempo de espera para moverse 1 mm en microsegundos

void put_sstime(uint8_t vtime){ ss_time=vtime; } void sstime(uint8_t x, uint8_t y) { double dx=abs(x-posx_old); double dy=abs(y-posy_old); double dist= sqrt(dx*dx+dy*dy); wait_ms((int)(ss_time*dist)); posx_old =x; posy_old=y;

}

void draw(){ myServoZ.pulsewidth_ms(2); wait_ms(ss_time*2); } void nodraw(){ myServoZ.pulsewidth_ms(1); wait_ms(ss_time*2); }

uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};

void print_num(uint8_t val)

{ if (val <10) command.putc(val+0x30); else command.putc(val-9+0x40);

} void print_bin2hex (uint8_t val) { command.printf(" 0x"); print_num(val>>4); print_num(val&0x0f); }

TODO : TIMEOUT UART SERIAL void Read_command() { for (uint8_t i=0; i<BUFF_SIZE;i++) buffer_command[i]=command.getc(); }

void echo_command() { for (uint8_t i=0; i<BUFF_SIZE;i++) print_bin2hex(buffer_command[i]); }

uint8_t check_command() { if (buffer_command[BUFF_SIZE-1]== '>'){

  1. if DEBUG command.printf("\nComando:"); print_bin2hex(buffer_command[COMM_N]); command.printf(" -> ");
  2. endif return 1; }
  3. if DEBUG command.printf("\n ERROR COMANDO -> "); echo_command();
  4. endif return 0;

} void resolucion(int res){ RSTEP = res;

  1. if DEBUG command.printf("Resolucion definida en =%i \n", RSTEP);
  2. endif } void command_led(int tm){ EJEMPLO DE COMANDO led=1; wait(tm); led=0; } Con el void punto lo que empezamos a realizar es el proceso de dibujado enviando las coordenadas en la cuales queremos que dibuje el punto void punto(uint8_t x, uint8_t y){ vertex2d(x,y); draw(); wait(1); nodraw();
  3. if DEBUG command.printf("Coord x= %i, coord y=%i \n",x,y);
  4. endif }

con el void linea realizamos el proceso de dibujado de la "linea " enviando 2 coordenadas una final y una inicial donde el piccolo inicia en un punto Xi y va A Xf calculando una pendiente y así dándonos la inclinación que tiene la linea

void linea(float xi, float yi, float xf, float yf) {

  1. if DEBUG command.printf("linea Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf);
  2. endif

float xp; float yp; float m= (yf-yi)/(xf-xi); float b=yf-m*xf; vertex2d(xi,yi); wait(0.5); draw(); for (xp =xi;xp<=xf;xp++) { yp =m*xp+b; vertex2d(xp,yp); } } con el void rectángulo enviamos 2 coordenadas una final y una inicial donde el piccolo inicia en un punto Xi lo replica a Yi y va A Xf pasando a Yf dibujándonos así el cuadrado linea a linea entrando en un bucle el cual vuelve y lo realiza void rectangle(float x, float y, float width,float height) {

  1. if DEBUG command.printf("Rectangulo x= %f,y= %f,Ancho= %f,Alto= %f \n", x,y,width,height);
  2. endif

vertex2d(x,y); draw(); wait(0.5);

linea(x,y,x+width,y); linea(x+width,y,x+width,y+height); linea(x+width,y+height,x,y+height); linea(x,y+height,x,y); wait(1); nodraw();

}

void home(){ vertex2d(0,0); }

con el void circulo introducimos un vértice, dandole a X y a Y un valor el cual el programa lo remplaza en una ecuacion determinando las posiciones del radio y demás valores necesarios para realizar el circulo void circle(float cx, float cy, float radio) { int y; int x; vertex2d(cx,cy);

for(double i=0; i<=90 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);

  1. if DEBUG command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180));
  2. endif wait_ms(50); } for(double i=90; i<=180 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  3. if DEBUG command.printf("position x =%li ; position y =%li \n",x,y);
  4. endif wait_ms(50); } for(double i=180; i<=270 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  5. if DEBUG command.printf("position x =%li ; position y =%li\n",x,y);
  6. endif wait_ms(50); }

for(double i=270; i<=360 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);

  1. if DEBUG command.printf("position x =%li , position y =%li \n",x,y);
  2. endif wait_ms(50); } nodraw(); }

void command_rstepservo(){

  1. if DEBUG command.printf("config rstepservo: ");
  2. endif } void command_sstime(){
  3. if DEBUG command.printf("config tiempo entre movimientos de los servos: "); command.printf("SSTIME = %i\n", buffer_command[1]);
  4. endif put_sstime(buffer_command[1]); } void command_stop(){
  5. if DEBUG command.printf("stop");
  6. endif } Se llama la función Pausa. Realizada por interrupciones void command_pausa(){
  7. if DEBUG command.printf("piccolo en pausa ");
  8. endif while(1){ wait(1); } }

void command_reanudar(int on){

  1. if DEBUG command.printf("reanuda piccolo ");
  2. endif

} void command_stepmotor(){

  1. if DEBUG command.printf("comando mover piccolo: ");
  2. endif }

void command_exe() {

  1. if DEBUG command.printf("Ejecutando comando: ");
  2. endif

switch (buffer_command[COMM_N]){

case (LED_NC):

  1. if DEBUG command.printf("led on 7 off\n");
  2. endif command_led(buffer_command[INITPARAMETER]); break;

case (DOT_NC):

  1. if DEBUG command.printf("Punto \n");
  2. endif punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]); break;

case (LINE_NC):

  1. if DEBUG command.printf("draw line\n");
  2. endif linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);

break;

case (RECTANGLE_NC):

  1. if DEBUG command.printf("cuadrado \n");
  2. endif rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);

break; case (HOME_NC):

  1. if DEBUG command.printf("Yendo a Home \n");
  2. endif home(); break; case (RESOLUCION_NC): resolucion(buffer_command[INITPARAMETER]);
  3. if DEBUG command.printf("Resolucion definida");
  4. endif break; case (CICLE_NC): circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
  5. if DEBUG command.printf("Circulo\n");
  6. endif break; case(REANUDAR_NC): command_reanudar(buffer_command[INITPARAMETER]);
  7. if DEBUG command.printf("Reanudar\n");
  8. endif break; default:
  1. if DEBUG command.printf("comando no encontrado\n");
  2. endif } }

int main() {

  1. if DEBUG command.printf("inicio con debug\n");
  2. else command.printf("inicio sin debug\n");
  3. endif uint8_t val; mybutton.fall(&command_pausa); while(1){ val=command.getc(); if (val== '<'){ Read_command(); if (check_command()){ command_exe();
  4. if DEBUG echo_command();
  5. endif } } else command.printf("error inicio trama: "); command.putc(val); } } include "mbed.h"
  6. include "InterruptIn.h" Serial command(USBTX, USBRX); DigitalOut led(LED1); DigitalIn button(PC_13); InterruptIn mybutton(USER_BUTTON); InterruptIn K();
  7. define DEBUG 1 PwmOut myServoX(PB_3); PwmOut myServoY(PB_5); PwmOut myServoZ(PB_4); *************** COMANDO MOVER MOTOR |POS 1|POS 2|POS 3|POS 4| POS 5| | < | #C | a | b | > | | 3C |0 al A| D1 | D2 | D3 | D4 | 3E | #C -> indica el comando. En este caso es de 0 a 10 (en hexadesimal para enviar comando desde techado de 0 a A) a,b,c,d parametros del comando <,> -> inicio, y fin de comando el inicio de comando no se almacena en el buffer ***************

VARIABLES PARA DEFINIR EL COMMANDO

  1. define BUFF_SIZE 6
  2. define COMM_N 0
  3. define INITPARAMETER 1
  4. define MAXPOS 50
  5. define POSDRAW 00
  6. define SS_TIME 500
  7. define PI 3.1415926 int RSTEP = 5; en milimetros

COMANDOS

  1. define LED_NC 0 ok
  2. define DOT_NC 1 ok
  3. define LINE_NC 2 ok
  4. define RECTANGLE_NC 3 ok
  5. define CICLE_NC 4 ok
  6. define HOME_NC 5 ok
  7. define RESOLUCION_NC 6 ok
  8. define TIEMPOPASOS_NC 10
  9. define STOP_NC 8
  10. define PAUSA_NC 9
  11. define REANUDAR_NC 7
  12. define MOVER_CN 11

int coord2pulsex(float coord) { if(0 <= coord <= MAXPOS) return int(800+coord*1850/50); u6 return 750;

} int coord2pulsey(float coord) { if(0 <= coord <= MAXPOS) return int(750+coord*1400/42); u6 return 750;

} void vertex2d(float x, float y){

int pulseX = coord2pulsex(x); int pulseY = coord2pulsey(y);

myServoX.pulsewidth_us(pulseX); myServoY.pulsewidth_us(pulseY+8); }

uint8_t posx_old=0; posición anterior del eje X uint8_t posy_old=0; posición anterior del eje Y uint8_t ss_time=100; tiempo de espera para moverse 1 mm en microsegundos

void put_sstime(uint8_t vtime){ ss_time=vtime; } void sstime(uint8_t x, uint8_t y) { double dx=abs(x-posx_old); double dy=abs(y-posy_old); double dist= sqrt(dx*dx+dy*dy); wait_ms((int)(ss_time*dist)); posx_old =x; posy_old=y;

}

void draw(){ myServoZ.pulsewidth_ms(2); wait_ms(ss_time*2); } void nodraw(){ myServoZ.pulsewidth_ms(1); wait_ms(ss_time*2); }

uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};

void print_num(uint8_t val)

{ if (val <10) command.putc(val+0x30); else command.putc(val-9+0x40);

} void print_bin2hex (uint8_t val) { command.printf(" 0x"); print_num(val>>4); print_num(val&0x0f); }

TODO : TIMEOUT UART SERIAL void Read_command() { for (uint8_t i=0; i<BUFF_SIZE;i++) buffer_command[i]=command.getc(); }

void echo_command() { for (uint8_t i=0; i<BUFF_SIZE;i++) print_bin2hex(buffer_command[i]); }

uint8_t check_command() { if (buffer_command[BUFF_SIZE-1]== '>'){

  1. if DEBUG command.printf("\nComando:"); print_bin2hex(buffer_command[COMM_N]); command.printf(" -> ");
  2. endif return 1; }
  3. if DEBUG command.printf("\n ERROR COMANDO -> "); echo_command();
  4. endif return 0;

} void resolucion(int res){ RSTEP = res;

  1. if DEBUG command.printf("Resolucion definida en =%i \n", RSTEP);
  2. endif } void command_led(int tm){ EJEMPLO DE COMANDO led=1; wait(tm); led=0; }

void punto(uint8_t x, uint8_t y){ vertex2d(x,y); draw(); wait(1); nodraw();

  1. if DEBUG command.printf("Coord x= %i, coord y=%i \n",x,y);
  2. endif }

void linea(float xi, float yi, float xf, float yf) {

  1. if DEBUG command.printf("linea Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf);
  2. endif

float xp; float yp; float m= (yf-yi)/(xf-xi); float b=yf-m*xf; vertex2d(xi,yi); wait(0.5); draw(); for (xp =xi;xp<=xf;xp++) { yp =m*xp+b; vertex2d(xp,yp); } }

void rectangle(float x, float y, float width,float height) {

  1. if DEBUG command.printf("Rectangulo x= %f,y= %f,Ancho= %f,Alto= %f \n", x,y,width,height);
  2. endif

vertex2d(x,y); draw(); wait(0.5);

linea(x,y,x+width,y); linea(x+width,y,x+width,y+height); linea(x+width,y+height,x,y+height); linea(x,y+height,x,y); wait(1); nodraw();

}

void home(){ vertex2d(0,0); }

void circle(float cx, float cy, float radio) { int y; int x; vertex2d(cx,cy);

for(double i=0; i<=90 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);

  1. if DEBUG command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180));
  2. endif wait_ms(50); } for(double i=90; i<=180 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  3. if DEBUG command.printf("position x =%li ; position y =%li \n",x,y);
  4. endif wait_ms(50); } for(double i=180; i<=270 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  5. if DEBUG command.printf("position x =%li ; position y =%li\n",x,y);
  6. endif wait_ms(50); }

for(double i=270; i<=360 ;i=i+5) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);

  1. if DEBUG command.printf("position x =%li , position y =%li \n",x,y);
  2. endif wait_ms(50); } nodraw(); }

void command_rstepservo(){

  1. if DEBUG command.printf("config rstepservo: ");
  2. endif } void command_sstime(){
  3. if DEBUG command.printf("config tiempo entre movimientos de los servos: "); command.printf("SSTIME = %i\n", buffer_command[1]);
  4. endif put_sstime(buffer_command[1]); } void command_stop(){
  5. if DEBUG command.printf("stop");
  6. endif } Se envia el pausa directamente desde hardware, oprimiendo el botón en la tarjeta. void command_pausa(){
  7. if DEBUG command.printf("piccolo en pausa ");
  8. endif while(1){ wait(1); } }

void command_reanudar(int on){

  1. if DEBUG command.printf("reanuda piccolo ");
  2. endif

} void command_stepmotor(){

  1. if DEBUG command.printf("comando mover piccolo: ");
  2. endif }

void command_exe() {

  1. if DEBUG command.printf("Ejecutando comando: ");
  2. endif

switch (buffer_command[COMM_N]){

case (LED_NC):

  1. if DEBUG command.printf("led on 7 off\n");
  2. endif command_led(buffer_command[INITPARAMETER]); break;

case (DOT_NC):

  1. if DEBUG command.printf("Punto \n");
  2. endif punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]); break;

case (LINE_NC):

  1. if DEBUG command.printf("draw line\n");
  2. endif linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);

break;

case (RECTANGLE_NC):

  1. if DEBUG command.printf("cuadrado \n");
  2. endif rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);

break; case (HOME_NC):

  1. if DEBUG command.printf("Yendo a Home \n");
  2. endif home(); break; case (RESOLUCION_NC): resolucion(buffer_command[INITPARAMETER]);
  3. if DEBUG command.printf("Resolucion definida");
  4. endif break; case (CICLE_NC): circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
  5. if DEBUG command.printf("Circulo\n");
  6. endif break; case(REANUDAR_NC): command_reanudar(buffer_command[INITPARAMETER]);
  7. if DEBUG command.printf("Reanudar\n");
  8. endif break; default:
  1. if DEBUG command.printf("comando no encontrado\n");
  2. endif } }

int main() {

  1. if DEBUG command.printf("inicio con debug\n");
  2. else command.printf("inicio sin debug\n");
  3. endif uint8_t val; mybutton.fall(&command_pausa); while(1){ val=command.getc(); if (val== '<'){ Read_command(); if (check_command()){ command_exe();
  4. if DEBUG echo_command();
  5. endif } } else command.printf("error inicio trama: "); command.putc(val); } }

Conclusiones Se evidencian fallas en algunos telecomandos, precisamente en el pausa, se desarrollo por medio de interrupciones.

Changes

RevisionDateWhoCommit message
0:cd988b084a05 2017-10-21 luischs Interrupciones default tip