File content as of revision 0:8180a93bf2dc:

#include "mbed.h"
#include "max7219.h" // incluir librerias.
#include "vision.h"
#include "stdlib.h"

int motorSpeed; // variable para la velocidad del motor
int cuadrante;//variable de cuadrante 
int pasos;

//#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(cuadrante >buffer_command[COMM_GRADOS_MOTOR]){ // seleccion direccion del motor paso a paso (derecha)
                Print_pantalla_tabla(m4right);  
                cuadrante=(cuadrante-buffer_command[COMM_GRADOS_MOTOR]);
                pasos=(cuadrante*255);
                stepper(pasos,00);
                }
                 else if(cuadrante<buffer_command[COMM_GRADOS_MOTOR]){// seleccion direccion del motor paso a paso (izquierda)
                Print_pantalla_tabla(m4left); 
                cuadrante=(cuadrante-buffer_command[COMM_GRADOS_MOTOR]);
                cuadrante=(abs(cuadrante));
                pasos=(cuadrante*255);
                stepper(pasos,01);
                }
                else{
                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)
{    
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);
   

    mipwm.pulsewidth_us(k);
       
}
else if (grados<0x5a){  // visualizacion cuando es menor que 90
   Print_pantalla_tabla(m1right);
   
    mipwm.pulsewidth_us(k);
       
   }
    else{                  // visualizacion cuando es mayor que 90
        Print_pantalla_tabla(m1left);
   
    mipwm.pulsewidth_us(k);
       
   }

      mipwm.pulsewidth_us(k);
      
break;
case 01:if(grados==0x5a){                       //90 grados

   Print_pantalla_tabla(m2up);
   
}
else if (grados<0x5a){    // visualizacion cuando es menor que 90
   Print_pantalla_tabla(m2right);
   
   }
    else{// visualizacion cuando es mayor que 90
        Print_pantalla_tabla(m2left);
   
   }
     mipwm2.pulsewidth_us(k);
      
break;
case 02:
if(grados==0x5a){                       //90 grados

   Print_pantalla_tabla(m3up);
  
}
else if (grados<0x5a){ // visualizacion cuando es menor que 90
   Print_pantalla_tabla(m3right);
   
   }
    else{               // visualizacion cuando es mayor que 90
        Print_pantalla_tabla(m3left);
  
   }
     mipwm3.pulsewidth_us(k);
      
break;
default:
break;
}



}



int main() {
          cuadrante=0;
         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();
     
     
     
     
     
     
     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;
                _A4=0;
                _A5=0;
                _A6=0;
                _A7=1;
            }
            break;
            case 1: {
                _A0=0;
                _A1=0;
                _A2=1;
                _A3=1;
                _A4=0;
                _A5=0;
                _A6=1;
                _A7=1;
            }
            break;
            case 2: {
                _A0=0;
                _A1=0;
                _A2=1;
                _A3=0;
                _A4=0;
                _A5=0;
                _A6=1;
                _A7=0;
            }
            break;
            case 3: {
                _A0=0;
                _A1=1;
                _A2=1;
                _A3=0;
                _A4=0;
                _A5=1;
                _A6=1;
                _A7=0;
            }
            break;
            case 4: {
                _A0=0;
                _A1=1;
                _A2=0;
                _A3=0;
                _A4=0;
                _A5=1;
                _A6=0;
                _A7=0;
            }
            break;
            case 5: {
                _A0=1;
                _A1=1;
                _A2=0;
                _A3=0;
                _A4=1;
                _A5=1;
                _A6=0;
                _A7=0;
            }
            break;
            case 6: {
                _A0=1;
                _A1=0;
                _A2=0;
                _A3=0;
                _A4=1;
                _A5=0;
                _A6=0;
                _A7=0;
            }
            break;
            case 7: {
                _A0=1;
                _A1=0;
                _A2=0;
                _A3=1;
                _A4=1;
                _A5=0;
                _A6=0;
                _A7=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;
                _A4=0;
                _A5=0;
                _A6=0;
                _A7=1;
            }
            break;
            case 1: {
                _A0=0;
                _A1=0;
                _A2=1;
                _A3=1;
                _A4=0;
                _A5=0;
                _A6=1;
                _A7=1;
            }
            break;
            case 2: {
                _A0=0;
                _A1=0;
                _A2=1;
                _A3=0;
                _A4=0;
                _A5=0;
                _A6=1;
                _A7=0;
            }
            break;
            case 3: {
                _A0=0;
                _A1=1;
                _A2=1;
                _A3=0;
                _A4=0;
                _A5=1;
                _A6=1;
                _A7=0;
            }
            break;
            case 4: {
                _A0=0;
                _A1=1;
                _A2=0;
                _A3=0;
                _A4=0;
                _A5=1;
                _A6=0;
                _A7=0;
            }
            break;
            case 5: {
                _A0=1;
                _A1=1;
                _A2=0;
                _A3=0;
                _A4=1;
                _A5=1;
                _A6=0;
                _A7=0;
            }
            break;
            case 6: {
                _A0=1;
                _A1=0;
                _A2=0;
                _A3=0;
                _A4=1;
                _A5=0;
                _A6=0;
                _A7=0;
            }
            break;
            case 7: {
                _A0=1;
                _A1=0;
                _A2=0;
                _A3=1;
                _A4=1;
                _A5=0;
                _A6=0;
                _A7=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

}