main.cpp

00001 #include "mbed.h"
00002 #include "def.h"
00003 #include "Step_Motor.h"
00004 DigitalOut output1(LED1); // digital output 
00005 
00006 
00007 
00008 
00009     stepmotor motorST(D6,D7,D8,D9,D10,D11,D12,D13);
00010 
00011 
00012 DigitalOut myled(LED1); //Led
00013 
00014 Serial pc(USBTX,USBRX); //Comunicacion Serial
00015 
00016 PwmOut mypwmX(PB_3); //Servo eje x
00017 PwmOut mypwmY(PB_5); //Servo eje y
00018 PwmOut mypwmZ(PB_4); //Servo eje z
00019 
00020 uint32_t mem_head = 0;  // SI SUBE LA CABEZA ES PORQUE ESTA GUADANDO
00021 uint32_t mem_tail = 0;  // SI SUBE LA COLA ES PORQUE SE EMPIEZAN A LEER LAS MEMORIAS
00022 uint32_t full=0;
00023 
00024 MEM_TYPE buffer[MEM_SIZE]; // EL ARREGLO VA A TENER EL TAMAÑO DE MEMSIZE ----> BUFFER ES L ARREGLO
00025 
00026 void mem_free() // LIBRERIA MEMORIA
00027 {
00028     mem_head=0;
00029     full=0;
00030     mem_tail=0;
00031 }
00032 
00033 uint32_t mem_put(MEM_TYPE data) // 
00034 {
00035     if(full)  // SI LA MEMORIA ESTA LLENA
00036     {
00037         return 1; // 
00038     }
00039     buffer [mem_head]=data;  // GUARDAR EL DATO EN LA POSICION MEM_HEAD DEL ARREGLO BUFFER
00040     mem_head+=1; // SE SUMA UNA POSICION A LA CABEZERA
00041     if(mem_head==MEM_SIZE)
00042     {
00043         full=1; 
00044     }
00045     return 0; // TODO EN ORDEN
00046 }
00047 
00048 uint32_t mem_get(MEM_TYPE* data) // EL * LE INDICA QUE ES UN APUNTADOR
00049 {
00050     if(mem_head == 0) // NO HAY DATOS
00051         return 1;
00052     if (mem_head == mem_tail) // YA SE TERMINO LA LECTURA DE DATOS
00053         return 1;
00054         
00055     *data=buffer[mem_tail];  // SE LE CARGA A DATA LO QUE ESTA EN BUFFER EN LA POSICION MEM_TAIL
00056     mem_tail +=1; // SE INCREMENTA EL APUNTADOR MEM_TAIL
00057     
00058     return 0;
00059 }
00060 
00061 void Vertex2D(uint8_t,uint8_t);
00062 void SetZ(uint8_t);
00063 uint8_t x,y,z;
00064 int xant=0,yant=0,xnew=0,ynew=0,xmov=0,ymov=0; // POSICIONES DEL PICOLO EN LA MATRIZ
00065 int mm2pulse(float vmm); //FUNCION MILIMETROS A PULSOS
00066 void ejecutar();
00067 int guardar();
00068 void P_Pause();
00069 
00070 int main()
00071 {
00072     mypwmX.period_ms(20);
00073     mypwmY.period_ms(20);
00074     mypwmZ.period_ms(20);
00075     uint8_t letra,subletra;
00076     
00077     Vertex2D(0,0);        
00078     SetZ(NODRAW);
00079     myled = 1;
00080     wait(2);
00081     pc.baud(9600);              
00082     pc.format(8,SerialBase::None,1); 
00083     while(1) 
00084     {
00085         pc.printf("ingrese comando \n");
00086         letra=pc.getc(); //PRIMER COMANDO FF O FE    
00087             if(letra==CM_EJECUTAR)
00088             {
00089                 if(mem_head!=mem_tail) // SI ES DIFERENTE ES PORQUE HAY DATOS EN LA MEMORIA
00090                 { 
00091                     pc.attach(&P_Pause,Serial::RxIrq);
00092                     ejecutar();
00093                     pc.attach(0,Serial::RxIrq);
00094                 }
00095                 else{
00096                     pc.printf("La memoria se encuentra vacia \n");
00097                 } 
00098                 }
00099             else if(letra==CM_GUARDAR){
00100                 if(guardar()==0) // LA FUNCION GUARDAR RETORNO UN 0, TODO SE ESTA GUARDANDO CORRECTAMENTE
00101                 {
00102                     pc.printf("el programa se guardo correctamente \n");
00103                 }
00104                 else
00105                 {
00106                     pc.printf("el programa no se pudo guardar... vuelva a intentar \n");
00107                 }
00108                 }
00109             else{ pc.printf("error de comando \n");}
00110             
00111         }
00112         
00113 }
00114 int mm2pulse(float vmm)
00115 {
00116   if (vmm < MAXPOS) // SI NO SOBREPASA LOS 50 mm
00117     return int (vmm*(MAXPULSE-MINPULSE)/(MAXPOS)) +MINPULSE; // SE REALIZA ESTA OPERACION DE CONVVERCION
00118   return MAXPULSE; // SINO, RETORNA MAXPULSE
00119 }
00120 void SetZ(uint8_t _z)
00121 {
00122     z=_z;
00123     int PULSEZ=mm2pulse(z); // SE HACE PASO DE MILIMETROS A PULSO Y SE GUARDA EN Z
00124     mypwmZ.pulsewidth_us(PULSEZ); 
00125 }
00126 void Vertex2D(uint8_t _x,uint8_t _y)
00127 {
00128     x=_x;
00129     y=_y;
00130     int PULSEX=mm2pulse(x);
00131     int PULSEY=mm2pulse(50-y);
00132     mypwmX.pulsewidth_us(PULSEX);
00133     mypwmY.pulsewidth_us(PULSEY);   
00134 }
00135 void ejecutar()
00136 {                                  
00137     pc.printf("el programa se esta ejecutando \n");
00138     uint8_t a=0,b=0,c=0,d=0; // SE CREAN 4 VARIABLES DE 8 BITS
00139     uint32_t l,l1,l2,l3; // SE CREAN 4 VARIABLES DE 32 BITS
00140     MEM_TYPE val; //
00141     mem_get(&val); // LLAMA A LA MEMORIA Y LO CARGA A VAL
00142     l1=(val&0xff00)>>8; // OBTIENE EL TERCER DATO
00143     l2=(val&0xff0000)>>16; // OBTIENE EL SEGUNDO VALOR QUE INGRESAMOS
00144     l3=(val&0xff000000)>>24; // OBTIENE EL PRIMER DATO QUE INGRESAMOS
00145     l=val&0xff; // OBTIENE EL ULTIMO DATO QUE INGRESAMOS
00146     int noy;
00147     while(c!=CM_END) // SI EL TERCER VALOR ES FA
00148     {
00149         a=l3;
00150         pc.putc(a);
00151         b=l2;
00152         pc.putc(b);
00153         c=l1;
00154         pc.putc(c);
00155         d=l;
00156         pc.putc(d);
00157         if(a==CM_SERVO)
00158         {
00159             Vertex2D(b,c);           
00160         }
00161         else if(a==CM_MOTORSTEP)
00162         {
00163             xnew=b;
00164             ynew=c;
00165             xmov=xnew-xant;
00166             ymov=ynew-yant;
00167             xant=xnew;
00168             yant=ynew;
00169             if(xmov==0)
00170             {
00171                 if(ymov>0)                                       //los valores de 1024 es estandar .step(#pasos,sentido) donde el sentido 0 es girar izquierda, 1 s girar derecha, 2 es avanzar y 3 es retroceder 
00172                 {
00173                     noy=STAN*abs(ymov);
00174                     motorST.step(noy,FORWARD);                        //se mueve para adelante
00175                 }
00176                 else if(ymov<0)
00177                 {
00178                     noy=STAN*abs(ymov);
00179                     motorST.step(noy,BEHIND);                        //se mueve para atras
00180                 }
00181             }
00182             else if(xmov>0)
00183             {
00184                  motorST.step(STAN,RIGHT);
00185                  noy=STAN*abs(xmov);
00186                  motorST.step(noy,FORWARD);
00187                  motorST.step(STAN,LEFT);                           //gira a la derecha y se mueve los cuadrantes y vuelve a girar a la misma posicion
00188             }  
00189             else if(xmov<0)
00190             {
00191                  motorST.step(STAN,LEFT);
00192                  noy=STAN*abs(xmov);
00193                  motorST.step(noy,FORWARD);
00194                  motorST.step(STAN,RIGHT);                            //gira a la izquierda y se mueve los cuadrantes y vuelve a girar a la misma posicion
00195             }
00196             if(ymov==0){}                               //ya esta en esa posicion
00197             else if(xmov>0)
00198             {
00199                 noy=STAN*abs(ymov);
00200                 motorST.step(noy,FORWARD);                            //se mueve a esa posicion avanzando
00201             }
00202             else if(xmov<0)
00203             {
00204                 noy=STAN*abs(ymov);
00205                 motorST.step(noy,BEHIND);                             //se mueve a esa posicion retrocediendo
00206             }
00207         }
00208         else if(c==CM_SDRAW)
00209         {
00210             SetZ(DRAW);              
00211         }
00212         else if(c==CM_SN0DRAW)
00213         {
00214             SetZ(NODRAW);            
00215         }
00216         mem_get(&val); // LO QUE HAY EN LA MEMORIA SE GUERDE EN VAL
00217         l1=(val&0xff00)>>8; // LLAMA A LA MEMORIA Y SE VUELVE A HACER LA SEPARACION
00218         l2=(val&0xff0000)>>16;
00219         l3=(val&0xff000000)>>24;
00220         l=val&0xff;
00221         wait(1);
00222     }
00223     pc.printf("el programa se termino de ejecutar \n");
00224     mem_tail=0; // DEVUELVE LA COLA POR SI SE QUIERE VOLVER A REPRODUCIR 
00225 }
00226 int guardar()
00227 {
00228     mem_free();// LIBERA MEMORIA
00229     uint32_t l=0;// LIBERA MEMORIA
00230     uint8_t sl; // LA QUE VA SER RECIBIDA POR LA COMUNICACION SERIAL
00231     pc.printf("ingrese sus comandos y coordenadas \n");
00232     sl=pc.getc(); // VA A COGER LO DEL PUERTO SERIAL Y 
00233     pc.putc(sl);
00234     while(sl!=CM_END) // SI ES DIFERENTE A FA
00235     {
00236         switch(sl)
00237         {
00238             case CM_SERVO:
00239                 l=l+sl<<8; // SE LE ASIGNA LA VARIABLE DE 8 BITS Y SE CORRE LA POSICION 8 BITS
00240                 sl=pc.getc(); // SE GUARDA EN SL EL NUEVO COMANDO
00241                 l=l+sl;
00242                 pc.putc(l);
00243                 l=l<<8;
00244                 sl=pc.getc();
00245                 l=l+sl;
00246                 pc.putc(l);
00247                 l=l<<8;
00248                 sl=pc.getc();
00249                 if(sl!=CM_STOP) // SI A LO QUE LLEO FUE A F0
00250                 {
00251                     pc.printf("comando End no definido \n");
00252                     return 1;
00253                 }
00254                 l=l+sl;
00255                 pc.putc(l);
00256                 mem_put(l); // SE GUARDA L , QUE YA ES UN VALOR DE 32 BITS
00257             break;
00258             case CM_SDRAW:
00259                 l=l+sl<<8;
00260                 sl=pc.getc();
00261                 if(sl!=CM_STOP)
00262                 {
00263                     pc.printf("comando End no definido \n");
00264                     return 1;
00265                 }
00266                 l=l+sl;
00267                 mem_put(l);
00268             break;
00269             case CM_SN0DRAW:
00270                 l=l+sl<<8;
00271                 sl=pc.getc();
00272                 if(sl!=CM_STOP)
00273                 {
00274                     pc.printf("comando End no definido \n");
00275                     return 1;
00276                 }
00277                 l=l+sl;
00278                 mem_put(l);
00279             break; 
00280             case CM_MOTORSTEP:
00281                 l=l+sl<<8; // SE LE ASIGNA LA VARIABLE DE 8 BITS Y SE CORRE LA POSICION 8 BITS
00282                 sl=pc.getc(); // SE GUARDA EN SL EL NUEVO COMANDO
00283                 l=l+sl;
00284                 pc.putc(l);
00285                 l=l<<8;
00286                 sl=pc.getc();
00287                 l=l+sl;
00288                 pc.putc(l);
00289                 l=l<<8;
00290                 sl=pc.getc();
00291                 if(sl!=CM_STOP) // SI A LO QUE LLEO FUE A F0
00292                 {
00293                     pc.printf("comando End no definido \n");
00294                     return 1;
00295                 }
00296                 l=l+sl;
00297                 pc.putc(l);
00298                 mem_put(l);
00299             break;
00300         }
00301         l=0;
00302         sl=pc.getc();   
00303     }
00304     sl=pc.getc();
00305     pc.printf("el programa termino de guardar \n");
00306     mem_put(0xfaf0);
00307     return 0;
00308 }
00309 
00310 void P_Pause()
00311 {
00312     int g=pc.getc();
00313     if(g==PAUSE)
00314     {
00315         pc.attach(0,Serial::RxIrq);
00316         pc.printf("interrupcion \n");
00317         while(pc.getc()!=PLAY){}
00318         pc.attach(&P_Pause,Serial::RxIrq);
00319     }
00320     return;
00321 }
00322 
00323 
00324 
00325 
00326