Esta última actualización contiene el código funcional en las 10 funciones.
Dependencies: mbed
Fork of jc by
main.cpp
- Committer:
- VaLaraC
- Date:
- 2017-11-24
- Revision:
- 2:773082426491
- Parent:
- 1:a9b3fdab473e
- Child:
- 3:c7e3f75d4f10
File content as of revision 2:773082426491:
//---------------------------------LIBRERIAS-------------------------------------------- #include "mbed.h" #include "math.h" #include "cstdlib" //--------------------------DECLARACON DE CONTANTES------------------------------- #define MAXPOS 255 #define SS_TIME 500 #define POSDRAW 300 #define BUFF_SIZE 6 #define COMM_N 0 #define INITPARAMETER 1 #define DEBUG 1 #define BUFF_POS 6 //--------------------------DECLARACION DE PUERTOS I/O---------------------------- PwmOut myServoX(PB_3); //MOTOR EJE X PwmOut myServoY(PB_4); //MOTOR EJE Y PwmOut myServoZ(PB_10);//MOTOR EJE Z InterruptIn mybutton2(PA_0); InterruptIn mybutton(USER_BUTTON); Serial command(USBTX, USBRX); //COMUNICACION SERIAL Serial Sdebug(USBTX, USBRX); DigitalOut led(LED1); //DECLARACION LED DE LA PLACA //---------------------------DEFINICION COMANDOS POR CONSOLA---------------------- #define LED 0 #define PUNTO 1 #define LINE 2 #define RECTANGLE 3 #define CIRCLE 4 #define STOP 5 #define PAUSE 6 #define REANUDAR 7 #define RESOLUCION 8 #define HOME 9 #define TIEMPO_DE_PASO 10 //--------------------------LLAMADO RUTINA DE INTERRUPCIONES---------------------- //Ticker ticker1; //Variable para rutina de interupciones //Timeout timeout;//Llamado de variable por conteo para manejo de pausa //-------------------------VARIABLES GLOBALES------------------------------------- int rap;//Pendiente int RSTEP = 5; double abajo=7; double arriba=0; double k,l,r,tp=50,paso=0; int p=0; uint8_t estado = 0; //***************************************************************************** // COMANDO MOVER MOTOR // |POS 1|POS 2|POS 3|POS 4| POS 5| POS 6| POS 7| // | < | #C | a | b | c | d | > | // // #C -> indica el comando // a,b,c,d parametros del comando // <,> -> inicio, y fin de comando // el inicio de comando no se almacena en el buffer //***************************************************************************** //--------------------------DETERMINACION TAMAÑO DE BUFFER------------------------ uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0}; float buffer_POS [BUFF_POS]= {0,0,0,0,0,0}; //--------------------FUNCIONES PARA COMANDO DE BUFFER---------------------------- int coord2us(float coord) { if(0 <= coord <= MAXPOS) return int(750+coord*1900/50);// u6 return 750; } 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); } 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]== '>') return 1; return 0; } void command_led(uint8_t tm) { #if DEBUG command.printf("%i, segundos", tm); #endif led=1; wait(tm); led=0; } void draw() { myServoZ.pulsewidth_us(POSDRAW); wait(1); } void nodraw() { myServoZ.pulsewidth_us(MAXPOS); wait(1); } uint8_t SSTIME = 100; void tiempo_de_paso(int SST) { //-------------------------CONFIURACION SSTIME/VERTEX----------------------------- SSTIME=SST; #if DEBUG command.printf("\nTiempo en pasos definida en:%i\n",SSTIME); #endif } void vertex2d(float x, float y, float z) { int pulseX = coord2us(x); int pulseY = coord2us(y); int pulseZ = coord2us(z); myServoX.pulsewidth_us(pulseX); myServoY.pulsewidth_us(pulseY); myServoZ.pulsewidth_us(pulseZ); wait_ms(SSTIME); } //--------------------------DECLARACION MEMORIA DE SERVOS------------------------- uint8_t posx_old=0; // posición anterior del eje X uint8_t posy_old=0; // posición anterior del eje Y uint8_t posz_old=0; // posición anterior del eje Y uint8_t ss_time=100; // tiempo de espera para moverse 1 mm en microsegundos //-------------------------VARIABLES SSTIME/RESOLUCION---------------------------- void put_sstime(uint8_t vtime){ ss_time=vtime; } void sstime(uint8_t x, uint8_t y, uint8_t z) { double dx=abs(x-posx_old); double dy=abs(y-posy_old); double dz=abs(z-posz_old); double dist= sqrt(dx*dx+dy*dy+dz*dz); wait_ms((int)(ss_time*dist)); posx_old=x; posy_old=y; posy_old=z; } void initdraw(float x, float y) { #if DEBUG Sdebug.printf("inicia dibujo \n"); #endif vertex2d (x,y,abajo); wait(1); draw(); } void resolucion(int res){ RSTEP = res; #if DEBUG command.printf("Resolucion definida en = %i \n", RSTEP); #endif } //---------------------------------FUNCION HOME----------------------------------- void home() { vertex2d(0,0,0); wait_ms(rap); } void maxpos() { vertex2d(250,250,150); } //---------------------------------FUNCION DOT----------------------------------- void punto(uint8_t x, uint8_t y) { command.printf("Coord x= %i, coord y=%i \n",x,y); vertex2d(x,y,0); wait_ms(1000); vertex2d(x,y,11); wait_ms(1000); vertex2d(x,y,0); wait_ms(1000); home(); } //------------------------------FUNCION LINE-------------------------------------- void line(float xi, float xf, float yi, float yf) { // los parametros son coordenadas iniciales y finales de la linea en milimetros #if DEBUG Sdebug.printf("linea Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf); #endif float xp; float yp; float m= (yf-yi)/(xf-xi); float b=(yf-m*xf); vertex2d(0,0,4); wait(1); vertex2d(xi,yi,abajo); wait(1); for (xp =xi;xp<=xf;xp+=RSTEP) { yp =m*xp+b; vertex2d(xp,yp,abajo); #if DEBUG Sdebug.printf("coord X = %f,coord Y = %f \n", xp,yp); #endif } nodraw(); home(); } //-----------------------------FUNCION RECTANGULO--------------------------------- void rectangle(uint8_t xi, uint8_t yi,uint8_t a, uint8_t h) { int x,y; x = a+ xi; y = h+ yi; vertex2d(xi,yi,arriba); wait_ms(600); vertex2d(xi,yi,abajo); wait_ms(600); vertex2d(x,yi,abajo); wait_ms(600); vertex2d(x,y,abajo); wait_ms(600); vertex2d(xi,y,abajo); wait_ms(600); vertex2d(xi,yi,abajo); wait_ms(600); home(); } //-----------------------------FUNCION CIRCULO----------------------------------- void circle(uint8_t x1, uint8_t y1,uint8_t r) { double x2,y2,i,j; vertex2d(x1 ,y1,arriba); wait(1); wait_ms(rap); for ( x2=0;x2<=r;x2++){ y2= pow(((r*r)-(x2*x2)),0.5); i= x2+x1; j=y1+y2; vertex2d(i ,j,abajo) ; wait_ms(30); #if DEBUG Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); #endif } for ( x2=r;x2>=0;x2--){ y2= pow(((r*r)-(x2*x2)),0.5); i= x2+x1; j= y1 - y2; vertex2d(i ,j,abajo) ; wait_ms(30); #if DEBUG Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); #endif } for ( x2=0;x2<=r;x2++){ y2= pow(((r*r)-(x2*x2)),0.5); i= x1- x2; j= y1 - y2; vertex2d(i ,j,abajo) ; wait_ms(30); #if DEBUG Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); #endif } for ( x2=r;x2>=0;x2--){ y2= pow(((r*r)-(x2*x2)),0.5); i= x1-x2; j=y1+y2; vertex2d(i ,j,abajo) ; wait_ms(30); #if DEBUG Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); #endif } vertex2d(i ,j,arriba) ; wait_ms(rap); #if DEBUG Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); #endif if(x2==r) { wait(1); } home(); } } //-------------------------------FUNCION REANUDAR--------------------------------- void reanudar(){ command.printf("piccolo reanudando \n"); wait (1); return; } //------------------------------FUNCION PAUSA------------------------------------ void pause(){ led = !led; command.printf("piccolo en pausa \n"); wait (5); return; } //-------------------------------FUNCION PARE------------------------------------- void stop() //Funcion para detener el programa { exit(0); home(); } void command_exe() { //-------------------------ASIGNACION EN SWTICH CASE------------------------------ #if DEBUG command.printf("Ejecutando comando: "); #endif switch (buffer_command[COMM_N]) { case (LED): #if DEBUG command.printf("LED on\n"); #endif command_led(buffer_command[INITPARAMETER]); break; case (PUNTO): punto(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]); #if DEBUG command.printf("Dibujando Punto"); #endif break; case LINE: #if DEBUG command.printf("Dibujando Linea\n"); #endif line(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]); break; case RECTANGLE: #if DEBUG command.printf("Dibujando Rectangulo\n"); #endif rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]); break; case (CIRCLE): #if DEBUG command.printf("Dibujando Circulo\n"); #endif circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]); break; case (STOP): #if DEBUG command.printf(" STOP\n"); #endif break; case (PAUSE): #if DEBUG command.printf("programa pausado\n"); #endif pause(); break; case (REANUDAR): #if DEBUG command.printf("draw reanudar programa\n"); #endif reanudar(); break; case (RESOLUCION): resolucion(buffer_command[INITPARAMETER]); #if DEBUG command.printf("Resolucion definida"); #endif break; case (HOME): home(); #if DEBUG command.printf("Posicion Home"); #endif break; case (TIEMPO_DE_PASO): #if DEBUG command.printf("Nuevo tiempo para paso:\res"); #endif tiempo_de_paso(buffer_command[INITPARAMETER]); break; default: #if DEBUG command.printf("comando no encontrado\n"); #endif } } //--------------------------***PENDIENTE***--------------------------------------- void Rx_interrupcion(){ char p = command.getc(); command.printf("picolo en pausa %c\n",p); wait_ms(2000); return ; } //----------------------------------MAIN DE PROGRAMA------------------------------ int main() { #if DEBUG command.printf("inicio con debug\n"); #else command.printf("inicio sin debug\n"); #endif uint8_t val; // mybutton.rise(&stop); while(1){ val=command.getc(); if (val== '<'){ Read_command(); mybutton.fall(&pause); mybutton.rise(&reanudar); if (check_command()){ command_exe(); } else{ #if DEBUG command.printf("\n ERROR COMANDO -> "); echo_command(); #endif } } else{ #if DEBUG command.printf("error de inicio de trama: "); command.putc(val); #endif } } }