Ståndpunkten uppskattning av en hjulförsedda bot med arduino. (3 / 5 steg)

Steg 3: Källkoden

Detta är ett exempel på koden för en arduino-mega:
(Glöm inte att sätta egna värden i TICK_PER_MM och DIAMETER
TICK_PER_MM är antalet fästingar som din kodare gör för att göra 1 millimeter på marken)

/*
* ----------------------------------------------------------------------------
* "Den öl-WARE licens" (revidering 42):
* JBot skrev denna fil. Så länge du behåller detta märker du
* kan göra vad du vill med det här. Om vi träffas någon dag, och du tror
* Det här är värt det, du kan köpa mig en öl i gengäld.
* ----------------------------------------------------------------------------
*/

Annat omfattar
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >

/***********/
/ * Definieras * /
/***********/
#define TICK_PER_MM_LEFT 90.9456817668
#define TICK_PER_MM_RIGHT 90.9456817668
#define DIAMETER 166.0 / / avstånd mellan 2 hjul (i millimeter)

#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radianer till grader konvertering * /

/********************/
/ * Globala variabler * /
/********************/
flyktiga lång left_cnt = 0;
flyktiga lång right_cnt = 0;

Double pos_X = 0;
Double pos_Y = 0;
dubbel theta = 0;

int last_left = 0;
int last_right = 0;

int left_diff = 0;
int right_diff = 0;

Double total_distance = 0.0;

/***********************/
/ * AVBRYTA FUNKTIONER * /
/***********************/

Externa avbryta 4 rutin = > PIN2
ISR(INT4_vect)
{
#asm("cli")
om ((PINB & 0x10)! = 0) {
om ((PINE & 0x10)! = 0)
left_cnt--;
annat
left_cnt ++;
} annat {
om ((PINE & 0x10) == 0)
left_cnt--;
annat
left_cnt ++;
}

#asm("sei")
}

Externa avbryta 5 rutin = > PIN3
ISR(INT5_vect)
{
om ((PINK & 0x80)! = 0) {
om ((PINE & 0x20)! = 0)
right_cnt ++;
annat
right_cnt--;
} annat {
om ((PINE & 0x20) == 0)
right_cnt ++;
annat
right_cnt--;
}

}

PIN change 0-7 avbrottstjänstens rutin = > PIN10
ISR(PCINT0_vect)
{
om ((PINE & 0x10)! = 0) {
om ((PINB & 0x10)! = 0) {
left_cnt ++;
} annat
left_cnt--;
} annat {
om ((PINB & 0x10) == 0) {
left_cnt ++;
} annat
left_cnt--;
}

}

Ändra PIN 16-23 avbrottstjänstens rutin = > PIN-ADC15
ISR(PCINT2_vect)
{
om ((PINE & 0x20)! = 0) {
om ((PINK & 0x80)! = 0)
right_cnt--;
annat
right_cnt ++;
} annat {
om ((PINK & 0x80) == 0)
right_cnt--;
annat
right_cnt ++;
}

}

Timer 1 spill avbrottstjänstens rutin
ISR(TIMER1_OVF_vect)
{
SEI(); Aktivera avbrott
get_Odometers();

}

/*************************/
/ * SYSTEM INITIALIZATION * /
/*************************/
void setup()
{
Kristalloscillator division faktor: 1
#pragma optsize-
CLKPR = 0X80;
CLKPR = 0X00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize +
#endif

Input/Output Ports initiering
Port en initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = i
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTA = 0X00;
DDRA = 0X00;

Port B initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = ut
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTB = 0X00;
DDRB = 0X00;

Port C initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = i
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTC = 0X00;
DDRC = 0X00;

Port D initiering
Func7 = i Func6 = i Func5 = i Func4 = i Func3 = i Func2 = i Func1 = i Func0 = i
State7 = T State6 = T State5 = T State4 = T State3 = T State2 = T State1 = T State0 = T
PORTD = 0X00;
DDRD = 0X00;

Port E initieringen
Func2 = i Func1 = i Func0 = i
State2 = T State1 = T State0 = T
PORTE = 0X00;
DDRE = 0X00;

PORTK = 0X00;
DDRK = 0X00;

/**********************/
/ * I/O-INITIERING * /
/**********************/
UTGÅNGAR
pinMode (13, OUTPUT);

Timer/Counter 1 initiering
Klocka Källa: systemklockan
Läge: Ph. rätt PWM topp = 00FFh
OC1A utgång: apparaten.
OC1B utgång: apparaten.
OC1C utgång: apparaten.
Brus Canceler: Off
Input Capture på fallande kant
Timer 1 spill avbryta: på
Input Capture avbrott: Off
Jämföra en Match avbryta: Off
Jämför B Match avbrott: Off
Jämföra C Match avbrott: Off
TCCR1A = 0X01;
TCCR1B = 0X04;
TCNT1H = 0X00;
TCNT1L = 0X00;
ICR1H = 0X00;
ICR1L = 0X00;
OCR1AH = 0X00;
OCR1AL = 0X00;
OCR1BH = 0X00;
OCR1BL = 0X00;
OCR1CH = 0X00;
OCR1CL = 0X00;

Externa Interrupt(s) initiering
EICRA = 0X00;
EICRB = 0X05;
EIMSK = 0X30;
EIFR = 0X30;
Avbryta på PCINT
PCICR = 0X05;
PCIFR = 0X05;
PCMSK0 = 0X10;
PCMSK1 = 0X00;
PCMSK2 = 0X80;

Timer(s) / Counter(s) Interrupt(s) initiering
TIMSK1 | = 0X01;
TIFR1 | = 0X01;

/******************************/
/ * Initieringen av koden * /
/******************************/

Globala aktivera avbrott
SEI();

}

/******************/
/ * MAIN KOD LOOP * /
/******************/
void loop()
{

}

/********************************/
/ * POSITION UPPSKATTNING FUNKTION * /
/********************************/

/ * Beräkna position roboten * /
void get_Odometers(void)
{
lång left_wheel = 0;
lång right_wheel = 0;

Double left_mm = 0.0;
Double right_mm = 0.0;

dubbla avståndet = 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 = ((dubbel) left_diff) / TICK_PER_MM_LEFT;
right_mm = ((dubbel) right_diff) / TICK_PER_MM_RIGHT;

avståndet = (left_mm + right_mm) / 2;
total_distance += avstånd;

theta += (right_mm - left_mm) / DIAMETER.

om (theta > PI)
theta-= TWOPI;
om (theta < (-PI))
theta += TWOPI;

pos_Y += avstånd * sin(theta);
pos_X += avstånd * cos(theta);

}

Några förklaringar:
Funktionen ISR (__VECT__) är de externa avbrott. De används för att räkna antalet fästingar varje givare.
ISR(TIMER1_OVF_vect) är ett timer-avbrott. Det används för att göra en perfekt tajmade computing ställning.
Get_Odometers() funktion är den funktion som beräkna positionen beroende till fästingar räknas innan.

Alla fungerar för dig?
Gratulationen! Nu kan din robot beräkna sin egen ståndpunkt!
Du kan nu göra något liknande:
Remote styra din robot och låt honom beräkna sin egen position och skicka den till din dator för att skriva ut robotens sökväg.

Du hittar att cool men du vill ha mer för din robot? Gå till nästa steg för att göra det gå självständigt till vägpunkter!