main.cpp

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