Estimación de la posición de un robot con ruedas con arduino. (3 / 5 paso)

Paso 3: El código fuente de

Haaa finalmente la interesante parte ;)

Este es un ejemplo del código por un arduino mega:
(No olvide poner sus propios valores en TICK_PER_MM y diámetro
TICK_PER_MM es el número de garrapatas que el codificador para hacer 1 milímetro en el suelo)

/*
* ----------------------------------------------------------------------------
* "La cerveza vajilla de licencia" (revisión 42):
* JBot escribió este archivo. Mientras conserve este aviso
* puede hacer lo que quieras con este material. Si nos encontramos algún día, y piensas
* Este material vale la pena, usted me puede comprar una cerveza a cambio.
* ----------------------------------------------------------------------------
*/

Otras incluyen
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >

/***********/
/ * Define * /
/***********/
#define TICK_PER_MM_LEFT 90.9456817668
#define TICK_PER_MM_RIGHT 90.9456817668
#define 166.0 diámetro / distancia entre las 2 ruedas (milímetro)

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radianes a grados de conversión * /

/********************/
/ * Variables globales * /
/********************/
volátil left_cnt largo = 0;
volátil right_cnt largo = 0;

doble pos_X = 0;
doble pos_Y = 0;
doble theta = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

doble total_distance = 0.0;

/***********************/
/ * FUNCIONES DE INTERRUPCIÓN * /
/***********************/

Rutina de servicio de interrupción 4 externo = > PIN2
ISR(INT4_vect)
{
#asm("cli")
Si ((PINB & 0x10)! = 0) {}
Si ((PINE & 0x10)! = 0)
left_cnt--;
otra cosa
left_cnt ++;
} else {}
Si ((PINE & 0x10) == 0)
left_cnt--;
otra cosa
left_cnt ++;
}

#asm("sei")
}

Rutina de servicio de interrupción 5 externo = > PIN3
ISR(INT5_vect)
{
Si ((PINK & 0x80)! = 0) {}
Si ((PINE & 0x20)! = 0)
right_cnt ++;
otra cosa
right_cnt--;
} else {}
Si ((PINE & 0x20) == 0)
right_cnt ++;
otra cosa
right_cnt--;
}

}

PIN cambio 0-7 interrupción servicio rutina = > PIN10
ISR(PCINT0_vect)
{
Si ((PINE & 0x10)! = 0) {}
Si ((PINB & 0x10)! = 0) {}
left_cnt ++;
} else
left_cnt--;
} else {}
Si ((PINB & 0x10) == 0) {}
left_cnt ++;
} else
left_cnt--;
}

}

Cambio de perno de la rutina de servicio de interrupción de 16-23 = > PIN-ADC15
ISR(PCINT2_vect)
{
Si ((PINE & 0x20)! = 0) {}
Si ((PINK & 0x80)! = 0)
right_cnt--;
otra cosa
right_cnt ++;
} else {}
Si ((PINK & 0x80) == 0)
right_cnt--;
otra cosa
right_cnt ++;
}

}

Rutina de servicio de temporizador 1 desbordamiento interrupción
ISR(TIMER1_OVF_vect)
{
SEI(); habilitar interrupciones
get_Odometers();

}

/*************************/
/ * INICIALIZACIÓN DEL SISTEMA * /
/*************************/
void setup()
{
Factor de división de oscilador de cristal: 1
#pragma optsize-
CLKPR = 0 X 80;
CLKPR = 0 X 00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize +
#endif

Inicialización de puertos de entrada/salida
Una inicialización del puerto
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = en
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTA = 0 X 00;
DDRA = 0 X 00;

Inicialización del puerto B
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = salida
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTB = 0 X 00;
DDRB = 0 X 00;

Inicialización del puerto C
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = en
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTC = 0 X 00;
DDR = 0 X 00;

Inicialización del puerto D
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = en Func2 = en Func1 = de Func0 = en
State7 = State6 T = State5 T = State4 T = T State3 = State2 T = T estado1 = State0 T = T
PORTD = 0 X 00;
DDRD = 0 X 00;

Inicialización del puerto E
Func2 = en Func1 = de Func0 = en
State2 = T estado1 = State0 T = T
PORTE = 0 X 00;
DDRE = 0 X 00;

PORTK = 0 X 00;
DDRK = 0 X 00;

/**********************/
/ * INICIALIZACIÓN DE LA ENTRADA-SALIDA * /
/**********************/
SALIDAS
pinMode (13, salida);

Inicialización del temporizador/contador 1
Fuente de reloj: reloj del sistema
Modo: PH correcto PWM parte superior = 00FFh
OC1A salida: desconectado.
OC1B salida: desconectado.
OC1C salida: desconectado.
Supresor de ruido: Off
Entrada captura en flanco descendente
Temporizador de interrupción 1 desbordamiento: en
Interrupción de la captura de entrada: Off
Comparar una interrupción del partido: Off
Compara B partido interrupción: Off
Comparar C partido interrupción: Off
TCCR1A = 0 X 01;
TCCR1B = 0 X 04;
TCNT1H = 0 X 00;
TCNT1L = 0 X 00;
ICR1H = 0 X 00;
ICR1L = 0 X 00;
OCR1AH = 0 X 00;
OCR1AL = 0 X 00;
OCR1BH = 0 X 00;
OCR1BL = 0 X 00;
OCR1CH = 0 X 00;
OCR1CL = 0 X 00;

Inicialización de Interrupt(s) externo
EICRA = 0 X 00;
EICRB = 0 X 05;
EIMSK = 0 X 30;
EIFR = 0 X 30;
Interrumpir el PCINT
PCICR = 0 X 05;
PCIFR = 0 X 05;
PCMSK0 = 0 X 10;
PCMSK1 = 0 X 00;
PCMSK2 = 0 X 80;

Temporizadores / contadores inicialización de Interrupt(s)
TIMSK1 | = 0 X 01;
TIFR1 | = 0 X 01;

/******************************/
/ * Inicialización del código * /
/******************************/

Habilitación global de interrupciones
SEI();

}

/******************/
/ * BUCLE DE CÓDIGO PRINCIPAL * /
/******************/
void loop()
{

}

/********************************/
/ * FUNCIÓN DE ESTIMACIÓN DE LA POSICIÓN * /
/********************************/

/ * Calcular la posición del robot * /
void get_Odometers(void)
{
left_wheel largo = 0;
right_wheel largo = 0;

doble left_mm = 0.0;
doble right_mm = 0.0;

doble distancia = 0.0;

left_wheel = left_cnt;
right_wheel = right_cnt;

left_diff = last_left - left_wheel;
right_diff = last_right - right_wheel;

last_left = left_wheel;
last_right = right_wheel;

left_mm = (left_diff (doble)) / TICK_PER_MM_LEFT;
right_mm = (right_diff (doble)) / TICK_PER_MM_RIGHT;

distancia = (left_mm + right_mm) / 2;
total_distance += distancia;

Theta += (right_mm - left_mm) / diámetro;

Si (theta > PI)
Theta-= TWOPI;
Si (theta < (-PI))
Theta += TWOPI;

pos_Y += distancia * sin(theta);
pos_X += distancia * cos(theta);

}

Algunas explicaciones:
La función de ISR (__VECT__) son las interrupciones externas. Se utilizan para contar el número de garrapatas de cada codificador.
La ISR(TIMER1_OVF_vect) es una interrupción de temporizador. Se utiliza para hacer un cálculo perfectamente cronometrada de la posición.
La función get_Odometers() es la función que calcula la posición a las garrapatas contadas antes.

¿Todo está funcionando para usted?
Gracias! Ahora tu robot puede calcular su propia posición.
Usted puede ahora hacer algo como eso:
Control remoto controlar tu robot y calcular su propia posición y enviar a su computadora para imprimir la trayectoria del robot.

¿Encontrará que cool pero usted quiere más para su robot? Ir al siguiente paso para ir autónomamente a waypoints!

Artículos Relacionados

Controlar la posición de un servomotor con un Sensor análogo

Controlar la posición de un servomotor con un Sensor análogo

En este Instructable, aprenderemos cómo utilizar un sensor análogo para controlar la posición de un servomotor. En este ejemplo, utilizaremos un potenciómetro, pero puede utilizar cualquier sensor analógico. Puede utilizar un sensor de luz para mover
Jurado del aparejo A conducir A baja tensión Motor Servo (DIY ESC)

Jurado del aparejo A conducir A baja tensión Motor Servo (DIY ESC)

cada fabricante buena ha utilizado un servo en algún momento u otro. Si es para un coche de RC o un brazo de robot con Arduino. Yo trabajaba en la construcción de un tanque de Arduino powered. He intentado usar servos de rotación continua como los mo
Línea básica siguiente Robot con Arduino

Línea básica siguiente Robot con Arduino

07/09/2015Han pasado unos años desde que publicamos inicialmente básica línea siguiente Robot con Arduino tutorial, y parece que mucha gente encuentra útil que debemos publicar una actualización que funciona actual de las bibliotecas de Arduino, incl
Lanzar bolas de fuego con tu mente con OSC y procesamiento

Lanzar bolas de fuego con tu mente con OSC y procesamiento

En mi tutorial pasado, hablé sobre cómo hacer un auricular Bluetooth MindFlex EEG y salida de mensajes OSC.Cuando empecé a jugar con el auricular de EEG, quería ver qué podía hacer con él. Tan naturalmente, experimentó con mover mi ratón con mi mente
Cómo hacer un obstáculo evitando Arduino Robot! "Mi Robot V1"

Cómo hacer un obstáculo evitando Arduino Robot! "Mi Robot V1"

Este instructable muestra las instrucciones de pasos para hacer un obstáculo evitando robot con Arduino.Empezar a dejarmi bloghttp://robot4pro.blogspot.com/p/My-robots.htmlPaso 1: Materiales necesariosAquí le damos todo que lo necesario para hacer My
ATOM

ATOM

HeyEste es mi primer instructables. Finalmente hice mi robot oruga, después de algunos planes no va bien. Se trata de un robot virase con dos 12v alto esfuerzo de torsión y velocidad. Lo seguimiento puede subir con un montón de cosas y hasta empinada
Guante de detección de posición suave-circuito

Guante de detección de posición suave-circuito

Advancer Technologies está desarrollando un guante de terapia de mano semi autónoma músculo neumático accionado para recuperación de accidentes cerebrovasculares y pacientes neurológicamente llamado ExoGlove. Parte del proyecto ExoGlove se centra en
Ratón 3D sin contacto (Sensor de posición 3D interactivo)

Ratón 3D sin contacto (Sensor de posición 3D interactivo)

Recientemente, he hecho este ratón sin contacto 3D, que utilizo para controlar (o tratar de control) el cursor de mi computadora. Puedes ver las funcionalidades en el siguiente video:El cómputo se hace sobre la arduino leonardo, que emula un ratón. D
Mediopié bicicleta Cala posición

Mediopié bicicleta Cala posición

Zapatos de bicicleta específicos han estado alrededor durante mucho tiempo. Básicamente, los tacos de sus zapatos clip en los pedales, lo que le permite generar energía durante todo el movimiento del pedal - no sólo en la carrera descendente. Por sup
Cómo hacer el plano del RC

Cómo hacer el plano del RC

Nos encantan los juguetes de RC, especialmente aviones RC. Usted puede comprar fácilmente uno, pero usted no puede personalizar. Así que ¿por qué no hacerlo? En este instructable te voy a mostrar cómo hacer trabajar completamente plano.¿Dónde empezar
Sistema de riego automático con sonda capacitiva y Arduino en el barato (y serio)

Sistema de riego automático con sonda capacitiva y Arduino en el barato (y serio)

Descargo de responsabilidad: Yo no soy un ingeniero en electrónica, por lo que no puedo ofrecer ninguna garantía para el diseño (mucho menos para su implementación). Sólo sé la presentó solución trabajada para mí por lo menos unos 5-6 meses (por lo q
Calibración simple magnetómetro Manual

Calibración simple magnetómetro Manual

Hay varios tipos de magnetómetro barato ahora disponible que puede utilizar para detectar una dirección de la brújula, pero todos ellos (que yo sepa) necesidad de calibración antes de poder obtener resultados razonables. En este Instructable, te demu
Bola de 3DOF en placa utilizando cerrado motores paso a paso de bucle

Bola de 3DOF en placa utilizando cerrado motores paso a paso de bucle

La bola en el problema de la placa consiste en una placa plana en la que una bola debe ubicarse. Bola de posicionamiento se logra a través de equilibrio inestable, donde cualquier pequeño cambio en el ángulo de la placa dará lugar a la aceleración co
Seguidor de línea de alto rendimiento Robot

Seguidor de línea de alto rendimiento Robot

¡ Hola! Voy a intentar presentarle el mundo de los robots seguidor de línea. Muchos de nosotros han escuchado acerca de este tipo de robots raros, que existen en una amplia variedad de versiones de las muy lentas y pesadas, que generalmente puede tra