Zelfgemaakte Cleanning Robot met Bluetooth (4 / 5 stap)

Stap 4: De Code

DE BIBLIOTHEKEN:

De "SweeperBot" maakt gebruik van draad, I2Cdev, TimerOne (voor onderbrekingen), Adafruit Sensor, HMC5883 en LCD bibliotheken. U kunt het downloaden van het Zip-bestand, waarin de code voor de Arduino, en alle bibliotheken. Bevat ook de Bluetooth-project voor Windows Phone.

DE WINDOWS PHONE 8.1 CODE:

Het project waarmee de robot via Bluetooth van de Windows Phone werd aangepast van een voorbeeld beschikbaar op het internet. Het werd gewijzigd om vier knoppen, één naar elke richting. Omhoog, omlaag, links en rechts. Elke knop een brief sturen naar de robot, respectievelijk U, O, L en R. Wanneer de robot deze ontvangt, maakt het een van de volgende handelingen:

• U - de robot zal de richting te volgen ongecompliceerd; instellen

• D - de robot zal ingaan op de rug en rechtdoor;

• L - de robot zal de richting te volgen precies 90 graden links van waar het is op dit moment instellen "op zoek";

• R - de robot zal de richting te volgen precies 90 graden vanaf waar het is op dit moment instellen "op zoek";

De manier waarop die de robot deze opdrachten verwerkt kan worden veranderd met de code op de Arduino. Het is mogelijk om toe te voegen meer knoppen om te schakelen van de robot, of om het te stoppen of zelfs om het te maken gaan achteruit. Deze code is slechts een voorbeeld. Het is ook mogelijk te maken van soortgelijke programma's naar andere apparaten, zoals Androids een voor IOS. Het enige wat je hoeft te sturen van de robot zijn deze brieven - U, D, L of R.

 private async void RedButton_Click_1(object sender, RoutedEventArgs e) { string command = "U"; await connectionManager.SendCommand(command); } private async void GreenButton_Click_1(object sender, RoutedEventArgs e) { string command = "L"; await connectionManager.SendCommand(command); } private async void YellowButton_Click_1(object sender, RoutedEventArgs e) { string command = "R"; await connectionManager.SendCommand(command); } private async void RedButton_Copy_Click(object sender, RoutedEventArgs e) { string command = "D"; await connectionManager.SendCommand(command); } 

Voor het implementeren van deze code naar uw Windows Phone zal u Visual Studio en een ontwikkelaar ontgrendeld telefoon. Dit kan worden gedaan na de instructies op deze koppeling.

DE ARDUINO-CODE:

In samenvatting is de belangrijkste logica van de robot als volgt:

1. Afstand tot het dichter bij object gevonden in elk van de 6 possibles sensoren;
2. Gedrag van de robot proces afstanden en punt gewenste richting met een vector X, Y (echter als een bericht wordt ontvangen van de smartphone, stap 2 is vervangen door de richting gestuurd van het apparaat);
3. Digitale kompas geeft gaat het richting de robot, om te controleren de wielen;
4. Informatie X', Y' van kompas is genormaliseerd naar ruimte [-1,1];
5. Kruisproduct tussen vector van gewenste richting en de werkelijke richting vindt fout tussen hen;
6. PID-methode berekent PWM (kracht) elke gelijkstroommotor voor de twee wielen (2WD);
7. Onderbreking activeert PWM;

Als het project geëvolueerd, ik heb de standaard bibliotheek beschikbaar op GitHub ontdekt voor de ultrasone sensoren conflicteert met de PWM-activering van de motoren, waren omdat ze beide de zelfde timer gebruikt. Als dit project gebruiken 5 pinnen voor het configureren van de PWM van de drie DC-motoren (en dit alleen mogelijk is omdat het tapijt sweeper alleen om één richting draait), en meer 7 naar de sensoren, die het micro-controller moet worden voortdurend te controleren als de lezing werd ontvangen, was het niet kunnen gebruiken van het materiaal beschikbaar op internet.

Dus, om conflicten te voorkomen, deze is geconfigureerd alleen de timer 1-interrupt. De Arduino via deze onderbreking, kunt controleren of er lezingen op elke sensor was: als niet, het verhoogt een variabele aan elk van hen, die worden gebruikt als parameter voor het vinden van de afstand tot het object.

 void callback() { //callback is the interruption name noInterrupts(); //if no reading, increases variable sonarCount[n], where n is the number of sensor if ((sonar[0] == 1) && (digitalRead(ECHO))) sonarCount[0]++; //if there is readind on that sensor, sonar[n] = 2 accuses object found if ((sonar[0] == 1) && (!digitalRead(ECHO)) && (sonarCount[0] > 0)) sonar[0] = 2; 

Ook binnen deze onderbreking gooit het volle kracht aan de motor. Als er al een aantal onderbrekingen die groter is dan het aantal PWM_R of PWM_L variabelen, dan gooit het nul potentie die motor.

 if (PWMCount == 0) { if (PWML) { //if pwm left is positive then turn to a certain way digitalWrite(MOTOR + 2, HIGH); digitalWrite(MOTOR + 3, LOW); } else { //else turn to the other side the left motor digitalWrite(MOTOR + 2, LOW); digitalWrite(MOTOR + 3, HIGH); } (...) if (PWMCount >= abs(PWM_L)) { //if the counter equals the power we want digitalWrite(MOTOR + 2, LOW); //shut down both pins of this motor digitalWrite(MOTOR + 3, LOW); } (...) PWMCount++; //increases counter if (PWMCount == 250) PWMCount = 0; //if counter equals maximum value we want Timer1.attachInterrupt(callback); //250 equals 100% power } 

Tijdens de hoofdlus blijft de robot zoeken van obstakels en berekenen van routes te vermijden of te wijzigen het pad. Als de ontwikkelaar wil potentiële velden controle methode configureren of wijzigen van de robot gedrag, hij gewoon moeten werken een beter obst()-functie. De andere onderdelen van het programma noodzakelijk zijn en hoeft niet te worden gewijzigd. Wanneer de robot ontvangt een bericht via Bluetooth, het stopt met het vermijden van hindernissen en luister gewoon voor gebruikersinvoer.

 void loop() //main loop { lcd.clear(); //clears previous messages on lcd delay(10); search(); //search obstacles and saves the distance value to each sensor readings(); //reads compass and calculates the error from its desired direction calcPWM(); //calculates the PWM for each motor based on PID if (!mensagem) { //message from Bluetooth? if not, continue doing this obst(); //here is where the robot defines its behavior } //and where it defines the direction it want to go (...) 

Uit alle delen van de code, alle van hen vrij basic zijn, de slechts één functie die is interessant om te nemen nota hier, is de readings(). Erin haalt de robot de waarde op X, Y en Z voor de huidige positie op het digitale kompas. Dus, hoe om te zetten die lezing in iets dat maakt de robot weet aan welke richting het is op zoek?

Eerst krijg je de ruwe waarden:

 double rawX = event.magnetic.x; double rawY = event.magnetic.y; double rawZ = event.magnetic.z; 

Vervolgens controleren of de robot op het juiste vliegtuig. Dit kan worden zien als de Z (loodrechte as) met een waarde tussen een bepaald klein bereik

 if ((rawZ > Zprim + 10) || (rawZ < Zprim - 10)) return; //if not right, return 

Nu is het tijd om te normaliseren van de X- en Y-waarden met een spatie tussen [-1,1], om te berekenen sin() en cos() van deze waarden.

 //Ymin, Ymax, Xmin and Xmax are obtained when the calibration occurs vector[1] = ((rawY) - ((Ymax + Ymin) / 2)) / ((Ymax - Ymin) / 2); vector[0] = ((rawX) - ((Xmax + Xmin) / 2)) / ((Xmax - Xmin) / 2); 

We maken nu vectorOBJ [n] een bidirectionele vector naar waar, over het plan, we willen de robot om te gaan. We moeten dus, om te zoeken naar de fout tussen twee vectoren maken de cross product tussen hen beide. De resultaatwaarde is een andere aanvalsvector, maar loodrecht op beide wijzen omhoog of omlaag, afhankelijk van het teken.

 error = (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])); error = (acos(error)) * 180 / PI; //from rad to degrees //find out the way the result vector is pointing sentido = vector[0] * vectorOBJ[1] - vector[1] * vectorOBJ[0]; if (sentido > 0) error = -error;