Stap 4: programmeren
Hier is onze voorbeeldcode voor navigatie met behulp van 2D-arrays. Binnen het nummerraster van 2D-matrix voeren we een 0 of een 1. Een 1 staat voor een object of een obstakel. Een 0 geeft vrije ruimte. De robot reist via de 0's.
Raster navigatie:
DIT IS EEN PROGRAMMA OM IN KAART TE BRENGEN EEN GEBIED MET 2D-ARRAYS EN GA ALLEEN DOOR HET
NULLEN AANWEZIG. ULTRASONE SENSOREN WORDEN NIET GEBRUIKT, HOEWEL ZE ZIJN GEÏNITIALISEERD.
DE ENCODERS WORDEN OOK NIET GEBRUIKT MAAR WORDEN GEÏNITIALISEERD
POTS
#include "Servo.h"
Motoren
Servo motor1;
Servo motor2;
Rotary Encoders
int encoderAPin = 4; Links / / pin voor de roterende encoderA
int encoderBPin = 2; Recht / / pin voor de roterende encoderB
int encoderAPosition = 0; positie teller voor encoderA
int encoderBPosition = 0; positie teller voor encoderB
int signalAInput1; Signaalinvoer 1 voor encoderA
int signalAInput2; signaalingang 2 voor encoderA
int signalBInput1; Signaalinvoer 1 voor encoderB
int signalBInput2; signaalingang 2 voor encoderB
robot locatie service
0 is
1 klopt
2 is ingedrukt
3 wordt gelaten
int robotDirection = 2;
Dit is de coördinaten in het raster van waar de robot is
het is ook de x en y indexen in de matrix.
Vergeet niet dat de matrix op index 0 begint.
int xcoordinate = 2;
int ycoordinate = 1;
Ultrasone pinnen
Const int Trig_pin = 5; PIN voor aanstuurimpuls INPUT
Const int Echo_pin = 6; PIN voor het ontvangen van echo output
lange duur; Hoe lang het duurt voordat het geluid rebound
motor pinnen
Const int Motor1Pin = 9; Linkerzijde
Const int Motor2Pin = 10; rechterkant
de matrix die hierin met
Dit kan een matrix van elke omvang
Zorg enkel ervoor dat de robot een vrije ruimte heeft om naar van haar aanvankelijke standpunt.
int arraything [6] [6] =
{
{1,1,1,1,1,1}
,
{1,1,0,1,0,1}
,
{1,1,0,1,0,1}
,
{1,1,0,1,0,1}
,
{1,1,0,1,1,1}
,
{1,1,1,0,1,1}
};
void setup () {}
Roterende encoder
pinMode (encoderAPin, INPUT);
pinMode (encoderBPin, INPUT);
Serial.begin(9600);
Serial.println ("vanaf nu...");
\
ultrasone
pinMode (Trig_pin, uitvoer); Initialiseer de puls-pin als output:
pinMode (Echo_pin, INPUT); Initialiseer de echo_pin pin als input:
motorsmmmmmmmmm
motor1.attach(Motor1Pin);
Motor2.attach(Motor2Pin);
}
In het Engels ziet dit programma als er iets achter zit.
Zo nee, beweegt hij rechtdoor. Zo ja, controleert het haar recht.
Als het recht vrij is, blijkt dat manier, of anders, gecontroleerd links daarvan.
Als links daarvan gratis is, blijkt dat manier, of anders, blijkt slechts 180 graden
en de manier het kwam teruggaat.
//
Naast ken is het encorporate begin en bestemming alsmede optimale pad vinden
//
Laatst is het gebruik van de ultrasone met het raster.
void loop () {}
terwijl (1 == 1) {}
Als (isFrontOpen() == true) {}
moveForward();
vertraging (2000);
}
anders
Als (isRightOpen() == true) {}
turnRight();
vertraging (2000);
}
anders
Als (isLeftOpen() == true) {}
turnLeft();
vertraging (2000);
}
else {}
turnAround();
vertraging (2000);
}
}
}
Controleert of er iets recht tegenover het met behulp van rasters
Booleaanse isFrontOpen () {}
int nextNumber = getFrontNumber();
Als (nextNumber == 0) {}
return true;
}
else {}
return false;
}
}
Controleert of er iets aan de rechterkant van het gebruik van rasters
Booleaanse isRightOpen() {}
int nextNumber = getRightNumber();
Als (nextNumber == 0) {}
return true;
}
else {}
return false;
}
}
Controleert of er iets aan de linkerkant van het gebruik van rasters
Booleaanse isLeftOpen() {}
int nextNumber = getLeftNumber();
Als (nextNumber == 0) {}
return true;
}
else {}
return false;
}
}
Ongecompliceerd beweegt.
VOID moveForward () {}
motor1.write(180);
Motor2.write(0);
Serial.println("Forward");
Als (robotDirection == 0)
ycoordinate = ycoordinate - 1;
Als (robotDirection == 1)
xcoordinate = xcoordinate + 1;
Als (robotDirection == 2)
ycoordinate = ycoordinate + 1;
Als (robotDirection == 3)
xcoordinate = xcoordinate - 1;
vertraging (100);
/ * Serial.print("xcoordinate");
Serial.Print(xcoordinate);
vertraging (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
vertraging (500);
Serial.Print ("robot richting:");
Serial.Print(robotDirection);
delay(500);
Serial.println ();
delay(1000);
*/
delay(800);
}
Bochten 90 graden naar rechts
VOID turnRight () {}
motor1.write (60);
Motor2.write (60);
delay(178);
Motor2.write(95);
delay(65);
motor1.write(90);
Serial.println("right");
Als (robotDirection == 0)
robotDirection = 1;
else if (robotDirection == 1)
robotDirection = 2;
else if (robotDirection == 2)
robotDirection = 3;
else if (robotDirection == 3)
robotDirection = 0;
vertraging (500);
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
vertraging (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
vertraging (500);
Serial.Print ("robot richting:");
Serial.Print(robotDirection);
vertraging (500);
Serial.println();
delay(1000);
}
Bochten 90 graden naar links
VOID turnLeft () {}
motor1.write(120);
Motor2.write(120);
delay(325);
Motor2.write(95);
delay(65);
motor1.write(90);
Serial.println("left");
Als (robotDirection == 0)
robotDirection = 3;
else if (robotDirection == 1)
robotDirection = 0;
else if (robotDirection == 2)
robotDirection = 1;
else if (robotDirection == 3)
robotDirection = 2;
vertraging (500);
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
vertraging (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
vertraging (500);
Serial.Print ("robot richting:");
Serial.Print(robotDirection);
delay(500);
Serial.println();
delay(1000);
}
Bochten 180 graden
VOID ommekeer () {}
delay(1000);
Serial.println("Around");
Als (robotDirection == 0)
robotDirection = 2;
else if (robotDirection == 1)
robotDirection = 3;
else if (robotDirection == 2)
robotDirection = 0;
else if (robotDirection == 3)
robotDirection = 1;
vertraging (500);
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
vertraging (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
vertraging (500);
Serial.Print ("robot richting:");
Serial.Print(robotDirection);
delay(500);
Serial.println();
delay(1000);
}
Deze eigenschap haalt het nummer op het rooster van de ruimte recht tegenover het.
int getFrontNumber() {}
Als (robotDirection == 0) {}
arraything [ycoordinate - 1] terug [xcoordinate];
}
Als (robotDirection == 1) {}
keren arraything [ycoordinate] [xcoordinate + 1];
}
Als (robotDirection == 2) {}
arraything keren [ycoordinate + 1] [xcoordinate];
}
Als (robotDirection == 3) {}
keren arraything [ycoordinate] [xcoordinate - 1];
}
}
Deze eigenschap haalt het nummer op het rooster van de ruimte rechts van het.
int getRightNumber() {}
Als (robotDirection == 0) {}
keren arraything [ycoordinate] [xcoordinate + 1];
}
Als (robotDirection == 1) {}
arraything keren [ycoordinate + 1] [xcoordinate];
}
Als (robotDirection == 2) {}
keren arraything [ycoordinate] [xcoordinate - 1];
}
Als (robotDirection == 3) {}
arraything [ycoordinate - 1] terug [xcoordinate];
}
}
Deze eigenschap haalt het nummer op het rooster van de ruimte aan de linkerkant van het.
int getLeftNumber() {}
Als (robotDirection == 0) {}
keren arraything [ycoordinate] [xcoordinate - 1];
}
Als (robotDirection == 1) {}
arraything [ycoordinate - 1] terug [xcoordinate];
}
Als (robotDirection == 2) {}
keren arraything [ycoordinate] [xcoordinate + 1];
}
Als (robotDirection == 3) {}
arraything keren [ycoordinate + 1] [xcoordinate];
}
}
Hier is een steekproefprogramma voor de ultrasone sensoren. Dit programma zal net voorkomen dat alles en nog wat die in de weg.
Opgenomen in het programma is de programmering voor een extra servo gewoon heen en weer draaien op de bovenkant van de robot. Als je dit niet doet op uw robot, negeren dat deel van de code, het moet geen invloed hebben op uw programma.
DEZE CODE VERMIJDT OBSTAKELS
HET KAN OFWEL EEN OBSTAKEL OP HET RECHT OF LINKS TEGENKOMEN
DUS WIJ GEPROGRAMMEERD HET DRAAI NAAR LINKS ALS HET AANGETROFFEN OBSTAKEL AAN DE RECHTERKANT
EN ALS WE AAN DE LINKERKANT ONDERVONDEN
POTS
/*
Ultrasone 1
Pin 5 Triggers de pols (gele lood)
Pin 6 ontvangt de Echo (oranje lood)
Ultrasone 2
Pin 7 Triggers de pols (gele lood)
Pin 8 ontvangt de Echo (oranje lood)
*/
#include "Servo.h"
Servo motorR;
Servo motorL;
Servomotoren;
Ultrasone pinnen
Ultrasone recht
Const int Trig1_pin = 5; PIN voor aanstuurimpuls INPUT
Const int Echo1_pin = 6; PIN voor het ontvangen van echo output
lange Duur1; Hoe lang het duurt voordat het geluid rebound
Ultrasone links
Const int Trig2_pin = 7;
Const int Echo2_pin = 8;
lange Duur2;
motor pinnen
Const int MotorLPin = 2; Links
Const int MotorRPin = 13; Rechts
draaien van de ultrasone motor
Const int MotorSpinPin = 3;
int spinCounter = 0;
Sensing of niet
Boole isARightWallThere = false;
Boole isALeftWallThere = false;
welke manier om te schakelen
Booleaanse rechts = true;
VOID Setup {}
Serial.begin(9600);
Serial.println ("vanaf");
Ultrasone 1
pinMode (Trig1_pin, uitvoer); Initialiseer de puls-pin als output:
pinMode (Echo1_pin, INPUT); Initialiseer de echo_pin pin als input:
Ultrasone 2
pinMode (7, OUTPUT); Initialiseer de puls-pin als output:
pinMode (Echo2_pin, INPUT); Initialiseer de echo_pin pin als input:
motoren
motorR.attach(MotorRPin);
motorL.attach(MotorLPin);
motorS.attach(MotorSpinPin);
}
void loop {}
terwijl (1 == 1) {}
Als (spinCounter < 2) {}
motorS.write(160);
}
else {}
motorS.write(20);
}
Als (spinCounter > = 3) {}
spinCounter = 0;
}
spinCounter ++;
checkTheRightWall();
checkTheLeftWall();
Als (isARightWallThere == false & & isALeftWallThere == false) {}
motorR.write(120);
motorL.write(72);
}
else {}
Als (isARightWallThere == true & & isALeftWallThere == true) {}
motorR.write(0);
motorL.write(180);
delay(300);
motorR.write(80);
motorL.write(80);
delay(500);
motorR.write(0);
motorL.write(180);
}
else {}
Als (isARightWallThere == false & & isALeftWallThere == true) {}
motorR.write(50);
motorL.write(90);
}
else {}
Als (isARightWallThere == true & & isALeftWallThere == false) {}
motorR.write(90);
motorL.write(140);
}
}
}
}
}
}
VOID checkTheRightWall () {}
digitalWrite (Trig1_pin, laag);
delayMicroseconds(2);
digitalWrite (Trig1_pin, hoge);
delayMicroseconds(10);
digitalWrite (Trig1_pin, laag);
Duur1 = pulseIn(Echo1_pin,10);
Serial.println ("Duur1:");
Serial.println (Duur1, DEC);
Als ((Duur1 > 4000 || Duur1 == 0)) {}
isARightWallThere = false;
}
else {}
isARightWallThere = true;
}
}
ongeldig checkTheLeftWall() {}
digitalWrite (7, laag);
delayMicroseconds(2);
digitalWrite (7, hoge);
delayMicroseconds(10);
digitalWrite (7, laag);
veld Duur2 = pulseIn(Echo2_pin,10);
Serial.println ("veld Duur2:");
Serial.println (Duur2, DEC);
Als ((Duur2 > 4000 || Duur2 == 0)) {}
isALeftWallThere = false;
}
else {}
isALeftWallThere = true;
}
}
Als je de tijd hebt, check out een van mijn vriend's. Hij is degene die alle de programmering voor ons deed.
Hier is de link naar zijn pagina:
Het einde
Vonk Industries,
