CODIGO

Page last updated 23 Nov 2017, by Diego Lopez. 0 replies

  1. include "mbed.h"
  2. include "InterruptIn.h"
  3. include "stm32f4xx_hal.h"
  4. include "TextLCD.h"

Serial command(USBTX, USBRX);TX->PA_9 RX->PA_10USBTX, USBRX DigitalOut led(LED1); Ticker tsk; InterruptIn button(USER_BUTTON);

  1. define DEBUG 1 PwmOut myServoX(PB_3); PwmOut myServoY(PB_5); PwmOut myServoZ(PB_4); TextLCD lcd(PC_7,PB_6,PA_7,PC_0,PA_8,PB_10, TextLCD::LCD16x2); rs, e, d4-d7 Salidas Motores paso a paso DigitalOut A(PB_8); DigitalOut B(PB_9); DigitalOut C(PA_5); DigitalOut D(PA_6);

DigitalOut E(PA_0); DigitalOut F(PA_1); DigitalOut G(PA_4); DigitalOut H(PB_0);

void pausa(); void command_led(); void punto(); void linea(); void rectangle(); void circle(); void home(); void resolucion(); void tiempo_pasos(); void stop(); void pausa(); void reanudar(); void mover(); void recibeByte(); void command_exe();

*************** 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 COMM_N2 12
  4. define INITPARAMETER 1
  5. define MAXPOS 50
  6. define POSDRAW 00
  7. define SS_TIME 500
  8. define PI 3.1415926 int RSTEP = 1; en milimetros uint8_t val; int cont=1; uint8_t estado; int tiempo=4;

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 7
  9. define STOP_NC 8
  10. define PAUSA_NC 9
  11. define REANUDAR_NC 10
  12. define MOVER_CN 11

////Funciones para mover los servos/////

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){ Funcion para enviarle la posicion a (x,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 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0}; Matriz del Comando enviado uint8_t buffer[BUFF_SIZE]={0,0,0,0,0,0};

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); }

void initdraw(float x, float y)ok { vertex2d(x,y); wait_ms(ss_time); draw(); }

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

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); }

TODO : TIMEOUT UART SERIAL

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() { 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 command.printf("\n ERROR COMANDO -> "); echo_command();
  4. endif return 0; }

///Funciones para dibujar////

void command_led(int tm){

led=1; wait(tm); led=0; lcd.cls(); lcd.printf("Led Encendido segundos %d",tm); }

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); lcd.cls(); lcd.printf("Dibujando Punto x= %i y=%i \n",x,y);
  2. endif }

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

  1. if DEBUG command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP); lcd.cls(); lcd.printf("Dibujando Linea xi=%i yi=%i xf=%i yf=%i \n",xi,yi,xf,yf);
  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);
  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);
  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);
  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);
  14. endif } } } nodraw(); }

void rectangle(uint8_t x, uint8_t y, uint8_t a, uint8_t h) {

  1. if DEBUG command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP); lcd.cls(); lcd.printf("Dibujando rectangulo x=%d y=%d",x,y);
  2. endif uint8_t A=x+a; uint8_t B=y+h; 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 for 1\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 for 2\n", x+a,yi);
  4. endif } for(uint8_t xf=A; xf>x; xf= xf - RSTEP){ vertex2d(xf,B);
  5. if DEBUG command.printf("Coordenadas x=%i,y=%i for 3\n", xf,B);
  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 for 4\n", x,yf);
  8. endif } vertex2d(x,y);
  9. if DEBUG command.printf("Coordenadas x=%i,y=%i for 4\n", x,y);
  10. endif nodraw(); } void circle(float cx, float cy, float radio) { lcd.cls(); lcd.printf("Dibujando Circulo r=%i",radio); int y; int x; vertex2d(cx,cy); draw(); wait(1); for(double i=0; i<=90 ;i+=RSTEP) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  11. if DEBUG command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180));
  12. endif wait_ms(10); } for(double i=90; i<=180 ;i+=RSTEP) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  13. if DEBUG command.printf("position x =%li ; position y =%li \n",x,y);
  14. endif wait_ms(10); } for(double i=180; i<=270 ;i+=RSTEP) { x=radio*cos(i*3.1416/180); y=radio*sin(i*3.1416/180); vertex2d(cx+x,cy+y);
  15. if DEBUG command.printf("position x =%li ; position y =%li\n",x,y);
  16. endif wait_ms(10); }

for(double i=270; i<=360 ;i+=RSTEP) { 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(10); } wait(1); nodraw(); }

void home(){ lcd.cls(); lcd.printf("HOME"); vertex2d(0,0); }

void resolucion(int res){

RSTEP = res;

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

void command_sstime(){

  1. if DEBUG command.printf("config tiempo entre movimientos de los servos: "); command.printf("SSTIME = %i\n", buffer_command[1]); lcd.cls(); lcd.printf("Tiempo pasos");
  2. endif put_sstime(buffer_command[1]); }

void stop(){

  1. if DEBUG command.printf("Stop"); lcd.cls(); lcd.printf("STOP");
  2. endif led=!led; wait(5);

}

void pausa(){

  1. if DEBUG command.printf("Piccolo en pausa "); lcd.cls(); lcd.printf("Piccolo en pausa");
  2. endif led=0; command.putc(cont+0x30); wait(5); }

void reanudar(){

  1. if DEBUG command.printf("Reanuda piccolo "); lcd.cls(); lcd.printf("Reanuda piccolo ");
  2. endif } **MOTOR PASO A PASO***

/* Paso A B C D Paso E F G H 1 1 1 0 0 1 1 1 0 0 2 0 1 1 0 2 0 1 1 0 3 0 0 1 1 3 0 0 1 1 4 1 0 0 1 4 1 0 0 1

  • /

void paso_der1(){ A = 1;B = 1;C = 0;D = 0; wait_ms(tiempo); A = 0;B = 1;C = 1;D = 0; wait_ms(tiempo); A = 0;B = 0;C = 1;D = 1; wait_ms(tiempo); A = 1;B = 0;C = 0;D = 1; wait_ms(tiempo); } void paso_der2(){ E = 1;F = 1;G = 0;H = 0; wait_ms(tiempo); E = 0;F = 1;G = 1;H = 0; wait_ms(tiempo); E = 0;F = 0;G = 1;H = 1; wait_ms(tiempo); E = 1;F = 0;G = 0;H = 1; wait_ms(tiempo); } void paso_izq1(){ A = 0;B = 0;C = 1;D = 1; wait_ms(tiempo); A = 0;B = 1;C = 1;D = 0; wait_ms(tiempo); A = 1;B = 1;C = 0;D = 0; wait_ms(tiempo); A = 1;B = 0;C = 0;D = 1; wait_ms(tiempo); } void paso_izq2(){ E = 0;F = 0;G = 1;H = 1; wait_ms(tiempo); E = 0;F = 1;G = 1;H = 0; wait_ms(tiempo); E = 1;F = 1;G = 0;H = 0; wait_ms(tiempo); E = 1;F = 0;G = 0;H = 1; wait_ms(tiempo); }

void apagado(){ A = 0;B = 0;C = 0;D = 0; E = 0;F = 0;G = 0;H = 0; wait_ms(tiempo); command.printf("off \n"); }

void stepmotor(double cuadrante, int dir){

  1. if DEBUG command.printf("comando mover piccolo PAP ");
  2. endif double dato_rx=0; valor recibido en grados double dato_rx2=0; double dato_rx3=0; valor recibido en grados double dato_rx4=0; float numero_pasos = 0; Valor en grados donde se encuentra el motor float numero_pasos2 = 0; float numero_pasos3 = 0; float numero_pasos4 = 0;

if (dir==0){ int cu=cuadrante; lcd.cls(); lcd.printf("Piccolo Adelante %i Cuadrantes",cu); dato_rx = cuadrante*(90 * 1.4222222222); 90 grados equivalen a 5 cm en desplazamiento while(dato_rx>numero_pasos){ Movimiento hacia adelante paso_der2(); paso_izq1(); numero_pasos = numero_pasos + 1; }} if (dir==1){ int cu=cuadrante; lcd.cls(); lcd.printf("Piccolo Atras %i Cuadrantes",cu); dato_rx2 = cuadrante*(90 * 1.4222222222); 90 grados equivalen a 5 cm en desplazamiento while(dato_rx2>numero_pasos2){ Movimiento hacia atras paso_der1(); paso_izq2(); numero_pasos2 = numero_pasos2 + 1; }} if (dir==2){ lcd.cls(); lcd.printf("Giro 360 grados derecha"); dato_rx3 = (450 * 1.4222222222); while(dato_rx3>numero_pasos3){ paso_izq1(); paso_izq2(); numero_pasos3 = numero_pasos3 + 1; }} if (dir==3){ lcd.cls(); lcd.printf("Giro 360 grados izquierda"); dato_rx4 = (450 * 1.4222222222); while(dato_rx4>numero_pasos3){ paso_der1(); paso_der2(); numero_pasos4 = numero_pasos4 + 1; }} apagado(); return; } **Interrupcion**

/CASOS PARA LA SELECCION DE COMANDOS///

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 (CICLE_NC): circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);

  1. if DEBUG command.printf("Circulo\n");
  2. endif break; case (HOME_NC):
  3. if DEBUG command.printf("Yendo a Home \n");
  4. endif home(); break; case (RESOLUCION_NC): resolucion(buffer_command[INITPARAMETER]);
  5. if DEBUG command.printf("Tiempo pasos");
  6. endif break; case (TIEMPOPASOS_NC): sstime(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);
  7. if DEBUG command.printf("Tiempo pasos");
  8. endif break; case (STOP_NC):
  9. if DEBUG command.printf("Stop");
  10. endif break;

case (PAUSA_NC):

  1. if DEBUG command.printf("Pausa");
  2. endif pausa(); break; case (REANUDAR_NC):
  3. if DEBUG command.printf("Reanudar");
  4. endif break; case (MOVER_CN):
  5. if DEBUG command.printf("PAP");
  6. endif stepmotor(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]); 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 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); }

} }

Please log in to post comments.