Piccolo


Piccolo 2do corte wiki

*****LIBRERIAS*******/

  1. include "mbed.h"
  2. define DEBUG 1

/**PUERTOS I/O DE SISTEMA EMBEBIDO NUCLEO-F411RE***/

Serial command(USBTX, USBRX); /* Para habilitar comunicación serial */ DigitalOut led(LED1); PwmOut myServoX(PB_3); Servo X /* definicion pin 3 de PWM para "x" */ PwmOut myServoY(PB_4); Servo Y /* definicion pin 4 de PWM para "y" */ PwmOut myServoZ(PC_7); Servo Z /* definicion pin 3 de PWM para "z" */

*************** 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) a,b,c,d parametros del comando <,> -> inicio, y fin de comando el inicio de comando no se almacena en el buffer *****************

*****VARIABLES DEFINIDAS*****/

  1. define BUFF_SIZE 6 tamaño del comando enviado desde CoolTerm
  2. define COMM_N 0
  3. define INITPARAMETER 1 Donde inicia el parametro
  4. define MAXPOS 50 Posicion maxima de la matriz de dibujo x,y en mm
  5. define POSDRAW 30 Posicion del servomotor Z para dibujar #define SS_TIME 100 no puedo dejarla declarada constante, pues se debe modificar por teclado
  6. define PI 3.1415926 uint8_t RSTEP = 5; Ini. variable de Resolucion para el dibujo uint8_t DET=1; uint8_t posx_old=0; posición anterior del eje X uint8_t posy_old=0; posición anterior del eje Y uint8_t SSTIME = 100;

/******COMANDOS DE TECLADO*****/

  1. define LED_NC 0 ok
  2. define DOT_NC 1 ok
  3. define LINE_NC 2 ok
  4. define RECTANGULO_NC 3 ok
  5. define CIRCLE_NC 4 ok
  6. define HOME_NC 5 ok
  7. define RESOLUCION_NC 6 ok
  8. define TIEMPOPASOS_NC 7 No se a que se refiere.
  9. define STOP_NC 8 Pendiente
  10. define PAUSA_NC 9 Pendiente
  11. define REANUDAR_NC 10 Pendiente #define MOVER_NC 11 Se refiere a el movimiento del motor paso a paso

/**FUNCIONES PARA MOVER LOS SERVOS X, Y, Z*****/

int coord2pulse(float coord) Mapeo Servomotor { if(0 <= coord <= MAXPOS) return int(750+coord*1900/50); u6 return 750; }

void vertex2d(float x, float y) Funcion para enviarle la posicion a (x,y) { wait_ms(SSTIME); int pulseY = coord2pulse(y); int pulseX = coord2pulse(x);

myServoY.pulsewidth_us(pulseY); myServoX.pulsewidth_us(pulseX);

}

void draw() Funcion para enviarle la posicion de dibujo a z. { wait_ms(SSTIME*10); int pulseZ=coord2pulse(POSDRAW); myServoZ.pulsewidth_us(pulseZ); wait_ms(SSTIME); } void nodraw() Funcion para enviarle la posicion de no dibujar a z. { wait_ms(SSTIME*10); int pulseZ=coord2pulse(0); myServoZ.pulsewidth_us(pulseZ);

}

void initdraw(float x, float y)para ubicacion y que de aqui inicie a dibujar { vertex2d(x,y); wait_ms(SSTIME); draw(); }

/***FUNCIONES PARA OBTENER E IMPRIMIR EL COMANDO***/

uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0}; Matriz del Comando enviado

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) Imprimir el comando enviado en Hexadecimal { command.printf(" 0x"); print_num(val>>4); print_num(val&0x0f); }

void Read_command() Leer el comando que se digito en CoolTerm { for (uint8_t i=0; i<BUFF_SIZE;i++) buffer_command[i]=command.getc(); }

void echo_command() Imprimir comando que le enviamos { for (uint8_t i=0; i<BUFF_SIZE;i++) print_bin2hex(buffer_command[i]); }

uint8_t check_command() Verifica el ultimo valor del comando enviado '>' { 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 imprime cuando se realiza la funcion, los resultados obtenidos de cuando ejecuto dicha funcion command.printf("\n !!!!!!!!ERROR EN EL COMANDO!!!!!!!!!!!!!! -> "); echo_command();
  4. endif return 0; }

/****FUNCIONES PARA DIBUJAR EN EL PICOLO****/

void Led(int tm) Funcion para definir el tiempo de led en milisegundos {

  1. if DEBUG command.printf("\nTiempo led: %i seg\n", tm);
  2. endif led=1; wait(tm); led=0; }

void punto(uint8_t x, uint8_t y) Funcion para dibujar un punto {

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

void linea(float xi, float yi, float xf, float yf) funcion para realizar linea {

  1. if DEBUG command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP);
  2. endif float xp,yp; float m=(yf-yi)/(xf-xi); float b=yf-(m*xf);
  3. if DEBUG command.printf("\n b =%f, m=%f \n", b,m);
  4. endif float nstep =(m/RSTEP); nstep=RSTEP;
  5. if DEBUG command.printf("\nstep = %f \n", nstep);
  6. endif if ((abs(xf-xi))>abs(yf-yi)){ if (xf>xi){ initdraw(xp,yp); for (xp=xi; xp<=xf; xp+=RSTEP){ yp =m*xp+b; vertex2d(xp,yp);
  7. if DEBUG command.printf(" CASO 1: ( dx>dy & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);Define que la resolucion la va a realizar respecto a "x" cuando nuestro dx sea mayor a dy y nuestro xf sea mayor a nuestro xi
  8. endif } } else{ float temp = xi; xi = xf; xf = temp; initdraw(xp,yp); for (xp=xi; xp<=xf; xp+=RSTEP){ yp =m*xp+b; vertex2d(xp,yp);
  9. if DEBUG command.printf(" CASO 2: ( dx>dy & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp); este caso nos define que la resolucion va a ser respecto a "x" pero que diferente al anterior caso nuestra medida de xi va a ser menor que xf, entonces, nos pintaria de xf a xi pero entonces, nos pintara de xi a xf
  10. endif }}} else { if (yf>yi){ initdraw(xp,yp); for (yp=yi; yp<=yf; yp+=RSTEP){ xp=(yp-b)/m; vertex2d(xp,yp);
  11. if DEBUG command.printf(" CASO 3: ( dy>dx & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp); la resolucion se va a hacer en sentido de "y" y cuando nuestro xf sea mayor a nuestro xi, va a dibujar desde xi hasta xf
  12. endif }} else{ float tempo = yi; yi = yf; yf = tempo; initdraw(xp,yp); for (yp=yi; yp<=yf; yp+=RSTEP){ xp=(yp-b)/m; vertex2d(xp,yp);
  13. if DEBUG command.printf(" CASO 4: ( dy>dx & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp); La resolucion la va a tomar en "y" y nos dibujara de xi a xf
  14. endif } } } nodraw(); }

void Rectangulo(uint8_t x, uint8_t y, uint8_t a, uint8_t h) Funcion rectangulo y decimos que nuestras variables ocupen 8 bits (con definir uint8_t) cada una, para que consuma menos memoria en el procesador {

  1. if DEBUG command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP);
  2. endif initdraw(x,y);

for(uint8_t xi=x; xi<=(x+a); xi+=RSTEP){ vertex2d(xi,y);

  1. if DEBUG command.printf("Coordenadas x=%i,y=%i\n", xi,y);
  2. endif } for (uint8_t yi=y; yi<=(y+h); yi+=RSTEP){ vertex2d(x+a,yi);
  3. if DEBUG command.printf("Coordenadas x=%i,y=%i\n", x+a,yi);
  4. endif } for( uint8_t xf=(x+a); xf>=x; xf-=RSTEP){ vertex2d(xf,y+h);
  5. if DEBUG command.printf("Coordenadas x=%i,y=%i\n", xf,y+h);
  6. endif } for ( uint8_t yf=(y+h); yf>=y; yf-=RSTEP){ vertex2d(x,yf);
  7. if DEBUG command.printf("Coordenadas x=%i,y=%i\n", x,yf);
  8. endif } nodraw(); }

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

  1. if DEBUG command.printf("\nCoordenadas xc =%i, yc =%i, Radio=%i \n",cx,cy,radio);
  2. endif

for(double i=0; i<=PI/2 ;i+=((PI/2)/RSTEP)) { x=radio*cos(i); y=radio*sin(i); initdraw(x+cx,y+cy);

  1. if DEBUG command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
  2. endif }
  3. if DEBUG command.printf("\n");
  4. endif for(double i=PI/2; i<=PI ;i+=((PI/2)/RSTEP)) { x=radio*cos(i); y=radio*sin(i); vertex2d(x+cx,y+cy);
  5. if DEBUG command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio, ((PI/2)/RSTEP));
  6. endif }
  7. if DEBUG command.printf("\n");
  8. endif for(double i=PI; i<=((3*PI)/2) ;i+=((PI/2)/RSTEP)) { x=radio*cos(i); y=radio*sin(i); initdraw(x+cx,y+cy);
  9. if DEBUG command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
  10. endif }
  11. if DEBUG command.printf("\n");
  12. endif for(double i=((3*PI)/2); i<=(2*PI) ;i+=((PI/2)/RSTEP)) { x=radio*cos(i); y=radio*sin(i); initdraw(x+cx,y+cy);
  13. if DEBUG command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
  14. endif } nodraw(); }

void home() nos lleva a cero grados en nuestros servomotores { nodraw(); vertex2d(0,0);

  1. if DEBUG command.printf("\nCoordenada HOME x=0, y =0");
  2. endif }

void resolucion(int res) Funcion para definir la resolucion del dibujo, cantidad de lineas que le toma para dibujar nuestra funcion { RSTEP = res;

  1. if DEBUG command.printf("\nResolucion definida en=%i \n", RSTEP);
  2. endif }

void TiempoPasos(int SST) Funcion para definir el tiempo de led { SSTIME=SST;

  1. if DEBUG command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
  2. endif } 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)(SSTIME*dist)); posx_old =x; posy_old=y;

} void Stop(int ST) Funcion para detener el programa { if(ST == 0){ home();

  1. if DEBUG command.printf("\n...Parada total...\n", ST);
  2. endif exit(ST); } }

void Pausa(int det) Funcion para definir el tiempo de led {

system("PAUSE()");

if(det == 0){ DET=det;

  1. if DEBUG command.printf("\n...Pausado...\n", DET);
  2. endif

}

void Reanudar(int det) Funcion para definir el tiempo de led { system("CLS()"); if(det == 1){ DET=det;

  1. if DEBUG command.printf("\nReanudando imagen...%i/n", DET);
  2. endif } }

/*void Mover(int tm) Funcion para definir el tiempo de led {

  1. if DEBUG command.printf("\nMover a: %i/n", tm);
  2. endif }
  • /

/***** CASOS DE USO PARA SELECCION DE COMANDOS***/

void command_exe() { switch (buffer_command[COMM_N]){

case (LED_NC):

  1. if DEBUG command.printf(" LED ON/OFF\n");
  2. endif 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(" LINEA\n");
  2. endif linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]); break;

case (RECTANGULO_NC):

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

case (CIRCLE_NC):

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

case (HOME_NC):

  1. if DEBUG command.printf(" HOME\n");
  2. endif home(); break;

case (RESOLUCION_NC):

  1. if DEBUG command.printf(" RESOLUCION\n");
  2. endif resolucion(buffer_command[INITPARAMETER]); break;

case (TIEMPOPASOS_NC):

  1. if DEBUG command.printf(" TIEMPO EN PASOS\n");
  2. endif TiempoPasos(buffer_command[INITPARAMETER]); break;

case (STOP_NC):

  1. if DEBUG command.printf(" STOP\n");
  2. endif Stop(buffer_command[INITPARAMETER]);

break;

case (PAUSA_NC):

  1. if DEBUG command.printf(" PAUSA\n");
  2. endif Pausa(buffer_command[INITPARAMETER]); break;

case (REANUDAR_NC):

  1. if DEBUG command.printf(" REANUDAR\n");
  2. endif Reanudar(buffer_command[INITPARAMETER]); break;

/* case (MOVER_NC):

  1. if DEBUG command.printf(" MOVER\n");
  2. endif Mover(buffer_command[INITPARAMETER]); break;
  • / default:
  1. if DEBUG command.printf("Comando no encontrado\n");
  2. endif } }

int main() { myServoX.period_ms(20); myServoY.period_ms(20); myServoZ.period_ms(20);

  1. if DEBUG command.printf("inicio con debug\n");
  2. else command.printf("inicio sin debug\n");
  3. endif uint8_t val; while(DET){ val=command.getc();

if (val== '<'){ Read_command(); if (check_command()){ command_exe();

  1. if DEBUG echo_command();
  2. endif } } } }
All wikipages