Revision 1:908a41a33553, committed 2017-04-27

Comitter:

Paxtel

Date:

Thu Apr 27 21:39:55 2017 +0000

Parent:

0:65caec68ab34

Commit message:

funcionamiento de 3 servomoteores y un paso a paso , con vizualizacion sentido de giro en una matriz 8x8

Changed in this revision

--- a/main.cpp	Mon Apr 24 22:40:04 2017 +0000
+++ b/main.cpp	Thu Apr 27 21:39:55 2017 +0000
@@ -1,79 +1,395 @@
 #include "mbed.h"
-#include "max7219.h" //libreria
+#include "max7219.h" // incluir librerias.
+#include "vision.h"
+
+ 
+int motorSpeed; // variable para la velocidad del motor
+
+
+#define DEBUG 1
+ 
+//*****************************************************************************
+//  COMANDO  MOVER MOTOR
+//  |POS 1|POS 2|POS 3|POS 4| POS 5|
+//  |  <  | #M  |  ,  | #°G |  >   |
+//
+// #M  -> indica el motor que se va a mover
+// #°G -> indica los grados a mover del  servomotor 
+// <,> -> inicio, separdor  y fin de comando 
+//  el inicio de comando no se almacena en el buffer
+//*****************************************************************************
+ 
+// VARIABLES PARA DEFINIR  EL  COMMANDO
+#define BUFF_SIZE 4
+#define COMM_NUM_MOTOR 0
+#define COMM_SEPARADOR 1
+#define COMM_GRADOS_MOTOR  2
+ 
+ 
+uint8_t buffer_command[BUFF_SIZE]={0,0,0,0};
+ 
+ 
+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);
+        
+        
+}
+ 
+// TODO : TIMEOUT UART SERIAL
+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]== '>' && buffer_command[COMM_NUM_MOTOR]==03){  // seleccion del motor paso a paso
+       #if DEBUG
+                command.printf("\nMover Motor:");
+                print_bin2hex(buffer_command[COMM_NUM_MOTOR]);
+                command.printf(" -> ");
+                print_bin2hex(buffer_command[COMM_GRADOS_MOTOR]);
+                command.printf(" pasos ");  
+                if(buffer_command[COMM_SEPARADOR]==00){ // seleccion direccion del motor paso a paso (derecha)
+                buffer_command[COMM_SEPARADOR]=0;
+                command.printf(" a la derecha \n"); 
+                Print_pantalla_tabla(m4right);  
+                stepper(buffer_command[COMM_GRADOS_MOTOR],buffer_command[COMM_SEPARADOR]);
+                
+                #endif
+                return 0;
+                }
+                 else if(buffer_command[COMM_SEPARADOR]==01){// seleccion direccion del motor paso a paso (izquierda)
+                 #if DEBUG
+                 buffer_command[COMM_SEPARADOR]=1;
+                 command.printf(" a la izquierda \n"); 
+                 Print_pantalla_tabla(m4left); 
+                 stepper(buffer_command[COMM_GRADOS_MOTOR],buffer_command[COMM_SEPARADOR]);
+                 
+                 
+                                  #endif
+                 return 0;
+                 }
+                 #if DEBUG
+                 command.printf("\n ERROR COMANDO -> "); // error si la direccion de entrada no es ni derecha ni izquierda
+                 echo_command();
+                 #endif
+                 return 0;    
+                    
+       }
+       
+   else if (buffer_command[BUFF_SIZE-1]== '>' && buffer_command[COMM_SEPARADOR]==','){ // seleccion solo servo motores, verifica que la entrada sea correcta
+        
+            #if DEBUG
+                command.printf("\nMover Motor:");
+                print_bin2hex(buffer_command[COMM_NUM_MOTOR]);
+                command.printf(" -> ");
+                print_bin2hex(buffer_command[COMM_GRADOS_MOTOR]);
+                command.printf(" grados \n");  
+            #endif
+            return 1;
+        }
+        #if DEBUG
+            command.printf("\n ERROR COMANDO -> "); // error si los simbolos de entrada son incorrectos
+            echo_command();
+        #endif
+        return 0;        
+        
+     
+}
 
 
 
-unsigned char arrowup[]= {0xaa,0x55,0xaa,0x55,0xaa,0x55,0xaa,0x55};  //secuencia 1
-unsigned char arrowleft[]= {0x55,0xaa,0x55,0xaa,0x55,0xaa,0x55,0xaa};  //secuencia 2
-unsigned char arrowright[]= {0x01,0x03,0x07,0x0f,0x1f,0x3f,0x7f,0xff};  //secuencia 3
-unsigned char motor1[]= {0x01,0x03,0x07,0x0f,0x1f,0x3f,0x7f,0xff};  //secuencia 4
-unsigned char motor2[]= {0xaa,0x55,0xaa,0x55,0xaa,0x55,0xaa,0x55};  //secuencia 5
-unsigned char motor3[]= {0x55,0xaa,0x55,0xaa,0x55,0xaa,0x55,0xaa};  //secuencia 6
-unsigned char motor4[]= {0x01,0x03,0x07,0x0f,0x1f,0x3f,0x7f,0xff};  //secuencia 7
+  
+    
+
+
+void command_motor(uint8_t nm,uint8_t grados)
+{
+    
+                led=1;  
+                wait(1);
+                led=0; 
+             unsigned int k;
+  
+k=((grados*9)+550);     // formula para los grados de los servomotores
+                
+                              
+                
+                
+switch(nm){ // seleccion del servo a mover
+case 00:
+if(grados==0x5a){                       //90 grados
+
+   Print_pantalla_tabla(m1up);
+   wait(1);
+
+    mipwm.pulsewidth_us(k);
+       wait(1); 
+}
+else if (grados<0x5a){  // visualizacion cuando es menor que 90
+   Print_pantalla_tabla(m1right);
+   wait(1); 
+    mipwm.pulsewidth_us(k);
+       wait(1);  
+   }
+    else{                  // visualizacion cuando es mayor que 90
+        Print_pantalla_tabla(m1left);
+   wait(1);  
+    mipwm.pulsewidth_us(k);
+       wait(1);  
+   }
+       
+      mipwm.pulsewidth_us(k);
+       wait(1); 
+break;
+case 01:if(grados==0x5a){                       //90 grados
 
-void Print_pantalla_tabla(unsigned char *pValor); //variable global
-void print_punto();//variable global
+   Print_pantalla_tabla(m2up);
+   wait(1);  
+}
+else if (grados<0x5a){    // visualizacion cuando es menor que 90
+   Print_pantalla_tabla(m2right);
+   wait(1);   
+   }
+    else{// visualizacion cuando es mayor que 90
+        Print_pantalla_tabla(m2left);
+   wait(1);   
+   } 
+     mipwm2.pulsewidth_us(k);
+       wait(1); 
+break;
+case 02:
+if(grados==0x5a){                       //90 grados
+
+   Print_pantalla_tabla(m3up);
+   wait(1);  
+}
+else if (grados<0x5a){ // visualizacion cuando es menor que 90
+   Print_pantalla_tabla(m3right);
+   wait(1);   
+   }
+    else{               // visualizacion cuando es mayor que 90
+        Print_pantalla_tabla(m3left);
+   wait(1);   
+   }  
+     mipwm3.pulsewidth_us(k);
+       wait(1);  
+break;
+default:
+break;
+}
+                
+                
+    
+}
+
+
+
+int main() {
+     Print_pantalla_tabla(clear);    // limpiar matriz
+    #if DEBUG
+    command.printf("inicio con debug\n");
+    #else
+    command.printf("inicio sin debug\n");
+    #endif
+    uint8_t val;
+    while(1){
+        val=command.getc();
+        if (val== '<'){
+            Read_command();
+            if (check_command()){
+              command_motor(buffer_command[COMM_NUM_MOTOR],buffer_command[COMM_GRADOS_MOTOR]);
+            }
+        }
+        
+    }
+}
+ 
+ 
+
+
+void Print_pantalla_tabla(unsigned char *pValor){
+   int i;                                                   //funcion de puntero, impresión de pantalla
+  for (i =0;i<8;i++)
+   pantalla.write_digit(1,i+1,pValor[i]);
+
+}
 
 
 
 
 
-Max7219 pantalla(PB_15, PB_14, PB_13, PB_12); //pines
+
+void anticlockwise() { // rotacion del motor paso a paso sentido antihorario
+    for (int i = 0; i < 8; i++) {
+
+        switch (i) { // activar(1) o desactivar(0) las bobinas
+            case 0: {
+                _A0=0;
+                _A1=0;
+                _A2=0;
+                _A3=1;
+            }
+            break;
+            case 1: {
+                _A0=0;
+                _A1=0;
+                _A2=1;
+                _A3=1;
+            }
+            break;
+            case 2: {
+                _A0=0;
+                _A1=0;
+                _A2=1;
+                _A3=0;
+            }
+            break;
+            case 3: {
+                _A0=0;
+                _A1=1;
+                _A2=1;
+                _A3=0;
+            }
+            break;
+            case 4: {
+                _A0=0;
+                _A1=1;
+                _A2=0;
+                _A3=0;
+            }
+            break;
+            case 5: {
+                _A0=1;
+                _A1=1;
+                _A2=0;
+                _A3=0;
+            }
+            break;
+            case 6: {
+                _A0=1;
+                _A1=0;
+                _A2=0;
+                _A3=0;
+            }
+            break;
+            case 7: {
+                _A0=1;
+                _A1=0;
+                _A2=0;
+                _A3=1;
+            }
+            break;
+        }
+
 
- int main()
-{ 
-   max7219_configuration_t cfg = {
-        .device_number = 1,
-        .decode_mode = 0,                              //configuracion del max7219
-        .intensity = Max7219::MAX7219_INTENSITY_5,
-        .scan_limit = Max7219::MAX7219_SCAN_8
-    };
-    
-    pantalla.init_device(cfg);
-    pantalla.enable_device(1);
-    pantalla.set_display_test();    //testeo de la matriz
-    wait(1);
-    pantalla.clear_display_test();
-        
-   while(1){
-   Print_pantalla_tabla(led1);
-   wait(1);
-   Print_pantalla_tabla(led2);
-   wait(1);                                      //ciclo de visualizaciòn
-   Print_pantalla_tabla(led3);
-   wait(1);
-   print_punto();  
-   }
+        wait_us(motorSpeed); // velocidad 
+    }
+}
+
+void clockwise() { // rotacion del motor paso a paso sentido horario
+    for (int i = 7; i >= 0; i--) {
 
+        switch (i) {                // activar(1) o desactivar(0) las bobinas
+            case 0: {
+                _A0=0;
+                _A1=0;
+                _A2=0;
+                _A3=1;
+            }
+            break;
+            case 1: {
+                _A0=0;
+                _A1=0;
+                _A2=1;
+                _A3=1;
+            }
+            break;
+            case 2: {
+                _A0=0;
+                _A1=0;
+                _A2=1;
+                _A3=0;
+            }
+            break;
+            case 3: {
+                _A0=0;
+                _A1=1;
+                _A2=1;
+                _A3=0;
+            }
+            break;
+            case 4: {
+                _A0=0;
+                _A1=1;
+                _A2=0;
+                _A3=0;
+            }
+            break;
+            case 5: {
+                _A0=1;
+                _A1=1;
+                _A2=0;
+                _A3=0;
+            }
+            break;
+            case 6: {
+                _A0=1;
+                _A1=0;
+                _A2=0;
+                _A3=0;
+            }
+            break;
+            case 7: {
+                _A0=1;
+                _A1=0;
+                _A2=0;
+                _A3=1;
+            }
+            break;
+        }
+
+
+        wait_us(motorSpeed); // velocidad de giro
+    }
 }
 
 
-void Print_pantalla_tabla(unsigned char *pValor)
-{
-int i;                                                   //funcion de puntero, impresión de pantalla
-  for (i =0;i<8;i++)
-   pantalla.write_digit(1,i+1,pValor[i]);
-   
-}
-
 
-void print_punto()
-{
-           unsigned char personal[8]= {0x0,0x0,0,0,0,0,0,0};  
-   unsigned char filas[8]= {1,3,7,0x0f,0x1f,0x3f,0x7f,0xff};  
-        int i,j;
-        
-        for (i=0;i<8;i++)                                     // secuencia de puntos
-            personal[i]= 0;  
-        for (j=0;j<8;j++)
-        for (i=0;i<8;i++){
-            personal[i]= filas[j];  
-            Print_pantalla_tabla(personal);
-            wait(0.2);
-        
-     
-        }
-    }
- 
- 
- 
+void stepper(uint8_t num_steps, uint8_t direction) {// funcion para el numero de pasos direccion(0-derecha, 1-izquierda)
+    int count=0; //inicializar conteo
+    motorSpeed=1200; //velocidad de giro
+    if (direction==0) // giro hacia la derecha
+        do {
+            clockwise();
+            count++;
+        } while (count<num_steps); // definicion de pasos 
+    else if (direction==1)// giro antihorario
+        do {
+            anticlockwise();
+            count++;
+        } while (count<num_steps);// definicion de pasos
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/vision.h	Thu Apr 27 21:39:55 2017 +0000
@@ -0,0 +1,44 @@
+
+
+
+
+DigitalOut _A0(PA_9); // pines motor paso a paso
+DigitalOut _A1(PC_7);
+DigitalOut _A2(PB_6);
+DigitalOut _A3(PA_7 );
+
+
+PwmOut mipwm(PA_0); // pin servomotor1
+PwmOut mipwm2(PA_1);// pin servomotor2
+PwmOut mipwm3(PB_0);// pin servomotor3
+
+Serial command(USBTX, USBRX); // definicion serial
+
+Max7219 pantalla(PB_15, PB_14, PB_13, PB_12); //pines matriz
+
+
+DigitalOut led(LED1); // pin led1
+
+
+
+
+void stepper(uint8_t num_steps, uint8_t direction); // funcion que define el numero de pasos
+void clockwise() ; // funcion para giro en sentido horario
+
+void anticlockwise();  // funcion para giro en sentido antihorario
+void Print_pantalla_tabla(unsigned char *pValor); //funcion imprimir imagen en la matriz
+
+unsigned char clear[]= {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; // limpiar matriz
+unsigned char m1up[]= {0x00,0x04,0x06,0x27,0xf7,0x06,0x04,0x00};  //secuencia 1
+unsigned char m1left[]= {0x04,0x0e,0x1f,0x00,0x00,0x20,0xf0,0x00};  //secuencia 2
+unsigned char m1right[]= {0x00,0x20,0xf0,0x00,0x00,0x1f,0x0e,0x04};  //secuencia 3
+unsigned char m2up[]= {0x00,0x04,0x06,0xf7,0x07,0xf6,0x04,0x00};  //secuencia 4
+unsigned char m2left[]= {0x04,0x0e,0x1f,0x00,0xf0,0x00,0xf0,0x00};  //secuencia 5
+unsigned char m2right[]= {0x00,0xf0,0x00,0xf0,0x00,0x1f,0x0e,0x04};  //secuencia 6
+unsigned char m3up[]= {0x00,0x04,0xf6,0x07,0xf7,0x06,0xf4,0x00};  //secuencia 7
+unsigned char m3left[]= {0x04,0x0e,0x1f,0xf0,0x00,0xf0,0x00,0xf0};  //secuencia 8
+unsigned char m3right[]= {0xf0,0x00,0xf0,0x00,0xf0,0x1f,0x0e,0x04};  //secuencia 9
+unsigned char m4up[]= {0x00,0x04,0x06,0x77,0x47,0xf6,0x04,0x00};  //secuencia 10
+unsigned char m4left[]= {0x04,0x0e,0x1f,0x00,0x70,0x40,0xf0,0x00};  //secuencia 11
+unsigned char m4right[]= {0x00,0x70,0x40,0xf0,0x00,0x1f,0x0e,0x04};  //secuencia 12
+