El Robot estadounidense Lazy (4 / 4 paso)

Paso 4: codificación

#include "Servo.h"
#include < SoftwareSerial.h >

const int Trig_pinRight = 7;   Pulso de disparadores para Sensor ultrasónico derecha
const int Echo_pinRight = 13;     Recibe eco sensor ultrasónico derecha
const int Trig_pinLeft = 12;   Dispara pulsos de izquierda Sensor de ultrasonidos
const int Echo_pinLeft = 8;     Recibe el eco dejado ultrasónico Sensor
largo durationRight;            Tiempo que tarda el pulso echar atrás a la derecha el Sensor ultrasónico
largo durationLeft;             Tiempo de pulso rebotar a la izquierda el Sensor ultrasónico

Servo motorLeft;               IZQUIERDA VEX Motor
Servo motorRight;              VEX derecho Motor

void setup() {}

Serial.Begin(9600);           Biblioteca Serial a 9600 bps

Serial.println("Initializing...");  Imprimir inicializando... para confirmar código está trabajando con la serie de la biblioteca

motorLeft.attach(11);         El Motor de izquierda es en el pin 11
motorRight.attach(10);        DERECHO de Motor es en pin 10


Serial.println ("desde");
Inicializa el pin de pulso como salida:
pinMode (Trig_pinRight, salida);
Inicializa el pin de pulso como salida:
pinMode (Trig_pinLeft, salida);
inicializar el echo_pin pin como una entrada:
pinMode (Echo_pinRight, entrada);
inicializar el echo_pin pin como una entrada:
pinMode (Echo_pinLeft, entrada);

}

void loop()
{

Pulso y Eco para el Sensor ultrasónico derecha; Proceso para determinar e imprimir las duraciones para Sensor ultrasónico derecho

digitalWrite (Trig_pinRight, bajo);
delayMicroseconds(2);
digitalWrite (Trig_pinRight, alto);
delayMicroseconds(5);
digitalWrite (Trig_pinRight, bajo);
durationRight = pulseIn(Echo_pinRight,HIGH);
Serial.println ("durationRight:");
Serial.println (durationRight, DEC);

Pulso y Eco para la izquierda ultrasónicos Sensor; Proceso para determinar e imprimir las duraciones para Sensor ultrasónico izquierda

digitalWrite (Trig_pinLeft, bajo);
delayMicroseconds(2);
digitalWrite (Trig_pinLeft, alto);
delayMicroseconds(5);
digitalWrite (Trig_pinLeft, bajo);
durationLeft = pulseIn(Echo_pinLeft,HIGH);
Serial.println ("durationLeft:");
Serial.println (durationLeft, DEC);

Si (durationRight > 0 & & durationRight < 4000 & & durationRight > 0 & & durationLeft < 4000) / / STOP cuando el ser humano es muy cerca de ambos sensores
{
motorLeft.write(90);
motorRight.write(90);
}
Si (durationRight > 4000 & & durationRight < 10000 & & durationLeft > 4000 & & durationLeft < 10000) / / mover hacia adelante cuando el humano es equadistant a ambos sensores
{
motorLeft.write(113);
motorRight.write(68);
}
Si (durationLeft - 1000 > durationRight & & durationRight > 4000) / / girar a la derecha cuando humanos cerca sensor derecho
{
motorLeft.write(120);
motorRight.write(100);
Delay(500);

motorLeft.write(120);
motorRight.write(75);
Delay(2000);
}
Si (durationRight - 1000 > durationLeft & & durationLeft > 4000) / / girar a la izquierda cuando humanos está más cerca del sensor izquierdo
{
motorLeft.write(100);
motorRight.write(75);
Delay(500);

motorLeft.write(120);
motorRight.write(75);
Delay(5000);
}
Si (durationRight > 10000 & & durationLeft > 10000) / moverse adelante rápidamente al humano es demasiado lejos pero aún equadistant
{
motorLeft.write(135);
motorRight.write(45);
}
}