Étape 3: Le code source
Il s’agit d’un exemple du code pour un arduino mega :
(N’oubliez pas de mettre vos propres valeurs dans TICK_PER_MM et diamètre
TICK_PER_MM est le nombre de graduations qui fait de votre codeur pour faire 1 millimètre sur le terrain)
/*
* ----------------------------------------------------------------------------
* "La bière-WARE licence" (révision 42) :
* JBot a écrit ce fichier. Tant que vous conservez cette remarque que vous
* peut faire tout ce que vous voulez avec ce genre de choses. Si nous rencontrons un jour et que vous pensez
* ça en vaut la peine, vous pouvez m’acheter une bière en retour.
* ----------------------------------------------------------------------------
*/
Autres comprend
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >
/***********/
/ * Définit * /
/***********/
#define TICK_PER_MM_LEFT 90.9456817668
#define TICK_PER_MM_RIGHT 90.9456817668
#define diamètre 166,0 / / Distance entre les 2 roues (en millimètres)
#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radians en degrés conversion * /
/********************/
/ * Variables globales * /
/********************/
volatils long left_cnt = 0 ;
volatils long right_cnt = 0 ;
double pos_X = 0 ;
double pos_Y = 0 ;
double theta = 0 ;
int last_left = 0 ;
int last_right = 0 ;
int left_diff = 0 ;
int right_diff = 0 ;
double total_distance = 0.0 ;
/***********************/
/ * INTERRUPTION FONCTIONS * /
/***********************/
Routine de service externe 4 interrompre = > code PIN2
ISR(INT4_vect)
{
#asm("cli")
Si ((PINB & 0x10)! = 0) {}
Si ((PINE & 0x10)! = 0)
left_cnt--;
d’autre
left_cnt ++ ;
} else {}
Si ((PINE & 0x10) == 0)
left_cnt--;
d’autre
left_cnt ++ ;
}
#asm("sei")
}
Routine de service externe interrompre 5 = > broche 3
ISR(INT5_vect)
{
Si ((PINK & 0x80)! = 0) {}
Si ((PINE & 0x20)! = 0)
right_cnt ++ ;
d’autre
right_cnt--;
} else {}
Si ((PINE & 0x20) == 0)
right_cnt ++ ;
d’autre
right_cnt--;
}
}
Goupille changement 0-7 interrupt service routine = > 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--;
}
}
Broche modifier la routine d’interruption du service 16-23 = > PIN-ADC15
ISR(PCINT2_vect)
{
Si ((PINE & 0x20)! = 0) {}
Si ((PINK & 0x80)! = 0)
right_cnt--;
d’autre
right_cnt ++ ;
} else {}
Si ((PINK & 0x80) == 0)
right_cnt--;
d’autre
right_cnt ++ ;
}
}
Routine de service de minuteur 1 débordement interruption
ISR(TIMER1_OVF_vect)
{
SEI() ; activer les interruptions
get_Odometers() ;
}
/*************************/
/ * INITIALISATION DU SYSTÈME * /
/*************************/
void setup()
{
Facteur de division Crystal Oscillator: 1
#pragma optsize-
CLKPR = 0 X 80 ;
CLKPR = 0 X 00 ;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize +
#endif
Initialisation de Ports d’entrée/sortie
Une initialisation de port
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = en
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTA = 0 X 00 ;
DDRA = 0 X 00 ;
Initialisation du port B
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = Out
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTB = 0 X 00 ;
DDRB = 0 X 00 ;
Initialisation du port C
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = en
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTC = 0 X 00 ;
DDRC = 0 X 00 ;
Initialisation du port D
Func7 = en Func6 = en Func5 = en Func4 = en Func3 = à Func2 = à Func1 = en Func0 = en
Etat7 = État6 T = State5 T = T Etat4 = Etat3 T = State2 T = T State1 = State0 T = T
PORTD = 0 X 00 ;
DDDR = 0 X 00 ;
Initialisation du port E
Func2 = à Func1 = en Func0 = en
State2 = T State1 = State0 T = T
PORTE = 0 X 00 ;
DDRE = 0 X 00 ;
PORTK = 0 X 00 ;
DDRK = 0 X 00 ;
/**********************/
/ * INITIALISATION D’E/S * /
/**********************/
SORTIES
pinMode (13, sortie) ;
Initialisation du timer/compteur 1
Source de l’horloge : horloge système
Mode : Haut pH correct PWM = 00FFh
OC1A sortie : aban.
OC1B sortie : aban.
OC1C sortie : aban.
Canceler de bruit : arrêt
Capture d’entrée sur front descendant
Minuterie 1 débordement interrompre : sur
Interruption de la prise d’entrée : Off
Comparer une interruption du Match : Off
Comparer B interruption de Match : Off
Comparer l’interruption du Match C: 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 ;
Initialisation de Interrupt(s) externe
EICRA = 0 X 00 ;
EICRB = 0 X 05 ;
EIMSK = 0 X 30 ;
EIFR = 0 X 30 ;
Interrompre le P.C.int
PCICR = 0 X 05 ;
PCIFR = 0 X 05 ;
PCMSK0 = 0 X 10 ;
PCMSK1 = 0 X 00 ;
PCMSK2 = 0 X 80 ;
Minuteries / ou les compteurs s Interrupt(s) d’initialisation
TIMSK1 | = 0 X 01 ;
TIFR1 | = 0 X 01 ;
/******************************/
/ * Initialisation du code * /
/******************************/
Enable global interruptions
SEI() ;
}
/******************/
/ * MAIN CODE BOUCLE * /
/******************/
void loop()
{
}
/********************************/
/ * FONCTION D’ESTIMATION DE POSITION * /
/********************************/
/ * Calculer la position du robot * /
Sub get_Odometers(void)
{
long left_wheel = 0 ;
long right_wheel = 0 ;
double left_mm = 0.0 ;
double right_mm = 0.0 ;
double distance = 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 (double)) / TICK_PER_MM_LEFT ;
right_mm = (right_diff (double)) / TICK_PER_MM_RIGHT ;
distance = (left_mm + right_mm) / 2 ;
TOTAL_DISTANCE += distance ;
thêta += (right_mm - left_mm) / diamètre ;
Si (thêta > PI)
thêta-= TWOPI ;
Si (thêta < (-PI))
thêta += TWOPI ;
pos_Y += distance * sin(theta) ;
pos_X += distance * cos (theta) ;
}
Quelques explications :
La fonction ISR (__VECT__) sont les interruptions externes. Ils servent à compter le nombre de graduations de chaque codeur.
Le ISR(TIMER1_OVF_vect) est un arrêt de la minuterie. Il est utilisé pour faire un calcul parfaitement chronométré du poste.
La fonction get_Odometers() est la fonction qui calcule la position selon les tiques comptés avant.
Tout fonctionne pour vous ?
Félicitations ! Maintenant, votre robot peut calculer sa propre position !
Vous pouvez maintenant faire quelque chose comme ça :
Remote commander votre robot et laissez-le calculer sa propre position et envoyez-le à votre ordinateur pour imprimer le chemin d’accès du robot.
Vous trouvez que cool, mais vous voulez plus de votre robot ? Passez à l’étape suivante à faire aller de façon autonome à waypoints !
