Autonoma Autonavigation Robot (Arduino) (4 / 4 steg)

Steg 4: programmering

Vi använder Arduino Uno programmeringsspråk. Den är baserad på Java.
Här är vår exempelkod för navigering med hjälp av 2D-matriser. Inne i 2D array rutnätet input vi antingen 0 eller 1. En 1 representerar ett objekt eller ett hinder. En 0 representerar ledigt utrymme. Roboten färdas genom 0.

Grid navigering:

DETTA ÄR ETT PROGRAM FÖR ATT KARTLÄGGA ETT OMRÅDE MED 2D MATRISER OCH NAVIGERA SEDAN ENDAST IGENOM DE
NOLLOR NÄRVARANDE. ULTRALJUDSSENSORER ANVÄNDS INTE ÄVEN OM DE HAR INITIERATS.
KODARE ANVÄNDS OCKSÅ INTE UTAN INITIERAS

Wolrath

#include "Servo.h"

Motorer
Servo motor1;
Servo motor2;

Roterande pulsgivare
int encoderAPin = 4; Vänster / / PIN-kod för den roterande encoderA
int encoderBPin = 2; Höger / / PIN-kod för den roterande encoderB
int encoderAPosition = 0; position-mätare för encoderA
int encoderBPosition = 0; position-mätare för encoderB
int signalAInput1; signalingång 1 för encoderA
int signalAInput2; signalingång 2 för encoderA
int signalBInput1; signalingång 1 för encoderB
int signalBInput2; signalingång 2 för encoderB

robot läge service
0 är upp
1 är rätt
2 är nere
3 är kvar
int robotDirection = 2;

Detta är koordinaterna i rutnätet med där roboten är
Det är också x och y index i matrisen.
kom ihåg att matrisen startar vid index 0.
int xcoordinate = 2;
int ycoordinate = 1;

Ultrasonic pins
CONST int Trig_pin = 5; PIN-kod för att utlösa puls INPUT
CONST int Echo_pin = 6; PIN-kod för att ta emot echo OUPUT
lång varaktighet; Hur lång tid det tar för ljudet att återhämta

motoriska pins
CONST int Motor1Pin = 9; Vänster sida
CONST int Motor2Pin = 10; höger sida

matrisen som det spår med
Detta kan vara en matris med valfri storlek
se bara till att roboten har ett fritt utrymme att flytta till från sitt ursprungliga läge.
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}
};

Ogiltiga inställningar () {
Rotationsencoder
pinMode (encoderAPin, ingång);
pinMode (encoderBPin, ingång);
Serial.BEGIN(9600);
Serial.println ("Start nu...");
\

ultraljud
pinMode (Trig_pin, OUTPUT); initiera puls stiftet som utdata:
pinMode (Echo_pin, indata); initiera echo_pin PIN-koden som indata:

motorsmmmmmmmmm
motor1.attach(Motor1Pin);
motor2.attach(Motor2Pin);

}

Detta program ser i svenska, om det finns något framför den.
Om nej, den rör sig rakt. Om ja, kontrollerar sin rätt.
Om sin rätt är gratis, det visar på så sätt, annars, kontrollerar den vänster.
Om vänster är gratis, det visar på så sätt, annars, det visar bara 180 grader
och går tillbaka hur det kom.
//
Bredvid do är att encorporate början och destination samt optimal väg att hitta
//
Senast är att använda det ultraljuds med grid.

Ogiltig loop () {

medan (1 == 1) {
om (isFrontOpen() == true) {
moveForward();
fördröjning (2000);
}
annat
om (isRightOpen() == true) {
turnRight();
fördröjning (2000);
}
annat
om (isLeftOpen() == true) {
turnLeft();
fördröjning (2000);
}
annat {
turnAround();
fördröjning (2000);
}
}
}

Kontrollerar om det finns något precis framför den med galler
booleska isFrontOpen () {
int nextNumber = getFrontNumber();
om (nextNumber == 0) {
return true;
}
annat {
returnera false;
}
}

Kontrollerar om det finns något till höger om den med galler
booleska isRightOpen() {
int nextNumber = getRightNumber();
om (nextNumber == 0) {
return true;
}
annat {
returnera false;
}
}

Kontrollerar om det finns något till vänster om den med galler
booleska isLeftOpen() {
int nextNumber = getLeftNumber();
om (nextNumber == 0) {
return true;
}
annat {
returnera false;
}
}

Rör sig rakt fram.
void moveForward () {
motor1.write(180);
motor2.write(0);

Serial.println("forward");
om (robotDirection == 0)
ycoordinate = ycoordinate - 1;
om (robotDirection == 1)
xcoordinate = xcoordinate + 1.
om (robotDirection == 2)
ycoordinate = ycoordinate + 1.
om (robotDirection == 3)
xcoordinate = xcoordinate - 1;
fördröjning (100);
/ * Serial.print("xcoordinate");
Serial.Print(xcoordinate);
fördröjning (500).
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
fördröjning (500).
Serial.Print ("robot riktning:");
Serial.Print(robotDirection);
Delay(500);
Serial.println ();
Delay(1000);

*/
Delay(800);
}

Vrider 90 grader till höger
void svänghöger () {
motor1.write (60);
motor2.write (60);
Delay(178);
motor2.write(95);
Delay(65);
motor1.write(90);
Serial.println("Right");
om (robotDirection == 0)
robotDirection = 1;
annat if (robotDirection == 1)
robotDirection = 2;
annat if (robotDirection == 2)
robotDirection = 3;
annat if (robotDirection == 3)
robotDirection = 0;
fördröjning (500).
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
fördröjning (500).
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
fördröjning (500).
Serial.Print ("robot riktning:");
Serial.Print(robotDirection);
fördröjning (500).
Serial.println();

Delay(1000);
}

Vrider 90 grader till vänster
void svängvänster () {
motor1.write(120);
motor2.write(120);
Delay(325);
motor2.write(95);
Delay(65);
motor1.write(90);
Serial.println("left");
om (robotDirection == 0)
robotDirection = 3;
annat if (robotDirection == 1)
robotDirection = 0;
annat if (robotDirection == 2)
robotDirection = 1;
annat if (robotDirection == 3)
robotDirection = 2;
fördröjning (500).
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
fördröjning (500).
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
fördröjning (500).
Serial.Print ("robot riktning:");
Serial.Print(robotDirection);
Delay(500);
Serial.println();
Delay(1000);
}

Vänder 180 grader
void vändning () {
Delay(1000);
Serial.println("Around");
om (robotDirection == 0)
robotDirection = 2;
annat if (robotDirection == 1)
robotDirection = 3;
annat if (robotDirection == 2)
robotDirection = 0;
annat if (robotDirection == 3)
robotDirection = 1;
fördröjning (500).
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
fördröjning (500).
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
fördröjning (500).
Serial.Print ("robot riktning:");
Serial.Print(robotDirection);
Delay(500);
Serial.println();

Delay(1000);
}

Får numret på nätet utrymme rätt framför den.
int getFrontNumber() {
om (robotDirection == 0) {
återgå arraything [ycoordinate - 1] [xcoordinate];
}
om (robotDirection == 1) {
återgå arraything [ycoordinate] [xcoordinate + 1];
}
om (robotDirection == 2) {
återgå arraything [ycoordinate + 1] [xcoordinate];
}
om (robotDirection == 3) {
återgå arraything [ycoordinate] [xcoordinate - 1];
}
}

Får numret på nätet av utrymmet till höger om den.
int getRightNumber() {
om (robotDirection == 0) {
återgå arraything [ycoordinate] [xcoordinate + 1];

}
om (robotDirection == 1) {
återgå arraything [ycoordinate + 1] [xcoordinate];

}
om (robotDirection == 2) {
återgå arraything [ycoordinate] [xcoordinate - 1];
}
om (robotDirection == 3) {
återgå arraything [ycoordinate - 1] [xcoordinate];
}
}

Blir numret på nätet av utrymmet till vänster om den.
int getLeftNumber() {
om (robotDirection == 0) {
återgå arraything [ycoordinate] [xcoordinate - 1];
}
om (robotDirection == 1) {
återgå arraything [ycoordinate - 1] [xcoordinate];
}
om (robotDirection == 2) {
återgå arraything [ycoordinate] [xcoordinate + 1];
}
om (robotDirection == 3) {
återgå arraything [ycoordinate + 1] [xcoordinate];
}
}

Här är prov program för ultraljudssensorer. Detta program kommer att bara undvika allting som kommer i vägen.
Ingår i programmet är programmering för en extra servo bara snurrar fram och tillbaka på toppen av roboten. Om du inte gör detta på din robot, ignorera den del av koden, det ska ha någon effekt på ditt program.

DENNA KOD UNDVIKER HINDER
DET KAN ANTINGEN STÖTER PÅ ETT HINDER PÅ HÖGER ELLER VÄNSTER SIDA
DÄRFÖR PROGRAMMERADE VI DET TUR TILL VÄNSTER OM DET UPPSTÅR ETT HINDER TILL HÖGER
OCH RÄTT OM VI STÖTT PÅ DET TILL VÄNSTER

Wolrath

/*
Ultraljud 1
Stift 5 utlöser pulsen (gula leda)
Stift 6 får Echo (Orange leda)

Ultraljud 2
Stift 7 utlöser pulsen (gula leda)
Stift 8 får Echo (Orange leda)
*/

#include "Servo.h"
Servo motorR;
Servo motorL;
Servomotorer;

Ultrasonic pins
ultraljud rätt
CONST int Trig1_pin = 5; PIN-kod för att utlösa puls INPUT
CONST int Echo1_pin = 6; PIN-kod för att ta emot echo OUPUT
lång varaktighet 1; Hur lång tid det tar för ljudet att återhämta

Ultrasonic vänster
CONST int Trig2_pin = 7.
CONST int Echo2_pin = 8;
långa Företagsvaraktighet 2;

motoriska pins
CONST int MotorLPin = 2; Vänster
CONST int MotorRPin = 13. Höger

spinning ultraljudsmotor
CONST int MotorSpinPin = 3;
int spinCounter = 0;

Sensing eller inte
booleska isARightWallThere = false;
booleska isALeftWallThere = false;

vilken väg att vända
booleska höger = sant;

void setup() {
Serial.BEGIN(9600);
Serial.println ("Start");
ultraljud 1
pinMode (Trig1_pin, OUTPUT); initiera puls stiftet som utdata:
pinMode (Echo1_pin, indata); initiera echo_pin PIN-koden som indata:
ultraljud 2
pinMode (7, OUTPUT); initiera puls stiftet som utdata:
pinMode (Echo2_pin, indata); initiera echo_pin PIN-koden som indata:
motorer
motorR.attach(MotorRPin);
motorL.attach(MotorLPin);
motorS.attach(MotorSpinPin);

}

void loop() {
medan (1 == 1) {

om (spinCounter < 2) {
motorS.write(160);
}
annat {
motorS.write(20);
}
om (spinCounter > = 3) {
spinCounter = 0;
}

spinCounter ++;

checkTheRightWall();
checkTheLeftWall();

om (isARightWallThere == false & & isALeftWallThere == false) {
motorR.write(120);
motorL.write(72);

}
annat {

om (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);

}

annat {
om (isARightWallThere == false & & isALeftWallThere == true) {

motorR.write(50);
motorL.write(90);

}
annat {
om (isARightWallThere == true & & isALeftWallThere == false) {

motorR.write(90);
motorL.write(140);
}

}

}

}

}
}

void checkTheRightWall () {
digitalWrite (Trig1_pin, låg);
delayMicroseconds(2);
digitalWrite (Trig1_pin, hög);
delayMicroseconds(10);
digitalWrite (Trig1_pin, låg);
varaktighet 1 = pulseIn(Echo1_pin,10);
Serial.println ("varaktighet 1:");
Serial.println (varaktighet 1, DEC);

om ((varaktighet 1 > 4000 || Företagsvaraktighet 1 == 0)) {
isARightWallThere = false;
}
annat {
isARightWallThere = sant;
}

}

void checkTheLeftWall() {
digitalWrite (7, låg).
delayMicroseconds(2);
digitalWrite (7, hög).
delayMicroseconds(10);
digitalWrite (7, låg).
Företagsvaraktighet 2 = pulseIn(Echo2_pin,10);
Serial.println ("Företagsvaraktighet 2:");
Serial.println (Företagsvaraktighet 2, DEC);

om ((Företagsvaraktighet 2 > 4000 || Företagsvaraktighet 2 == 0)) {
isALeftWallThere = false;
}
annat {
isALeftWallThere = sant;
}

}

Om du har tid, kolla in en av min väns sidor. Han är den som gjorde all programmering för oss.
Här är länken till hans sida:

Slutet
Gnista industrier,