Paso 4: programación
Aquí es nuestro código de ejemplo para navegación mediante matrices 2D. Interior de la rejilla de matriz 2D, tenemos de entrada un 0 o un 1. Un 1 representa un objeto o un obstáculo. Un 0 representa el espacio libre. El robot recorre el 0.
Navegación de red:
SE TRATA DE UN PROGRAMA DE UN ÁREA CON MATRICES 2D Y LUEGO NAVEGAR SÓLO A TRAVÉS DEL
CEROS PRESENTES. SENSORES ULTRASÓNICOS NO SE UTILIZAN AUNQUE SE INICIALIZAN.
LOS CODIFICADORES TAMBIÉN NO SE UTILIZAN PERO SE INICIALIZAN
Pranav
#include "Servo.h"
Motores
Motor1 servo;
Motor2 servo;
Codificadores rotativos
int encoderAPin = 4; Izquierda / / perno para el encoderA rotatorio
int encoderBPin = 2; Derecho / / perno para el encoderB rotatorio
int encoderAPosition = 0; contador de posición de encoderA
int encoderBPosition = 0; contador de posición de encoderB
int signalAInput1; entrada de señal 1 para encoderA
int signalAInput2; entrada de señal 2 para encoderA
int signalBInput1; entrada de señal 1 para encoderB
int signalBInput2; entrada de señal 2 para encoderB
servicio de localización de robot
0 es arriba
1 es correcto
2 es abajo
3 queda
int robotDirection = 2;
se trata de las coordenadas de la cuadrícula de Dónde está el robot
también es el x y y los índices de la matriz.
recordar que el array comienza en el índice 0.
int xcoordinate = 2;
int ycoordinate = 1;
pernos ultrasónicos
const int Trig_pin = 5; PIN de entrada de pulso de disparo
const int Echo_pin = 6; PIN para recibir eco OUPUT
de larga duración; Cuánto tiempo tarda el sonido de rebote
pernos de motor
const int Motor1Pin = 9; Lado izquierdo
const int Motor2Pin = 10; lado derecho
la matriz que realiza un seguimiento con
Esto puede ser un array de cualquier tamaño
sólo asegúrese de que el robot tiene un espacio libre para moverse a su posición inicial.
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 () {}
Codificador rotatorio
pinMode (encoderAPin, entrada);
pinMode (encoderBPin, entrada);
Serial.Begin(9600);
Serial.println ("a partir de ahora...");
\
ultrasónico
pinMode (Trig_pin, salida); Inicializa el pin de pulso como salida:
pinMode (Echo_pin, entrada); inicializar el echo_pin pin como una entrada:
motorsmmmmmmmmm
motor1.Attach(Motor1Pin);
motor2.Attach(Motor2Pin);
}
En inglés, este programa considera que si hay algo delante de ella.
Si no, que se mueve recto. En caso afirmativo, comprueba su derecho.
Si su derecho es libre, se convierte de esa manera, o bien, comprueba su izquierda.
Si la izquierda está libre, se convierte de esa manera, o bien, da vuelta a 180 grados
y vuelve la forma en llegó.
//
Junto a do es encorporate principio y destino así como encontrar la ruta óptima
//
Último es utilizar los ultrasonidos con la rejilla.
void loop () {}
mientras (1 == 1) {}
Si (isFrontOpen() == true) {}
moveForward();
retrasar (2000);
}
otra cosa
Si (isRightOpen() == true) {}
turnRight();
retrasar (2000);
}
otra cosa
Si (isLeftOpen() == true) {}
turnLeft();
retrasar (2000);
}
Else {}
turnAround();
retrasar (2000);
}
}
}
Comprueba si hay algo justo en frente con las rejillas
Boolean isFrontOpen () {}
int nextNumber = getFrontNumber();
Si (nextNumber == 0) {}
verdaderas;
}
Else {}
devuelven el valor false;
}
}
Comprueba si hay algo a la derecha usando rejillas
Boolean isRightOpen() {}
int nextNumber = getRightNumber();
Si (nextNumber == 0) {}
verdaderas;
}
Else {}
devuelven el valor false;
}
}
Comprueba si hay algo a la izquierda usando rejillas
Boolean isLeftOpen() {}
int nextNumber = getLeftNumber();
Si (nextNumber == 0) {}
verdaderas;
}
Else {}
devuelven el valor false;
}
}
Se mueve hacia adelante.
void moveForward () {}
motor1.Write(180);
motor2.Write(0);
Serial.println("forward");
Si (robotDirection == 0)
ycoordinate = ycoordinate - 1;
Si (robotDirection == 1)
xcoordinate = xcoordinate + 1;
Si (robotDirection == 2)
ycoordinate = ycoordinate + 1;
Si (robotDirection == 3)
xcoordinate = xcoordinate - 1;
Delay (100);
/ * Serial.print("xcoordinate");
Serial.Print(xcoordinate);
Delay (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
Delay (500);
Serial.Print ("dirección robot:");
Serial.Print(robotDirection);
Delay(500);
Serial.println ();
Delay(1000);
*/
Delay(800);
}
Gira 90 grados a la derecha
turnRight nula () {}
motor1.Write (60);
motor2.Write (60);
Delay(178);
motor2.Write(95);
Delay(65);
motor1.Write(90);
Serial.println("Right");
Si (robotDirection == 0)
robotDirection = 1;
else if (robotDirection == 1)
robotDirection = 2;
else if (robotDirection == 2)
robotDirection = 3;
else if (robotDirection == 3)
robotDirection = 0;
Delay (500);
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
Delay (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
Delay (500);
Serial.Print ("dirección robot:");
Serial.Print(robotDirection);
Delay (500);
Serial.println();
Delay(1000);
}
Gira 90 grados a la izquierda
vacío de la izquierda () {}
motor1.Write(120);
motor2.Write(120);
Delay(325);
motor2.Write(95);
Delay(65);
motor1.Write(90);
Serial.println("left");
Si (robotDirection == 0)
robotDirection = 3;
else if (robotDirection == 1)
robotDirection = 0;
else if (robotDirection == 2)
robotDirection = 1;
else if (robotDirection == 3)
robotDirection = 2;
Delay (500);
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
Delay (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
Delay (500);
Serial.Print ("dirección robot:");
Serial.Print(robotDirection);
Delay(500);
Serial.println();
Delay(1000);
}
Gira 180 grados
Respuesta nula () {}
Delay(1000);
Serial.println("Around");
Si (robotDirection == 0)
robotDirection = 2;
else if (robotDirection == 1)
robotDirection = 3;
else if (robotDirection == 2)
robotDirection = 0;
else if (robotDirection == 3)
robotDirection = 1;
Delay (500);
Serial.Print("xcoordinate");
Serial.Print(xcoordinate);
Delay (500);
Serial.Print ("ycoordinate");
Serial.Print(ycoordinate);
Delay (500);
Serial.Print ("dirección robot:");
Serial.Print(robotDirection);
Delay(500);
Serial.println();
Delay(1000);
}
Obtiene el número en la cuadrícula de la derecha del espacio delante de ella.
int getFrontNumber() {}
Si (robotDirection == 0) {}
volver arraything [ycoordinate - 1] [xcoordinate];
}
Si (robotDirection == 1) {}
volver arraything [ycoordinate] [xcoordinate + 1];
}
Si (robotDirection == 2) {}
volver arraything [ycoordinate + 1] [xcoordinate];
}
Si (robotDirection == 3) {}
volver arraything [ycoordinate] [xcoordinate - 1];
}
}
Obtiene el número en la cuadrícula del espacio a la derecha de él.
int getRightNumber() {}
Si (robotDirection == 0) {}
volver arraything [ycoordinate] [xcoordinate + 1];
}
Si (robotDirection == 1) {}
volver arraything [ycoordinate + 1] [xcoordinate];
}
Si (robotDirection == 2) {}
volver arraything [ycoordinate] [xcoordinate - 1];
}
Si (robotDirection == 3) {}
volver arraything [ycoordinate - 1] [xcoordinate];
}
}
Obtiene el número en la cuadrícula del espacio a la izquierda de él.
int getLeftNumber() {}
Si (robotDirection == 0) {}
volver arraything [ycoordinate] [xcoordinate - 1];
}
Si (robotDirection == 1) {}
volver arraything [ycoordinate - 1] [xcoordinate];
}
Si (robotDirection == 2) {}
volver arraything [ycoordinate] [xcoordinate + 1];
}
Si (robotDirection == 3) {}
volver arraything [ycoordinate + 1] [xcoordinate];
}
}
Aquí es un programa de muestra para los sensores ultrasónicos. Este programa sólo evitará cualquier cosa y todo lo que se pone en el camino.
Incluido en el programa es la programación para un servo adicional simplemente girando hacia adelante y hacia atrás en la parte superior del robot. Si no están haciendo esto en su robot, ignorar esa parte del código, no debe tener ningún efecto en su programa.
ESTE CÓDIGO EVITA OBSTÁCULOS
QUE CUALQUIERA PUEDE ENCONTRAR UN OBSTÁCULO EN LA DERECHA O LA IZQUIERDA
POR LO TANTO, HEMOS PROGRAMADO PARA GIRAR A LA IZQUIERDA SI ENCUENTRA UN OBSTÁCULO A LA DERECHA
Y SI ENCONTRAMOS A LA IZQUIERDA A LA DERECHA
Pranav
/*
1 ultrasonido
Pin 5 dispara el pulso (cable amarillo)
PIN 6 recibe el eco (cable naranja)
2 ultrasonidos
PIN 7 dispara el pulso (cable amarillo)
Pin 8 recibe el eco (cable naranja)
*/
#include "Servo.h"
MotorR del servo;
MotorL servo;
Servo motores;
pernos ultrasónicos
Derecho de ultrasonidos
const int Trig1_pin = 5; PIN de entrada de pulso de disparo
const int Echo1_pin = 6; PIN para recibir eco OUPUT
largo duration1; Cuánto tiempo tarda el sonido de rebote
Izquierda ultrasónico
const int Trig2_pin = 7;
const int Echo2_pin = 8;
largo duration2;
pernos de motor
const int MotorLPin = 2; Izquierda
const int MotorRPin = 13; Derecho
motor ultrasónico de spinning
const int MotorSpinPin = 3;
int spinCounter = 0;
detección o no
Boolean isARightWallThere = false;
Boolean isALeftWallThere = false;
que manera de convertir
Boolean correcto = true;
void setup() {}
Serial.Begin(9600);
Serial.println ("desde");
1 ultrasonido
pinMode (Trig1_pin, salida); Inicializa el pin de pulso como salida:
pinMode (Echo1_pin, entrada); inicializar el echo_pin pin como una entrada:
2 ultrasonidos
pinMode (7, salida); Inicializa el pin de pulso como salida:
pinMode (Echo2_pin, entrada); inicializar el echo_pin pin como una entrada:
motores
motorR.attach(MotorRPin);
motorL.attach(MotorLPin);
motorS.attach(MotorSpinPin);
}
void loop() {}
mientras (1 == 1) {}
Si (spinCounter < 2) {}
motorS.write(160);
}
Else {}
motorS.write(20);
}
Si (spinCounter > = 3) {}
spinCounter = 0;
}
spinCounter ++;
checkTheRightWall();
checkTheLeftWall();
Si (isARightWallThere == false & & isALeftWallThere == false) {}
motorR.write(120);
motorL.write(72);
}
Else {}
Si (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 {}
Si (isARightWallThere == false & & isALeftWallThere == true) {}
motorR.write(50);
motorL.write(90);
}
Else {}
Si (isARightWallThere == true & & isALeftWallThere == false) {}
motorR.write(90);
motorL.write(140);
}
}
}
}
}
}
void checkTheRightWall () {}
digitalWrite (Trig1_pin, bajo);
delayMicroseconds(2);
digitalWrite (Trig1_pin, alto);
delayMicroseconds(10);
digitalWrite (Trig1_pin, bajo);
duration1 = pulseIn(Echo1_pin,10);
Serial.println ("Duration1:");
Serial.println (duration1, DEC);
Si ((duration1 > 4000 || duration1 == 0)) {}
isARightWallThere = false;
}
Else {}
isARightWallThere = true;
}
}
void checkTheLeftWall() {}
digitalWrite (7, bajo);
delayMicroseconds(2);
digitalWrite (7, alto);
delayMicroseconds(10);
digitalWrite (7, bajo);
duration2 = pulseIn(Echo2_pin,10);
Serial.println ("Duration2:");
Serial.println (duration2, DEC);
Si ((duration2 > 4000 || duration2 == 0)) {}
isALeftWallThere = false;
}
Else {}
isALeftWallThere = true;
}
}
Si tienes tiempo, revisa una de las páginas de mi amigo. Él es el que hizo toda la programación para nosotros.
Aquí está el enlace a su página:
Fin
Industrias, la chispa
