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)

main.cpp

Committer:
Paxtel
Date:
2017-04-27
Revision:
1:908a41a33553
Parent:
0:65caec68ab34

File content as of revision 1:908a41a33553:

#include "mbed.h"
#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;        
        
     
}



  
    


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

   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]);

}






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;
        }


        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 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

}