Paso 3: El código fuente de
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!
