Hur man bygger en själv navigera roboten (4 / 7 steg)

Steg 4: Öppen källkod

Hela projektet är tillgänglig som öppen källkod på Google Code
http://code.Google.com/p/dspid33/
Se det för den senaste versionen, kommentar, beskrivningar, etc.
Programmet beskrivs steg för steg inne i koden. För att få en hög nivå av kommentarer och en mer lättläst kod, på varje väsentlig punkt det är ett nummer inom parentes (e.g.: [7]) som en referens till en extern fil (dvs.: descrEng.txt) i MPLAB projektet.

Diagrammet visar den övergripande arkitekturen av förfarandena för kontroll av dsNav ombord och navigation strategier tillämpas på basen av projektet.
De motoriska styrenheterna kan ses som svarta lådor som tar hand om hastigheten på hjulen. Den handledare delen av programmet skickar dem referenshastigheten (VeldDesX: önskad hastighet). Input Capture moduler av mikrokontroller få pulser från kodare är ansluten till den motoriska axeln och härleda motorerna rotationshastighet (VelMesX: uppmätt hastighet). Kombinera varje 1ms detta värden i kontrollen PID "Hastighet PID" få vi rätt PWM värdet för att hålla önskad hastighet för varje enskild hjul.
QEI (Quadrature Encoder Interface) modulerna få både A och B pulser från pulsgivare och ge tillbaka till funktionen handledare resande riktning och antalet pulser i 4 x-läge (räkna de stigande och fallande kanterna av signal A och signal B: 2 x 2 = 4).
Att multiplicera antalet pulser med K som anger utrymmet för varje enskild encoder puls, få vi avståndet reste av höger och vänster hjul varje 10ms. Handledaren kombinerar detta reser informationer och gäller död räkenskap förfarandet för att erhålla de uppmätta positionskoordinater bot: Xmes, Ymes, θMes (orientering vinkel).

Handledaren får navigering kommandot från utanför av seriellt gränssnitt (telemetri).
Olika strategier kan tillämpas:
A - resa i en given hastighet i en viss riktning (VelDes, θDes).
B - resa mot en given punkt med koordinater XDes, YDes.
C - resor för ett givet avstånd i en viss riktning (DistDes, θDes).

Läge A : med "logisk kontroll växlar" i läge 1, endast den PID control "Vinkel PID" används av funktionerna som handledare. Här kombinerar önskad vinkel θDes med den uppmätta vinkeln θMes beräknas av odometry förfarande, för att få värdet av rotation rusat vinkelformigt ω runt sin vertikala axel, behövs för att rätta till felet orientering.
Värdet av DeltaV är proportionell mot ω. Lagt till VelDes att få hastigheten på vänstra hjulet har. dras till VelDes att få hastigheten på högra hjulet, för att hålla rubriken motsvarande θDes värde, medan mitten av roboten fortfarande reser VelDes hastighet

Läge B : med "logisk kontroll växlar" i position 2, önskad hastighet VelDes beräknas genom kontrollen PID "Dist PID" och det används som i läge A. Den uppmätta ingången för denna PID (DistMes) beräknas som en funktion av de aktuella koordinaterna och destination koordinaterna. Önskad orientering vinkel θDes kommer också från samma förfarande och det används som referens ingång för "Vinkel PID". Hänvisningen ingång för "Dist PID" är 0, vilket innebär att målet nås. Med ω och VelDes tillgängliga går varvtalsreglering av hjulen som i läge A.

Läge C : med "logisk kontroll växlar" i position 2, destination cordinates Xdes, Ydes beräknas en gång i början som en funktion av indataparametrar DistDes, θDes. Efter att allt går som i läge B