main.cpp

00001 //---------------------------------LIBRERIAS--------------------------------------------
00002 #include "mbed.h"
00003 #include "math.h"
00004 #include "cstdlib"
00005 #include "InterruptIn.h"
00006 
00007 //--------------------------DECLARACON DE CONTANTES-------------------------------
00008 #define MAXPOS 255
00009 #define SS_TIME 500
00010 #define POSDRAW 300
00011 #define BUFF_SIZE 6
00012 #define COMM_N 0
00013 #define INITPARAMETER  1
00014 #define DEBUG 1
00015 #define BUFF_POS 6
00016  
00017 //--------------------------DECLARACION DE PUERTOS I/O---------------------------- 
00018 PwmOut myServoX(PB_3); //MOTOR EJE X
00019 PwmOut myServoY(PB_4); //MOTOR EJE Y
00020 PwmOut myServoZ(PB_10);//MOTOR EJE Z
00021 
00022 InterruptIn boton(PC_0);
00023 InterruptIn mybutton(USER_BUTTON);
00024 Serial command(USBTX, USBRX); //COMUNICACION SERIAL
00025 Serial Sdebug(USBTX, USBRX);  //DEBUG SERIAL
00026 DigitalOut led(LED1);          //DECLARACION LED DE LA PLACA
00027  
00028 //---------------------------DEFINICION COMANDOS POR CONSOLA---------------------- 
00029 #define LED 0
00030 #define PUNTO 1
00031 #define LINE 2
00032 #define RECTANGLE 3
00033 #define CIRCLE 4
00034 #define STOP 5
00035 #define PAUSE 6
00036 #define REANUDAR 7
00037 #define RESOLUCION 8
00038 #define HOME 9
00039 #define TIEMPO_DE_PASO 10
00040 
00041 //--------------------------LLAMADO RUTINA DE INTERRUPCIONES----------------------
00042 //Ticker ticker1; //Variable para rutina de interupciones
00043 //Timeout timeout;//Llamado de variable por conteo para manejo de pausa
00044 
00045 //-------------------------VARIABLES GLOBALES-------------------------------------
00046 int rap;//Pendiente 
00047 int RSTEP = 5;
00048 double abajo=7;
00049 double arriba=0;
00050 double k,l,r,tp=50,paso=0;
00051 int p=0;
00052 uint8_t estado = 0;
00053 
00054 //*****************************************************************************
00055 //  COMANDO  MOVER MOTOR
00056 //  |POS 1|POS 2|POS 3|POS 4| POS 5| POS 6| POS 7|
00057 //  |  <  | #C  | a   | b   |  c   |  d   |  >   |
00058 //
00059 // #C -> indica el comando
00060 // a,b,c,d parametros del comando
00061 // <,> -> inicio,  y fin de comando 
00062 //  el inicio de comando no se almacena en el buffer
00063 //*****************************************************************************
00064 
00065 //--------------------------DETERMINACION TAMAÑO DE BUFFER------------------------
00066 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};
00067 float buffer_POS [BUFF_POS]= {0,0,0,0,0,0};
00068 
00069 //--------------------FUNCIONES PARA COMANDO DE BUFFER----------------------------
00070 
00071 int coord2us(float coord)
00072     {
00073         if(0 <= coord <= MAXPOS)
00074         return int(750+coord*1900/50);// u6
00075         return 750;
00076     }
00077     void print_num(uint8_t val)
00078     {
00079         if (val <10)
00080             command.putc(val+0x30);
00081         else 
00082             command.putc(val-9+0x40);
00083     }  
00084 void print_bin2hex (uint8_t val)
00085     {
00086         command.printf(" 0x");
00087         print_num(val>>4);
00088         print_num(val&0x0f); 
00089     }
00090 void Read_command()
00091     {
00092         for (uint8_t i=0; i<BUFF_SIZE;i++)
00093         {
00094             buffer_command[i]=command.getc();
00095                 
00096         }
00097     }
00098     
00099 void echo_command()
00100     {
00101         for (uint8_t i=0; i<BUFF_SIZE;i++)
00102         {
00103             print_bin2hex(buffer_command[i]);
00104         }
00105     }
00106 uint8_t check_command()
00107     {
00108        if (buffer_command[BUFF_SIZE-1]== '>')
00109              return 1;
00110              return 0;        
00111     }
00112 void command_led(uint8_t tm)
00113     {
00114         #if DEBUG
00115             command.printf("%i, segundos", tm);
00116         #endif
00117         led=1;  
00118         wait(tm);
00119         led=0;            
00120     }
00121 void draw()
00122     {
00123         myServoZ.pulsewidth_us(POSDRAW);
00124         wait(1);
00125     }
00126 void nodraw()
00127     {
00128         myServoZ.pulsewidth_us(MAXPOS);
00129         wait(1);
00130     }
00131     uint8_t SSTIME = 100;  
00132 void tiempo_de_paso(int SST) 
00133     {
00134    
00135    
00136 //-------------------------CONFIURACION SSTIME/VERTEX-----------------------------
00137      SSTIME=SST;     
00138     #if DEBUG
00139     command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
00140     #endif              
00141     }
00142 void vertex2d(float x, float y, float z)
00143     {
00144         int pulseX = coord2us(x);
00145         int pulseY = coord2us(y);
00146         int pulseZ = coord2us(z);
00147           myServoX.pulsewidth_us(pulseX);
00148           myServoY.pulsewidth_us(pulseY);
00149           myServoZ.pulsewidth_us(pulseZ);
00150         wait_ms(SSTIME);
00151     }
00152 
00153 //--------------------------DECLARACION MEMORIA DE SERVOS-------------------------
00154 uint8_t  posx_old=0;     // posición anterior del eje X
00155 uint8_t  posy_old=0;     // posición anterior del eje Y
00156 uint8_t  posz_old=0;     // posición anterior del eje Y
00157 uint8_t  ss_time=100;     // tiempo  de espera para moverse 1 mm en microsegundos
00158 //-------------------------VARIABLES SSTIME/RESOLUCION----------------------------
00159 void put_sstime(uint8_t vtime){
00160     ss_time=vtime;
00161     }
00162 void sstime(uint8_t x, uint8_t y, uint8_t z)
00163 {
00164     double dx=abs(x-posx_old);
00165     double dy=abs(y-posy_old);
00166     double dz=abs(z-posz_old);
00167     double dist= sqrt(dx*dx+dy*dy+dz*dz);
00168     wait_ms((int)(ss_time*dist));
00169     posx_old=x;
00170     posy_old=y;
00171     posy_old=z;
00172  }
00173 void initdraw(float x, float y)
00174     {
00175         #if DEBUG
00176             Sdebug.printf("inicia dibujo \n");
00177         #endif
00178         vertex2d (x,y,abajo);
00179         wait(1);
00180         draw();
00181     }
00182 
00183 void resolucion(int res){
00184     RSTEP = res;
00185     #if DEBUG
00186     command.printf("Resolucion definida en = %i \n", RSTEP);
00187     #endif
00188     }
00189     
00190 //---------------------------------FUNCION HOME-----------------------------------
00191 void home()
00192     {
00193         vertex2d(0,0,0);
00194         wait_ms(rap); 
00195     }    
00196 void maxpos()
00197     {
00198         vertex2d(250,250,150);
00199     }
00200     
00201 //---------------------------------FUNCION DOT-----------------------------------
00202 void punto(uint8_t x, uint8_t y)
00203   {  
00204     command.printf("Coord x= %i, coord y=%i \n",x,y);
00205     vertex2d(x,y,0);
00206     wait_ms(1000);
00207     vertex2d(x,y,11);
00208     wait_ms(1000);
00209     vertex2d(x,y,0);
00210     wait_ms(1000);
00211     home();
00212   }   
00213   
00214   
00215 //------------------------------FUNCION LINE--------------------------------------
00216 void line(float xi, float xf, float yi, float yf)
00217     {
00218  // los parametros son coordenadas iniciales  y finales de la linea en milimetros   
00219         #if DEBUG
00220             Sdebug.printf("linea  Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf);
00221         #endif
00222         float xp; float yp;
00223         float m= (yf-yi)/(xf-xi);
00224         float b=(yf-m*xf);
00225         vertex2d(0,0,4);
00226         wait(1);
00227         vertex2d(xi,yi,abajo);
00228         wait(1);
00229         for (xp =xi;xp<=xf;xp+=RSTEP)
00230             {
00231                 yp =m*xp+b;
00232                 vertex2d(xp,yp,abajo);
00233          #if DEBUG
00234             Sdebug.printf("coord X = %f,coord Y = %f \n", xp,yp);
00235          #endif
00236         }
00237         nodraw();
00238         home();
00239     } 
00240     
00241 //-----------------------------FUNCION RECTANGULO---------------------------------    
00242  void rectangle(uint8_t xi, uint8_t yi,uint8_t a, uint8_t h)
00243 {
00244  int x,y;
00245  x = a+ xi;
00246  y = h+ yi;
00247  
00248  vertex2d(xi,yi,arriba);
00249  wait_ms(600);
00250  vertex2d(xi,yi,abajo);
00251  wait_ms(600);
00252  vertex2d(x,yi,abajo);  
00253  wait_ms(600);
00254  vertex2d(x,y,abajo); 
00255  wait_ms(600);
00256  vertex2d(xi,y,abajo); 
00257  wait_ms(600);
00258  vertex2d(xi,yi,abajo); 
00259  wait_ms(600);
00260  home();
00261  }
00262  
00263  //-----------------------------FUNCION CIRCULO-----------------------------------
00264  void circle(uint8_t x1, uint8_t y1,uint8_t r)
00265   {
00266   double  x2,y2,i,j;
00267   vertex2d(x1 ,y1,arriba);
00268   wait(1);
00269   wait_ms(rap);
00270  
00271   for (  x2=0;x2<=r;x2++){
00272       y2= pow(((r*r)-(x2*x2)),0.5);
00273       i=  x2+x1;  
00274       j=y1+y2;
00275       vertex2d(i ,j,abajo) ;
00276       wait_ms(30);
00277       #if DEBUG
00278             Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
00279          #endif
00280       }
00281        
00282     for (  x2=r;x2>=0;x2--){
00283       y2= pow(((r*r)-(x2*x2)),0.5);   
00284       i=  x2+x1;  
00285       j= y1 - y2;
00286       vertex2d(i ,j,abajo) ;
00287       wait_ms(30);
00288       #if DEBUG
00289             Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
00290          #endif
00291        }
00292   
00293   for (  x2=0;x2<=r;x2++){
00294       y2= pow(((r*r)-(x2*x2)),0.5);   
00295       i=  x1- x2;  
00296       j= y1 - y2;
00297       vertex2d(i ,j,abajo) ;
00298       wait_ms(30);
00299       #if DEBUG
00300             Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
00301          #endif
00302        }
00303   for (  x2=r;x2>=0;x2--){
00304       y2= pow(((r*r)-(x2*x2)),0.5);   
00305       i=  x1-x2;  
00306       j=y1+y2;
00307       vertex2d(i ,j,abajo) ;
00308       wait_ms(30);
00309       #if DEBUG
00310             Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
00311          #endif
00312        }
00313       vertex2d(i ,j,arriba) ;
00314       wait_ms(rap);
00315       #if DEBUG
00316             Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
00317          #endif
00318       
00319       if(x2==r)
00320       {
00321       wait(1);  
00322     home();
00323     }
00324         
00325 }
00326 //-------------------------------FUNCION REANUDAR---------------------------------
00327     void reanudar(){
00328         
00329         command.printf("piccolo reanudando \n");
00330         wait (1);
00331         return;
00332     
00333         }
00334 //------------------------------FUNCION PAUSA------------------------------------        
00335 void pause(){
00336        
00337        led = !led;                            
00338        command.printf("piccolo en pausa \n");
00339        wait (5);
00340        //return; 
00341        
00342     }
00343        
00344 //-------------------------------FUNCION PARE-------------------------------------            
00345 void stop() //Funcion para detener el programa  
00346     {
00347     command.printf("piccolo en Stop \n");
00348     home();  
00349     exit(0);
00350     
00351              
00352     }  
00353 
00354 void command_exe()
00355     {
00356         
00357 //-------------------------ASIGNACION EN SWTICH CASE------------------------------        
00358         #if DEBUG
00359             command.printf("Ejecutando comando: ");
00360         #endif  
00361         switch (buffer_command[COMM_N])
00362         {
00363             case (LED): 
00364                 #if DEBUG
00365                     command.printf("LED on\n");
00366                 #endif
00367                 command_led(buffer_command[INITPARAMETER]);
00368             break;
00369         
00370             case (PUNTO): 
00371                 punto(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);    
00372                 #if DEBUG
00373                 command.printf("Dibujando Punto");
00374                 #endif
00375             break;
00376 
00377             case LINE:
00378                 #if DEBUG
00379                     command.printf("Dibujando Linea\n");
00380                 #endif
00381                 line(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
00382                 
00383                 
00384             break;
00385 
00386             case RECTANGLE: 
00387                 #if DEBUG
00388                     command.printf("Dibujando Rectangulo\n");
00389                 #endif
00390                 rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
00391             break;
00392             
00393             case  (CIRCLE): 
00394                 #if DEBUG
00395                     command.printf("Dibujando Circulo\n");
00396                 #endif
00397                 circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
00398             break;
00399             
00400             case  (STOP): 
00401                  #if DEBUG
00402                  command.printf("    STOP\n");
00403                  #endif
00404              break;
00405             
00406             case  (PAUSE): 
00407                #if DEBUG
00408                     command.printf("programa pausado\n");
00409               #endif
00410                 pause();
00411              break;
00412             
00413            case  (REANUDAR): 
00414               #if DEBUG
00415                    command.printf("draw reanudar programa\n");
00416                 #endif
00417                 reanudar();
00418            break;
00419             
00420             case  (RESOLUCION): 
00421                   resolucion(buffer_command[INITPARAMETER]);
00422                   #if DEBUG
00423                   command.printf("Resolucion definida");
00424                   #endif
00425             break;
00426             case  (HOME): 
00427                  home();
00428                 #if DEBUG
00429                     command.printf("Posicion Home");
00430                 #endif
00431             break;
00432             
00433             case  (TIEMPO_DE_PASO): 
00434                 #if DEBUG
00435                     command.printf("Nuevo tiempo para paso:\res");
00436                 #endif
00437                 tiempo_de_paso(buffer_command[INITPARAMETER]);
00438             break;
00439             
00440             default:
00441                 #if DEBUG
00442                     command.printf("comando  no encontrado\n");
00443                 #endif
00444        
00445         }
00446 }
00447 
00448 //--------------------------***PENDIENTE***---------------------------------------
00449     void Rx_interrupcion(){
00450         
00451         char p = command.getc();
00452     
00453         command.printf("picolo en pausa %c\n",p);
00454       wait_ms(2000);
00455      return ;
00456            
00457         }    
00458         
00459     
00460 //----------------------------------MAIN DE PROGRAMA------------------------------
00461 int main() {
00462     #if DEBUG
00463     command.printf("inicio con debug\n");
00464     #else
00465     command.printf("inicio sin debug\n");
00466     #endif
00467          uint8_t val;
00468    // mybutton.rise(&stop);
00469     while(1){
00470     val=command.getc();
00471         if (val== '<'){
00472             Read_command();
00473             mybutton.fall(&pause);
00474             mybutton.rise(&reanudar);
00475             boton.fall(&stop);
00476             if (check_command()){
00477                 command_exe();
00478             }
00479             else{
00480                 #if DEBUG
00481                     command.printf("\n ERROR COMANDO -> ");
00482                     echo_command();
00483                 #endif
00484             }        
00485         }
00486         else{
00487                 #if DEBUG
00488                  command.printf("error de inicio de trama: ");
00489                  command.putc(val);
00490                 #endif
00491         
00492            }
00493           }
00494                      
00495             }
00496         
00497                 
00498     
00499     
00500