main.cpp

00001 #include "mbed.h"
00002 #include "max7219.h" // incluir librerias.
00003 #include "vision.h"
00004 
00005  
00006 int motorSpeed; // variable para la velocidad del motor
00007 
00008 
00009 #define DEBUG 1
00010  
00011 //*****************************************************************************
00012 //  COMANDO  MOVER MOTOR
00013 //  |POS 1|POS 2|POS 3|POS 4| POS 5|
00014 //  |  <  | #M  |  ,  | #°G |  >   |
00015 //
00016 // #M  -> indica el motor que se va a mover
00017 // #°G -> indica los grados a mover del  servomotor 
00018 // <,> -> inicio, separdor  y fin de comando 
00019 //  el inicio de comando no se almacena en el buffer
00020 //*****************************************************************************
00021  
00022 // VARIABLES PARA DEFINIR  EL  COMMANDO
00023 #define BUFF_SIZE 4
00024 #define COMM_NUM_MOTOR 0
00025 #define COMM_SEPARADOR 1
00026 #define COMM_GRADOS_MOTOR  2
00027  
00028  
00029 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0};
00030  
00031  
00032 void print_num(uint8_t val)
00033  
00034 {
00035 if (val <10)
00036         command.putc(val+0x30);
00037     else 
00038         command.putc(val-9+0x40);
00039         
00040 }
00041 void print_bin2hex (uint8_t val)
00042 {
00043     command.printf(" 0x");
00044     print_num(val>>4);
00045     print_num(val&0x0f);
00046         
00047         
00048 }
00049  
00050 // TODO : TIMEOUT UART SERIAL
00051 void Read_command()
00052 {
00053     for (uint8_t i=0; i<BUFF_SIZE;i++)
00054         buffer_command[i]=command.getc();
00055     
00056 }
00057  
00058 void echo_command()
00059 {
00060     for (uint8_t i=0; i<BUFF_SIZE;i++)
00061         print_bin2hex(buffer_command[i]);
00062       
00063 }
00064  
00065  
00066 uint8_t check_command()
00067 {       if(buffer_command[BUFF_SIZE-1]== '>' && buffer_command[COMM_NUM_MOTOR]==03){  // seleccion del motor paso a paso
00068        #if DEBUG
00069                 command.printf("\nMover Motor:");
00070                 print_bin2hex(buffer_command[COMM_NUM_MOTOR]);
00071                 command.printf(" -> ");
00072                 print_bin2hex(buffer_command[COMM_GRADOS_MOTOR]);
00073                 command.printf(" pasos ");  
00074                 if(buffer_command[COMM_SEPARADOR]==00){ // seleccion direccion del motor paso a paso (derecha)
00075                 buffer_command[COMM_SEPARADOR]=0;
00076                 command.printf(" a la derecha \n"); 
00077                 Print_pantalla_tabla(m4right);  
00078                 stepper(buffer_command[COMM_GRADOS_MOTOR],buffer_command[COMM_SEPARADOR]);
00079                 
00080                 #endif
00081                 return 0;
00082                 }
00083                  else if(buffer_command[COMM_SEPARADOR]==01){// seleccion direccion del motor paso a paso (izquierda)
00084                  #if DEBUG
00085                  buffer_command[COMM_SEPARADOR]=1;
00086                  command.printf(" a la izquierda \n"); 
00087                  Print_pantalla_tabla(m4left); 
00088                  stepper(buffer_command[COMM_GRADOS_MOTOR],buffer_command[COMM_SEPARADOR]);
00089                  
00090                  
00091                                   #endif
00092                  return 0;
00093                  }
00094                  #if DEBUG
00095                  command.printf("\n ERROR COMANDO -> "); // error si la direccion de entrada no es ni derecha ni izquierda
00096                  echo_command();
00097                  #endif
00098                  return 0;    
00099                     
00100        }
00101        
00102    else if (buffer_command[BUFF_SIZE-1]== '>' && buffer_command[COMM_SEPARADOR]==','){ // seleccion solo servo motores, verifica que la entrada sea correcta
00103         
00104             #if DEBUG
00105                 command.printf("\nMover Motor:");
00106                 print_bin2hex(buffer_command[COMM_NUM_MOTOR]);
00107                 command.printf(" -> ");
00108                 print_bin2hex(buffer_command[COMM_GRADOS_MOTOR]);
00109                 command.printf(" grados \n");  
00110             #endif
00111             return 1;
00112         }
00113         #if DEBUG
00114             command.printf("\n ERROR COMANDO -> "); // error si los simbolos de entrada son incorrectos
00115             echo_command();
00116         #endif
00117         return 0;        
00118         
00119      
00120 }
00121 
00122 
00123 
00124   
00125     
00126 
00127 
00128 void command_motor(uint8_t nm,uint8_t grados)
00129 {
00130     
00131                 led=1;  
00132                 wait(1);
00133                 led=0; 
00134              unsigned int k;
00135   
00136 k=((grados*9)+550);     // formula para los grados de los servomotores
00137                 
00138                               
00139                 
00140                 
00141 switch(nm){ // seleccion del servo a mover
00142 case 00:
00143 if(grados==0x5a){                       //90 grados
00144 
00145    Print_pantalla_tabla(m1up);
00146    wait(1);
00147 
00148     mipwm.pulsewidth_us(k);
00149        wait(1); 
00150 }
00151 else if (grados<0x5a){  // visualizacion cuando es menor que 90
00152    Print_pantalla_tabla(m1right);
00153    wait(1); 
00154     mipwm.pulsewidth_us(k);
00155        wait(1);  
00156    }
00157     else{                  // visualizacion cuando es mayor que 90
00158         Print_pantalla_tabla(m1left);
00159    wait(1);  
00160     mipwm.pulsewidth_us(k);
00161        wait(1);  
00162    }
00163        
00164       mipwm.pulsewidth_us(k);
00165        wait(1); 
00166 break;
00167 case 01:if(grados==0x5a){                       //90 grados
00168 
00169    Print_pantalla_tabla(m2up);
00170    wait(1);  
00171 }
00172 else if (grados<0x5a){    // visualizacion cuando es menor que 90
00173    Print_pantalla_tabla(m2right);
00174    wait(1);   
00175    }
00176     else{// visualizacion cuando es mayor que 90
00177         Print_pantalla_tabla(m2left);
00178    wait(1);   
00179    } 
00180      mipwm2.pulsewidth_us(k);
00181        wait(1); 
00182 break;
00183 case 02:
00184 if(grados==0x5a){                       //90 grados
00185 
00186    Print_pantalla_tabla(m3up);
00187    wait(1);  
00188 }
00189 else if (grados<0x5a){ // visualizacion cuando es menor que 90
00190    Print_pantalla_tabla(m3right);
00191    wait(1);   
00192    }
00193     else{               // visualizacion cuando es mayor que 90
00194         Print_pantalla_tabla(m3left);
00195    wait(1);   
00196    }  
00197      mipwm3.pulsewidth_us(k);
00198        wait(1);  
00199 break;
00200 default:
00201 break;
00202 }
00203                 
00204                 
00205     
00206 }
00207 
00208 
00209 
00210 int main() {
00211      Print_pantalla_tabla(clear);    // limpiar matriz
00212     #if DEBUG
00213     command.printf("inicio con debug\n");
00214     #else
00215     command.printf("inicio sin debug\n");
00216     #endif
00217     uint8_t val;
00218     while(1){
00219         val=command.getc();
00220         if (val== '<'){
00221             Read_command();
00222             if (check_command()){
00223               command_motor(buffer_command[COMM_NUM_MOTOR],buffer_command[COMM_GRADOS_MOTOR]);
00224             }
00225         }
00226         
00227     }
00228 }
00229  
00230  
00231 
00232 
00233 void Print_pantalla_tabla(unsigned char *pValor){
00234    int i;                                                   //funcion de puntero, impresión de pantalla
00235   for (i =0;i<8;i++)
00236    pantalla.write_digit(1,i+1,pValor[i]);
00237 
00238 }
00239 
00240 
00241 
00242 
00243 
00244 
00245 void anticlockwise() { // rotacion del motor paso a paso sentido antihorario
00246     for (int i = 0; i < 8; i++) {
00247 
00248         switch (i) { // activar(1) o desactivar(0) las bobinas
00249             case 0: {
00250                 _A0=0;
00251                 _A1=0;
00252                 _A2=0;
00253                 _A3=1;
00254             }
00255             break;
00256             case 1: {
00257                 _A0=0;
00258                 _A1=0;
00259                 _A2=1;
00260                 _A3=1;
00261             }
00262             break;
00263             case 2: {
00264                 _A0=0;
00265                 _A1=0;
00266                 _A2=1;
00267                 _A3=0;
00268             }
00269             break;
00270             case 3: {
00271                 _A0=0;
00272                 _A1=1;
00273                 _A2=1;
00274                 _A3=0;
00275             }
00276             break;
00277             case 4: {
00278                 _A0=0;
00279                 _A1=1;
00280                 _A2=0;
00281                 _A3=0;
00282             }
00283             break;
00284             case 5: {
00285                 _A0=1;
00286                 _A1=1;
00287                 _A2=0;
00288                 _A3=0;
00289             }
00290             break;
00291             case 6: {
00292                 _A0=1;
00293                 _A1=0;
00294                 _A2=0;
00295                 _A3=0;
00296             }
00297             break;
00298             case 7: {
00299                 _A0=1;
00300                 _A1=0;
00301                 _A2=0;
00302                 _A3=1;
00303             }
00304             break;
00305         }
00306 
00307 
00308         wait_us(motorSpeed); // velocidad 
00309     }
00310 }
00311 
00312 void clockwise() { // rotacion del motor paso a paso sentido horario
00313     for (int i = 7; i >= 0; i--) {
00314 
00315         switch (i) {                // activar(1) o desactivar(0) las bobinas
00316             case 0: {
00317                 _A0=0;
00318                 _A1=0;
00319                 _A2=0;
00320                 _A3=1;
00321             }
00322             break;
00323             case 1: {
00324                 _A0=0;
00325                 _A1=0;
00326                 _A2=1;
00327                 _A3=1;
00328             }
00329             break;
00330             case 2: {
00331                 _A0=0;
00332                 _A1=0;
00333                 _A2=1;
00334                 _A3=0;
00335             }
00336             break;
00337             case 3: {
00338                 _A0=0;
00339                 _A1=1;
00340                 _A2=1;
00341                 _A3=0;
00342             }
00343             break;
00344             case 4: {
00345                 _A0=0;
00346                 _A1=1;
00347                 _A2=0;
00348                 _A3=0;
00349             }
00350             break;
00351             case 5: {
00352                 _A0=1;
00353                 _A1=1;
00354                 _A2=0;
00355                 _A3=0;
00356             }
00357             break;
00358             case 6: {
00359                 _A0=1;
00360                 _A1=0;
00361                 _A2=0;
00362                 _A3=0;
00363             }
00364             break;
00365             case 7: {
00366                 _A0=1;
00367                 _A1=0;
00368                 _A2=0;
00369                 _A3=1;
00370             }
00371             break;
00372         }
00373 
00374 
00375         wait_us(motorSpeed); // velocidad de giro
00376     }
00377 }
00378 
00379 
00380 
00381 void stepper(uint8_t num_steps, uint8_t direction) {// funcion para el numero de pasos direccion(0-derecha, 1-izquierda)
00382     int count=0; //inicializar conteo
00383     motorSpeed=1200; //velocidad de giro
00384     if (direction==0) // giro hacia la derecha
00385         do {
00386             clockwise();
00387             count++;
00388         } while (count<num_steps); // definicion de pasos 
00389     else if (direction==1)// giro antihorario
00390         do {
00391             anticlockwise();
00392             count++;
00393         } while (count<num_steps);// definicion de pasos
00394 
00395 }