Paso 4: Pruebe el sensor con el tanque
Luego combinar el código en la primera etapa con el código en el tercer proyecto y revisar un poco: Si la distancia entre el sensor y el objeto es más de 30cm, él va adelante. De lo contrario gira a la derecha.
#define trigPin 5#define echoPin 6int motorPin = 8; //right side to IB - forwwardint motorPin2 = 9; //left side to IA - forwwardint motorPin3 = 10; //right side to IA - backwardint motorPin4 = 11; //left side to IB - backwardvoid setup() { Serial.begin (9600); pinMode(motorPin, OUTPUT); pinMode(motorPin2, OUTPUT); pinMode(motorPin3, OUTPUT); pinMode(motorPin4, OUTPUT); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);}void forward(){ digitalWrite(motorPin, HIGH); digitalWrite(motorPin2, HIGH); digitalWrite(motorPin3, LOW); digitalWrite(motorPin4, LOW); } void backward() { digitalWrite(motorPin, LOW); digitalWrite(motorPin2, LOW); digitalWrite(motorPin3, HIGH); digitalWrite(motorPin4, HIGH); } void turnLeft() { digitalWrite(motorPin, HIGH); digitalWrite(motorPin2, LOW); digitalWrite(motorPin3, LOW); digitalWrite(motorPin4, HIGH); } void turnRight() { digitalWrite(motorPin, LOW); digitalWrite(motorPin2, HIGH); digitalWrite(motorPin3, HIGH); digitalWrite(motorPin4, LOW); }int CheckDistance(){ long duration, distance; digitalWrite(trigPin, LOW); // Added this line delayMicroseconds(2); // Added this line digitalWrite(trigPin, HIGH);// delayMicroseconds(1000); - Removed this line delayMicroseconds(10); // Added this line digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); distance = (duration/2) / 29.1; return distance;}void loop() { int testDistance = CheckDistance(); /// get object distance using ping if (testDistance >= 30 || testDistance <= 0) /// if object is more than 30 cm away it goes forward { forward(); } else /// object is closer than 30cm, turns right { turnRight(); } delay(500); ///wait half a sec before next ping} El resultado parece ok.
