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.
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:02b166bd1478
- Child:
- 1:c0ff30bf8db2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Nov 18 14:53:56 2017 +0000 @@ -0,0 +1,541 @@ +#include "mbed.h" +#include "InterruptIn.h" +#include "stm32f4xx_hal.h" + +//Serial blue(PA_9, PA_10); //PA_10=TX , PA_9=RX ... D8 y D2 respectivamente en la nucleo tx1 +Serial pc(PA_9, PA_10); +Serial command(USBTX, USBRX ,9600); +DigitalOut led(LED1); + + +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 stepmotor(); +void command_exe(); + +#define DEBUG 1 // Servomotores +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 +#define BUFF_SIZE 6 +#define COMM_N 0 +#define INITPARAMETER 1 +#define MAXPOS 50 +#define POSDRAW 00 +#define SS_TIME 500 +#define PI 3.1415926 +int RSTEP = 1; // en milimetros +int cont = 1; +int tiempo = 5; +//int time = 0; +int pause = 1; // Tiempo en us (tiempo entre pasos) nos da la velocidad +uint8_t val; +// COMANDOS +#define LED_NC 0 //ok +#define DOT_NC 1 //ok +#define LINE_NC 2 //ok +#define RECTANGLE_NC 3 //ok +#define CICLE_NC 4 //ok +#define HOME_NC 5 //ok +#define RESOLUCION_NC 6 //ok +#define TIEMPOPASOS_NC 7 +#define STOP_NC 8 +#define PAUSA_NC 9 +#define REANUDAR_NC 10 +#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(){ + wait(2); + myServoZ.pulsewidth_ms(2); + wait_ms(ss_time*2); } + +void nodraw(){ + myServoZ.pulsewidth_ms(1); + wait_ms(ss_time*2); + wait(1); + //home(); + } + + +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); + pc.putc(val+0x30);} + else { + command.putc(val-9+0x40); + pc.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]== '>'){ + + #if DEBUG + command.printf("\nComando:"); + print_bin2hex(buffer_command[COMM_N]); + command.printf(" -> "); + #endif + return 1; + } + #if DEBUG + command.printf("\n ERROR COMANDO -> "); + echo_command(); + #endif + return 0; } + +///////////////Funciones para dibujar//////////////// + +void command_led(int tm){ + + led=1; + wait(tm); + led=0; } + +void punto(uint8_t x, uint8_t y){ + + vertex2d(x,y); + wait(1); + draw(); + wait(1); + nodraw(); + #if DEBUG + command.printf("Coord x= %i, coord y=%i \n",x,y); + #endif + + } + +void linea(float xi, float yi, float xf, float yf) + { + #if DEBUG + command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP); + #endif + float xp,yp; + float m=(yf-yi)/(xf-xi); + float b=yf-(m*xf); + #if DEBUG + command.printf("\n b =%f, m=%f \n", b,m); + #endif + float nstep =(m/RSTEP); + //nstep=RSTEP; + #if DEBUG + command.printf("\nstep = %f \n", nstep); + #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); + #if DEBUG + command.printf(" CASO 1: ( dx>dy & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp); + #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); + #if DEBUG + command.printf(" CASO 2: ( dx>dy & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp); + #endif + }}} + else { + if (yf>yi){ + initdraw(xp,yp); + for (yp=yi; yp<=yf; yp+=RSTEP){ + xp=(yp-b)/m; + vertex2d(xp,yp); + #if DEBUG + command.printf(" CASO 3: ( dy>dx & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp); + #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); + #if DEBUG + command.printf(" CASO 4: ( dy>dx & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp); + #endif + } + } + } + nodraw(); + } + +void rectangle(uint8_t x, uint8_t y, uint8_t a, uint8_t h) + { + #if DEBUG + command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP); + #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); + #if DEBUG + command.printf("Coordenadas x=%i,y=%i for 1\n", xi,y); + #endif + } + wait(10); + for (uint8_t yi=y; yi<=(y+h); yi+=RSTEP){ + vertex2d(x+a,yi); + #if DEBUG + command.printf("Coordenadas x=%i,y=%i for 2\n", x+a,yi); + #endif + } + for(uint8_t xf=A; xf>x; xf= xf - RSTEP){ + vertex2d(xf,B); + #if DEBUG + command.printf("Coordenadas x=%i,y=%i for 3\n", xf,B); + #endif + } + for (uint8_t yf=(y+h); yf>y; yf-=RSTEP){ + vertex2d(x,yf); + #if DEBUG + command.printf("Coordenadas x=%i,y=%i for 4\n", x,yf); + #endif + } + vertex2d(x,y); + #if DEBUG + command.printf("Coordenadas x=%i,y=%i for 4\n", x,y); + #endif + nodraw(); + } +void circle(float cx, float cy, float 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); + #if DEBUG + command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180)); + #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); + #if DEBUG + command.printf("position x =%li ; position y =%li \n",x,y); + #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); + #if DEBUG + command.printf("position x =%li ; position y =%li\n",x,y); + #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); + #if DEBUG + command.printf("position x =%li , position y =%li \n",x,y); + #endif + wait_ms(10); + } + wait(1); + nodraw(); + } + +void home(){ + vertex2d(0,0); + nodraw(); + + } + +void resolucion(int res){ + + RSTEP = res; + #if DEBUG + command.printf("Resolucion definida en =%i \n", RSTEP); + #endif +} + +void command_sstime(){ + #if DEBUG + command.printf("config tiempo entre movimientos de los servos: "); + command.printf("SSTIME = %i\n", buffer_command[1]); + #endif + put_sstime(buffer_command[1]); + } + +void stop(){ + + #if DEBUG + command.printf("stop"); + #endif + exit(0); + } + +void pausa(){ + + // while(pause == 0){ + + wait(10000); + //} + /* #if DEBUG + command.printf("piccolo en pausa "); + #endif + led=0; + wait(2);*/ +} + +void reanudar(){ + + while(pause == 1){ + + wait(0); + } + #if DEBUG + command.printf("reanuda piccolo "); + #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 +*/ + + +//*********Interrupcion*******// + +void interrup(){ + val=command.getc(); + if (val== '<'){ + Read_command(); + if (check_command()){ + command_exe(); + #if DEBUG + echo_command(); + #endif + command.printf("%s", buffer_command); + + } + } + else command.printf("error inicio trama: %d ",val); + command.putc(val); + return; + + } + +/////////CASOS PARA LA SELECCION DE COMANDOS///////////// + +void command_exe() + { + #if DEBUG + command.printf("Ejecutando comando: \n"); + #endif + +switch (buffer_command[COMM_N]){ + +case (LED_NC): + #if DEBUG + command.printf("led on 7 off\n"); + #endif + command_led(buffer_command[INITPARAMETER]); +break; + +case (DOT_NC): + #if DEBUG + command.printf("Punto \n"); + #endif + punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]); +break; + +case (LINE_NC): + #if DEBUG + command.printf("draw line\n"); + #endif + linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]); + + + + +break; + +case (RECTANGLE_NC): + #if DEBUG + command.printf("cuadrado \n"); + #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]); + #if DEBUG + command.printf("Circulo\n"); + #endif +break; +case (HOME_NC): + #if DEBUG + command.printf("Yendo a Home \n"); + #endif + home(); +break; +case (RESOLUCION_NC): + resolucion(buffer_command[INITPARAMETER]); + #if DEBUG + command.printf("Tiempo pasos \n"); + #endif +break; +case (TIEMPOPASOS_NC): + sstime(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]); + #if DEBUG + command.printf("Tiempo pasos \n"); + #endif +break; +case (STOP_NC): + #if DEBUG + command.printf("Stop \n"); + #endif +break; + +case (PAUSA_NC): + #if DEBUG + command.printf("Pausa \n"); + #endif + pausa(); +break; +case (REANUDAR_NC): + #if DEBUG + command.printf("Reanudar \n"); + #endif +break; + +default: + + #if DEBUG + command.printf("comando no encontrado\n"); + #endif +}} + +int main() { + #if DEBUG + command.printf("inicio con debug\n"); + #else + command.printf("inicio sin debug\n"); + #endif + command.attach(&interrup); + /*while(1){ + + //command.printf("%d",buffer_command[0]); + wait(1); + }*/ + +}