Stap 3: De broncode
Dit is een voorbeeld van de code voor een arduino-mega:
(Vergeet niet om uw eigen waarden gestoken met TICK_PER_MM en DIAMETER
TICK_PER_MM is het aantal teken dat uw encoder doet hiervoor 1 millimeter op de grond)
/*
* ----------------------------------------------------------------------------
* "De bier-WARE LICENSE" (revisie 42):
* JBot schreef dit bestand. Zolang u dit behouden merk je
* kan doen wat je wilt met dit spul. Als we een dag ontmoeten, en u denkt
* Dit spul is de moeite waard, u kunt kopen me een biertje in ruil.
* ----------------------------------------------------------------------------
*/
Andere omvat
#include < avr/io.h >
#include < util/delay.h >
#include < avr/interrupt.h >
#include < math.h >
/***********/
/ * Definieert * /
/***********/
#define TICK_PER_MM_LEFT 90.9456817668
#define TICK_PER_MM_RIGHT 90.9456817668
#define DIAMETER 166.0 / / afstand tussen de 2 wielen (in millimeter)
#define TWOPI 6.2831853070
#define RAD2DEG 57.2958 / * radialen naar graden conversie * /
/********************/
/ * Globale variabelen * /
/********************/
vluchtige lange left_cnt = 0;
vluchtige lange right_cnt = 0;
dubbele pos_X = 0;
dubbele pos_Y = 0;
dubbele theta = 0;
int last_left = 0;
int last_right = 0;
int left_diff = 0;
int right_diff = 0;
dubbele total_distance = 0,0;
/***********************/
/ * INTERRUPT FUNCTIES * /
/***********************/
Externe 4 Interrupt service routine = > PIN2
ISR(INT4_vect)
{
#asm("cli")
Als ((PINB & 0x10)! = 0) {}
Als ((PINE & 0x10)! = 0)
left_cnt--;
anders
left_cnt ++;
} else {}
Als ((PINE & 0x10) == 0)
left_cnt--;
anders
left_cnt ++;
}
#asm("sei")
}
Externe 5 Interrupt service routine = > PIN3
ISR(INT5_vect)
{
Als ((PINK & 0x80)! = 0) {}
Als ((PINE & 0x20)! = 0)
right_cnt ++;
anders
right_cnt--;
} else {}
Als ((PINE & 0x20) == 0)
right_cnt ++;
anders
right_cnt--;
}
}
PIN verandering 0-7 interrupt service routine = > PIN10
ISR(PCINT0_vect)
{
Als ((PINE & 0x10)! = 0) {}
Als ((PINB & 0x10)! = 0) {}
left_cnt ++;
} else
left_cnt--;
} else {}
Als ((PINB & 0x10) == 0) {}
left_cnt ++;
} else
left_cnt--;
}
}
PIN wijzigen 16-23 interrupt service routine = > PIN-ADC15
ISR(PCINT2_vect)
{
Als ((PINE & 0x20)! = 0) {}
Als ((PINK & 0x80)! = 0)
right_cnt--;
anders
right_cnt ++;
} else {}
Als ((PINK & 0x80) == 0)
right_cnt--;
anders
right_cnt ++;
}
}
Timer 1 overflow interrupt service routine
ISR(TIMER1_OVF_vect)
{
Sei(); inschakelen interrupts
get_Odometers();
}
/*************************/
/ * INITIALISATIE VAN HET SYSTEEM * /
/*************************/
VOID Setup
{
Kristal Oscillator divisie factor: 1
#pragma optsize-
CLKPR = 0X80;
CLKPR = 0X00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize +
#endif
Initialisatie van de Input/Output-poorten
Poort een initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = In
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTA = 0X00;
DDRA = 0X00;
Poort B initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = uit
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTB = 0X00;
DDRB = 0X00;
Poort C initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = In
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTC = 0X00;
DDRC = 0X00;
Poort D initialisatie
Func7 = In Func6 In Func5 = = In Func4 In Func3 = = In Func2 In Func1 = = In Func0 = In
State7 = State6 voor T = T State5 = T staat vallen4 = State3 voor T = T State2 = T gebracht1 = T State0 = T
PORTD = 0X00;
DDRD = 0X00;
Port E initialisatie
Func2 = In Func1 In Func0 = = In
State2 = T gebracht1 = State0 voor T = T
PORTE = 0X00;
DDRE = 0X00;
PORTK = 0X00;
DDRK = 0X00;
/**********************/
/ * I/O-INITIALISATIE * /
/**********************/
UITGANGEN
pinMode (13, OUTPUT);
Timer/teller 1 initialisatie
Klokbron: systeemklok
Modus: Ph. juiste PWM top = 00FFh
OC1A output: Discon.
OC1B output: Discon.
OC1C output: Discon.
Lawaai van de Canceler: uit
Input Capture aan dalende rand
Timer 1 Overflow Interrupt: op
Input Capture Interrupt: uit
Vergelijk een Match-Interrupt: uit
Vergelijk B Match Interrupt: uit
Vergelijk C Match Interrupt: uit
TCCR1A = 0X01;
TCCR1B = 0X04;
TCNT1H = 0X00;
TCNT1L = 0X00;
ICR1H = 0X00;
ICR1L = 0X00;
OCR1AH = 0X00;
OCR1AL = 0X00;
OCR1BH = 0X00;
OCR1BL = 0X00;
OCR1CH = 0X00;
OCR1CL = 0X00;
Externe Interrupt(s) initialisatie
EICRA = 0X00;
EICRB = 0X05;
EIMSK = 0X30;
EIFR = 0X30;
Onderbreken op PCINT
PCICR = 0X05;
PCIFR = 0X05;
PCMSK0 = 0X10;
PCMSK1 = 0X00;
PCMSK2 = 0X80;
Timer(s) / Counter(s) Interrupt(s) initialisatie
TIMSK1 | = 0X01;
TIFR1 | = 0X01;
/******************************/
/ * Initialisatie van de code * /
/******************************/
Global inschakelen interrupts
Sei();
}
/******************/
/ * MAIN CODE LUS * /
/******************/
void loop
{
}
/********************************/
/ * POSITIE SCHATTING FUNCTIE * /
/********************************/
/ * Het berekenen van de positie van de robot * /
VOID get_Odometers(void)
{
lange left_wheel = 0;
lange right_wheel = 0;
dubbele left_mm = 0,0;
dubbele right_mm = 0,0;
dubbele afstand = 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 = ((dubbele) left_diff) / TICK_PER_MM_LEFT;
right_mm = ((dubbele) right_diff) / TICK_PER_MM_RIGHT;
afstand = (left_mm + right_mm) / 2;
total_distance += afstand;
Theta += (right_mm - left_mm) / DIAMETER;
Als (theta > PI)
Theta-= TWOPI;
Als (theta < (-PI))
Theta += TWOPI;
pos_Y += afstand * sin(theta);
pos_X += afstand * cos(theta);
}
Sommige uitleg:
De functie van de ISR (__VECT__) zijn de externe onderbrekingen. Ze worden gebruikt om te tellen van het aantal teken van elke encoder.
De ISR(TIMER1_OVF_vect) is een onderbreking van de timer. Het wordt gebruikt om een perfect getimede computing van het standpunt.
De get_Odometers() functie is de functie die de positie afhankelijk van naar de teken geteld voor berekenen.
Alles wat voor u werkt?
Heilwens! Uw robot kan nu zijn eigen positie berekenen!
U kunt nu doen iets dergelijks:
Afstandsbediening uw robot controle en laat hem zijn eigen positie berekenen en stuur het naar uw computer om af te drukken van de robot de pad.
U vind dat cool, maar je meer voor uw robot wilt? Ga naar de volgende stap om het autonoom Ga naar waypoints te maken!
