Paso 3: Diagrama de flujo y código
En primer lugar que comenzamos con la forma del diamante, la forma del diamante representa un cheque. Para esta prueba estamos viendo si hay un objeto más cerca de 30 cm para nuestro robot. La comprobación tiene dos posibles resultados, sí o no. Sí, lo que significa que hay hecho algún objeto está más cerca de 30 cm. No, lo que significa que no hay ningún objeto detectado dentro de los 30 cm.
Si no hay nada dentro de los 30 cm que el robot puede simplemente mueva hacia adelante el camino es claro. Si hay algo menos de 30 cm el robot debe realizar maniobras de evitación del obstáculo.
La primera etapa de evitación del obstáculo es parar el robot! Si no deja el robot inmediatamente caerá!
¡ UF! Fue espectacular.
Después de que el robot ha detenido tiene que ver de qué manera debe ir. Esto logra mirando ambas direcciones, tanto como que si al cruzar la carretera. Primero el robot gira a la izquierda, toma una lectura, gira a la derecho, toma una lectura.
Otra comprobación se presenta ver qué dirección es la mejor manera de ir. Si izquierda es el camino a seguir tiene que dar marcha atrás a la izquierda y luego seguir adelante. Si bien es el camino a seguir el robot simplemente avanza que ya está mirando en la dirección correcta. Así que fue bastante fácil ¿no? Todo lo que tenemos que hacer ahora es convertir ese diagrama en código de Arduino y eso es todo, código de evitación del obstáculo hecho!
#include
#include
////////////////////////////////////////////////servo motor and sonar sensor ////////////////////////////////////////////
const int dangerdistance = 30; distancia de seguridad para panMotor de Servo de obstáculos (en cm); crea un objeto servo #define 3 TRIGGER_PIN / / pin Arduino atado para activar el pin en el sensor de ultrasonidos. #define ECHO_PIN 2 / / atado de Arduino pin a pin en el sensor ultrasónico del eco. #define MAX_distance 200 / / distancia máxima queremos ping para (en centímetros). Distancia máxima del sensor es clasificada en 400-500 cm. sonar de NewPing (TRIGGER_PIN, ECHO_PIN, MAX_distance); NewPing instalación de pernos y distancia máxima. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////// MOTOR //////////////////////////////////////////////////////////////////
*
Este código realiza algunas maniobras sencillas para ilustrar las funciones: - motorDrive (motorNumber, motorDirection, motorSpeed) - motorBrake(motorNumber) - motorStop(motorNumber) - motorsStandby
Conexiones: - Pin 5---> PWMA - Pin 7---> AIN2 - Pin 8---> AIN1 - Pin 11---> STBY - Pin 12---> BIN1 - Pin 13---> BIN2 - Pin 6---> PWMB
-Motor 1: A01 y A02 - Motor 2: B01 y B02
*
Definir los pines
Motor 1 int pinAIN1 = 8; Dirección int pinAIN2 = 7; Dirección int pinPWMA = 5; Velocidad
Motor 2 int pinBIN1 = 12; Dirección int pinBIN2 = 13; Dirección int pinPWMB = 6; Velocidad
PinSTBY int espera = 11;
Constantes para ayudar a recordar lo parámetros estáticos boolean turnCW = 0; para motorDrive función static boolean turnCCW = 1; para motorDrive función static boolean motor1 = 0; para motorDrive, motorStop, introduce funciones static boolean motor2 = 1; para motorDrive motorStop, introduce funciones
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// unsigned long time; contador de tiempo para hacer de boucle... mientras timeplus largo sin firmar; / / temporizador para boucle hacer... mientras
int distancia; int; void setup() {//Set el PIN modos motor pinMode (pinPWMA, salida); pinMode (pinAIN1, salida); pinMode (pinAIN2, salida);
pinMode (pinPWMB, salida); pinMode (pinBIN1, salida); pinMode (pinBIN2, salida); pinMode (pinSTBY, salida); Serial.Begin(9600); panMotor.write(90); Delay(15000); / / espera hasta que panMotor.attach(4) listo; se fija el servo en el pin 9 al objeto servo}
//////////////////////////////////////////////////////////LOOP//////////////////////////////////////////////////
void loop() {panMotor.write(90); / / convertir un sensor sonar a 90 grados
Delay(500); sensor de sonar ping hacernos distancia int = sonar.ping(); int centerdistance = uS / US_ROUNDTRIP_CM; if(centerdistance==0) {centerdistance = 200;} Fix newping biblioteca equivocado
Si (centerdistance > dangerdistance) ruta //if es claro {/ / *** mientras ningún obstáculo en su frente móvil y CHECK FOR obstáculo cada 100 MS ***
hacer / / (mientras que el sensor da menos de robot de mantener 30cm avanzar) {int nos = sonar.ping(); int centerdistance = uS / US_ROUNDTRIP_CM; / / comprobar los sensores delay(100); / / esperar a que los sensores se estabilicen motorDrive (motor1, turnCW, 150); motorDrive (motor2, turnCW, 150);} mientras que (centerdistance < dangerdistance);
} / /---pathe está bloqueado---else //if camino es bloqueado {motorBrake(motor1); motorBrake(motor2); si (centerdistance
panMotor.write(175); / / girar a la izquierda delay(900); int leftdistance = sonar.ping () / US_ROUNDTRIP_CM; exploración a la izquierda delay(500); if(leftdistance==0) {leftdistance = 200;} fijar newping biblioteca equivocado / / comparar distancia derecha y la izquierda / / / si (leftdistance > rightdistance & & leftdistance > dangerdistance) //if queda menos dangerdistance obstruido y super que {//Stop Motor1, Motor2 motorStop(motor1); motorStop(motor2); time=millis(); panMotor.write (175); //turn sonar a izquierda delay(400) "Prepárate comprobar el lado izquierdo del robot cuando vuelva a" hacer / o (apagar el robot a la izquierda mientras que el sensor de dar menos de 30cm y no más tiempo luego 800 ms) {int nos = sonar.ping(); distancia = uS / US_ ROUNDTRIP_CM; Compruebe los sensores delay(100); esperar a que los sensores estabilizar //Turn hacia motor1: parada Motor1, lento motorDrive Motor2 (motor1, turnCCW, 180); motorDrive (motor2, turnCW, 180); timeplus=Millis(); } y (distancia > dangerdistance & timeplus-tiempo < 800);
motorBrake(motor1); motorBrake(motor2); panMotor.write(90); / gire sonar a delay(2000) de la posición del origen;
} else if (rightdistance > leftdistance & & rightdistance > dangerdistance) //if derecho es menos dangerdistance obstruido y super que {//Stop Motor1, Motor2 motorStop(motor1); motorStop(motor2); panMotor.write(5); delay(400); time=millis(); hacer //(do turn robot to right while sensor give less than 30cm and time not more then 800 ms) {int nos = sonar.ping(); distancia = uS / US_ROUNDTRIP_CM; / / comprobar los sensores delay(100); / / esperar a que los sensores se estabilicen motorDrive (motor2 turnCCW, 180); motorDrive (motor1, turnCW, 180); timeplus=Millis(); } y (distancia > dangerdistance & timeplus-tiempo < 800); motorBrake(motor1); motorBrake(motor2);
panMotor.write(90); Delay(2000); } else / / si no hay solución {/ / stop todo motorBrake(motor1); motorBrake(motor2); motorsStandby(); panMotor.write(90); delay(9000);}} }
///////////////////////////// FUNCTIONS /////////////////////////////////////////
anular motorDrive (booleano motorNumber, boolean motorDirection, motorSpeed int) {/ * esto conduce un motor especificado en una dirección específica, a una velocidad especificada:-motorNumber: motor1 o motor2---> Motor 1 y Motor 2 - motorDirection:---> turnCW o turnCCW en sentido horario o antihorario - motorSpeed: 0 a 255---> 0 = parada / 255 = rápido * /
pinIn1 Boolean; Se refiere a AIN1 o BIN1 (dependiendo del número de motor especificado)
Especificar la dirección para girar el motor/en sentido horario: AIN1/BIN1 = alta y AIN2/BIN2 = baja / / Counter-Clockwise: AIN1/BIN1 = baja y AIN2/BIN2 = alto si (motorDirection == turnCW) pinIn1 = alto; Else pinIn1 = bajo;
Seleccione el motor para girar y fijar la dirección y la velocidad if(motorNumber == motor1) {digitalWrite (pinAIN1, pinIn1); digitalWrite (pinAIN2,! pinIn1); //This es lo contrario de la AIN1 analogWrite (pinPWMA, motorSpeed);} else {digitalWrite (pinBIN1, pinIn1); digitalWrite (pinBIN2,! pinIn1); //This es lo contrario de la BIN1 analogWrite (pinPWMB, motorSpeed);}
Por último, asegúrese de que está desactivada STBY - tire alta digitalWrite (pinSTBY, HIGH);
}
vacío introduce (boolean motorNumber) {/ * este "Freno corto" s el motor especificado, mediante el establecimiento de velocidad a cero * /
Si (motorNumber == motor1) analogWrite (pinPWMA, 0); otro analogWrite (pinPWMB, 0); }
anular motorStop (boolean motorNumber) {/ * Esto detiene el motor especificado estableciendo ambos pernos de la IN en bajo * / si (motorNumber == motor1) {digitalWrite (pinAIN1, LOW); digitalWrite (pinAIN2, LOW);} else {digitalWrite (pinBIN1, bajo); digitalWrite (pinBIN2, LOW);}}
void motorsStandby() {/ * esto pone los motores en modo de espera * / digitalWrite (pinSTBY, LOW);}
