servo visual

Dependencies:   MAX7219 mbed

Realizamos mediante comunicacion serial, ingresando el comando para los servomotores:

COMANDO MOVER MOTOR

POS 1POS 2POS 3POS 4POS 5
<#M,#°G>
  1. M -> indica el motor que se va a mover (00,01,02)
  2. °G -> indica los grados a mover del servomotor <,> -> inicio, separdor y fin de comando el inicio de comando no se almacena en el buffer

si el comando no tiene correctamente ingresado los simbolos ',' y '>', nos dara un error.

comando para el paso a paso:

COMANDO MOVER PASO A PASO

POS 1POS 2POS 3POS 4POS 5
<#MDIR#PASOS>
  1. M -> indica el motor que se va a mover ( 03)
  2. PASOS -> indica los pasos a mover del motor paso a paso DIR -> indica la direccion en la que girara el motor (00-derecha, 01- izquierda) < > ->inicio, separdor y fin de comando

el inicio de comando no se almacena en el buffer.

En la visualizacion de la matriz se observara el numero del motor que se esta moviendo y su direccion indicada con una flecha

<- hacia la izquierda cuando el angulo de los servos sea mayor que 90, y el sentido del paso a paso sea este. ^ hacia arriba cuando el angulo de los servos sea 90 -> hacia la derecha cuando el angulo de los servos sea menor que 90, y el sentido del paso a paso sea este.

1- servomotor uno (00) 2-servomotor dos (01) 3-servomotor tres (02) 4- paso a paso (03)

Files at this revision

API Documentation at this revision

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

main.cpp Show annotated file Show diff for this revision Revisions of this file
vision.h Show annotated file Show diff for this revision Revisions of this file
diff -r 65caec68ab34 -r 908a41a33553 main.cpp
--- 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
+
+}
diff -r 65caec68ab34 -r 908a41a33553 vision.h
--- /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
+