Hemgjord rimliga Robot med Bluetooth (4 / 5 steg)

Steg 4: Koden

BIBLIOTEKEN:

"SweeperBot" använder sig av tråd, I2Cdev, TimerOne (för avbrott), Adafruit Sensor, HMC5883 och LCD bibliotek. Du kan ladda ner Zip-filen, som innehåller koden för Arduino och alla bibliotek. Innehåller även Bluetooth projektet för Windows Phone.

WINDOWS PHONE 8.1 KODEN:

Det projekt som styr roboten via Bluetooth från Windows Phone var anpassad från ett exempel på internet. Det ändrades för att ha fyra knappar, en för varje riktning. Upp, ner, vänster och höger. Varje knapp skicka ett brev till roboten, respektive U, D, L och R. När roboten tar emot dessa, gör något av följande:

• U - roboten kommer att ställa in riktning att följa rakt fram;

• D - roboten kommer att vända sig till ryggen och gå rak;

• L - roboten kommer att ange riktningen att följa exakt 90 grader vänster från där det är för närvarande "ser";

• R - roboten kommer att ange riktningen att följa exakt 90 grader från där det är för närvarande "ser";

Roboten hanterar dessa kommandon kan ändras med koden på Arduino. Det är möjligt att lägga till fler knappar att vända roboten, eller att stoppa den, eller ens att göra det gå bakåt. Denna kod är bara ett exempel. Det är också möjligt att göra liknande program till andra enheter, såsom androider en för IOS. Det enda du har att skicka roboten är dessa bokstäver - U, D, L eller R.

privat async void RedButton_Click_1 (objekt avsändare, RoutedEventArgs e) {
String kommando = "U";
väntar på connectionManager.SendCommand(command);
}

privat async void GreenButton_Click_1 (objekt avsändare, RoutedEventArgs e) {
String kommando = "L".
väntar på connectionManager.SendCommand(command);
}

privat async void YellowButton_Click_1 (objekt avsändare, RoutedEventArgs e) {
String kommando = "R";
väntar på connectionManager.SendCommand(command);
}

privat async void RedButton_Copy_Click (objekt avsändare, RoutedEventArgs e) {
String kommando = "D";
väntar på connectionManager.SendCommand(command);
}

Om du vill distribuera denna kod till din Windows Phone måste Visual Studio och en utvecklare olåst telefon. Detta kan göras efter instruktioner om detta länk.

ARDUINO KODEN:

I CV är huvudsakliga logiken av roboten följande:

1. Avståndet till närmare objektet finns i vart och ett av 6 possibles sensorer;
2. Beteende av roboten process avstånd och punkt önskad riktning med en vektor X, Y (dock om ett meddelande tas emot från smartphone, steg 2 ersätts av riktning skickas från enheten);
3. Digital kompass ger riktning roboten kommer, för att kontrollera dess hjul;
4. Information X', Y' från kompass är normaliserat till rymden [-1,1];
5. Cross produkt mellan vektor i önskad riktning och faktiska riktning hittar fel mellan dem;
6. PID-metoden beräknar PWM (power) till varje DC-motor för de två hjul (2WD);
7. Avbrott aktiverar PWM;

Som projektet utvecklats, jag grunda ute tillgänglig på GitHub standardbiblioteket för ultraljudssensorer var motstridiga med PWM aktiveringen av motorerna, eftersom de båda används samma timern. Som detta projekt använder 5 stift för att konfigurera PWM av de tre likströmsmotorer (och detta är endast möjligt eftersom matta sopmaskin endast vänder sig till en riktning), och ytterligare 7 till sensorer, som mikro-controller att ständigt kontrollera om läsningen togs emot, det var inte möjligt att använda materialet som finns tillgängligt på internet.

Så, för att undvika konflikter, det har konfigurerats endast timer 1 avbrottet. Genom detta avbrott, Arduino kan kontrollera om det fanns avläsningar på varje sensor: om det ökar inte, en variabel för att var och en av dem, som kommer att användas som parameter för att hitta avståndet till objektet.

void callback() {//callback heter avbrott
noInterrupts();
om ingen läsning, ökar variabeln sonarCount [n], där n är antalet sensor
om ((sonar [0] == 1) & & (digitalRead(ECHO))) sonarCount [0] ++;
om det finns readind på att sensorn, sonar [n] = 2 anklagar objekt hittades
om ((sonar [0] == 1) & & (! digitalRead(ECHO)) & & (sonarCount [0] > 0)) ekolod [0] = 2;

Också, inuti detta avbrott, det kastar full effekt motorn. Om det funnits ett visst antal avbrott som överstiger antalet PWM_R eller PWM_L variabler, kastar noll styrka till att motor.

om (PWMCount == 0) {
om (PWML) {//if pwm vänster är positiva då vända sig till ett visst sätt
digitalWrite (MOTOR + 2, hög);
digitalWrite (MOTOR + 3, låg);
} annat {//else tur att den andra sidan vänstra motorn
digitalWrite (MOTOR + 2, låg);
digitalWrite (MOTOR + 3, hög);
}

(...)

om (PWMCount > = abs(PWM_L)) {//if räknaren är lika med kraften vi vill
digitalWrite (MOTOR + 2, låg); stänga ner båda stiften på denna motor
digitalWrite (MOTOR + 3, låg);
}

(...)

PWMCount ++; ökar counter
om (PWMCount == 250) PWMCount = 0; om räknaren är lika med maximala värde vi vill
Timer1.attachInterrupt(callback); 250 är lika med 100% effekt
}

Under de viktigaste loop stannar roboten söka hinder och beräkning av vägar att undvika dem eller ändra dess sökväg. Om byggherren vill konfigurera potentiella fält kontrollmetod eller ändra robotens beteende, måste han bara arbetar en bättre obst() funktion. Andra delar av programmet är nödvändig och behöver inte ändras. När roboten tar emot ett meddelande via Bluetooth, det slutar att undvika hinder och lyssna bara på användarinmatad.

void loop() //main loop
{
LCD.Clear(); rensar tidigare meddelanden på lcd
Delay(10);
search(); Sök hinder och sparar avståndet värde till varje sensor
Readings(); läser kompass och beräknar fel från dess önskad riktning
calcPWM(); beräknar PWM för varje motor baserat på PID
om (! mensagem) {//message från Bluetooth? om inte, Fortsätt göra detta
Obst(); här är där roboten definierar dess beteende
} //and där den definierar riktningen det vill gå

(...)

Från alla delar av koden, alla av dem är ganska grundläggande, den enda funktion som är intressanta att ta märka här, är readings(). Inuti den hämtar roboten värdet X, Y och Z för den aktuella positionen på den digitala kompassen. Så, hur till vända att läsning till något som gör roboten vet till vilken riktning det är ute?

Först får du raw värdena:

dubbel rawX = event.magnetic.x;
Double rawY = event.magnetic.y;
Double rawZ = event.magnetic.z;

Sedan kontrollera om roboten är på rätt plan. Detta kan vara att få se om Z (vinkelrät axel) är med ett värde mellan ett visst litet urval

om ((rawZ > Zprim + 10) || (rawZ < Zprim - 10)) hemkomst. om inte rätt, avkastning

Nu är det dags att normalisera X och Y-värden till ett utrymme mellan [-1,1], för att beräkna sin() och cos() från dessa värden.

YMin och Ymax, Xmin och Xmax erhålls när kalibreringen sker
Vector [1] = ((rawY) - ((Ymax + Ymin) / 2)) / ((Ymax-Ymin) / 2);
Vector [0] = ((rawX) - ((Xmax + Xmin) / 2)) / ((Xmax-Xmin) / 2);

Vi gör nu vectorOBJ [n] en dubbelriktad vektorn pekar där på planen, vi vill ha roboten att gå. Så, för att hitta felet mellan två vektorer har vi att göra korset produkt mellan dem båda. Det resulterande värdet är en annan vektor, men vinkelrätt mot båda, pekar uppåt eller nedåt, beroende på dess tecken.

fel = (vectorOBJ [0] * vector [0] + vectorOBJ [1] * vector[1]) /
(sqrt (vectorOBJ [0] * vectorOBJ [0] + vectorOBJ [1] * vectorOBJ[1])
* sqrt(vector[0] * vector[0] + vector[1] * vector[1]));

fel = (acos(error)) * 180 / PI; från rad till grader
ta reda på hur resultatvektorn pekar
Sentido = vector [0] * vectorOBJ [1] - vector [1] * vectorOBJ [0];
om (sentido > 0) fel = - fel;