![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
servo visual
Realizamos mediante comunicacion serial, ingresando el comando para los servomotores:
COMANDO MOVER MOTOR
POS 1 | POS 2 | POS 3 | POS 4 | POS 5 |
< | #M | , | #°G | > |
- M -> indica el motor que se va a mover (00,01,02)
- °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 1 | POS 2 | POS 3 | POS 4 | POS 5 |
< | #M | DIR | #PASOS | > |
- M -> indica el motor que se va a mover ( 03)
- 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 }