Paso 7: El código: - para el SENSOR PING
<p>const int serialPeriod = 250; // a period of 250ms = a frequency of 4 Hz <br>unsigned long timeSerialDelay = 0;</p><p>const int UltraloopPeriod = 20; // a period of 20ms = a frequency of 50 Hz unsigned UltraLoopDelay = 0;</p><p> const int sensor_1 = 10; // input/output from the sensor_1 int motorL1 = 6; // output for motor driver pin 2 int motorL2 = 7; // output for motor driver pin 7 int motorR1 = 8; // output for motor driver pin 15 int motorR2 = 9; // output for motor driver pin 10 int d0 = 2; // input from DTMF pin D0 int d1 = 3; // input from DTMF pin D1 int d2 = 4; // input from DTMF pin D2 int d3 = 5; // input from DTMF pin D3 int ultrasonicTime; // variable to store time int ultrasonicDistance; // variable to store distance calculated void setup() { Serial.begin(9600); // setting serial communication speed pinMode(motorL1, OUTPUT); pinMode(motorL2, OUTPUT); pinMode(motorR1, OUTPUT); pinMode(motorR2, OUTPUT); pinMode(d0, INPUT); pinMode(d1, INPUT); pinMode(d2, INPUT); pinMode(d3, INPUT); }</p><p>void loop() { int z = digitalRead(d0); int y = digitalRead(d1); int x = digitalRead(d2); int w = digitalRead(d3); /*----------------------------------------- M A N U A L M O D E ---------------------------------------*/ if(w == LOW) { if((w == LOW)&&(x == LOW)&&(y == HIGH)&&(z == LOW)) { digitalWrite(motorL1, HIGH); digitalWrite(motorL2, LOW); digitalWrite(motorR1, HIGH); digitalWrite(motorR2, LOW); } if((w == LOW)&&(x == HIGH)&&(y == LOW)&&(z == HIGH)) { digitalWrite(motorL1, LOW); digitalWrite(motorL2, HIGH); digitalWrite(motorR1, LOW); digitalWrite(motorR2, HIGH); } if((w == LOW)&&(x == HIGH)&&(y == LOW)&&(z == LOW)) { digitalWrite(motorL1, LOW); digitalWrite(motorL2, HIGH); digitalWrite(motorR1, HIGH); digitalWrite(motorR2, LOW); } if((w == LOW)&&(x == HIGH)&&(y == HIGH)&&(z == LOW)) { digitalWrite(motorL1, HIGH); digitalWrite(motorL2, LOW); digitalWrite(motorR1, LOW); digitalWrite(motorR2, HIGH); } if((w == LOW)&&(x == HIGH)&&(y == HIGH)&&(z == HIGH)) { digitalWrite(motorL1, LOW); digitalWrite(motorL2, LOW); digitalWrite(motorR1, LOW); digitalWrite(motorR2, LOW); } } /*----------------------------------- A U T O N O M O U S M O D E -----------------------------------*/ if(w == HIGH) { viewDistance(); if((millis() - UltraLoopDelay) >= UltraloopPeriod) { readUltrasonicsensor_1(); motorStart(); UltraLoopDelay = millis(); } } } void readUltrasonicsensor_1() // fuction to take sensor_1 data and find distance { pinMode(sensor_1, OUTPUT); digitalWrite(sensor_1, LOW); delay(2); digitalWrite(sensor_1, HIGH); delay(10); digitalWrite(sensor_1, LOW); pinMode(sensor_1, INPUT); ultrasonicTime = pulseIn(sensor_1, HIGH); ultrasonicDistance = (ultrasonicTime/2)/29; //calculation to measure the distance of obstacle from ultrasoni // c sensor }</p><p>void motorStart() //function to drive the motor according to sensed distance { if(ultrasonicDistance > 10) { digitalWrite(motorL1, HIGH); digitalWrite(motorL2, LOW); digitalWrite(motorR1, HIGH); digitalWrite(motorR2, LOW); } if((irsL == HIGH)&&(irsR == LOW)) { digitalWrite(motorL1, HIGH); digitalWrite(motorL2, LOW); digitalWrite(motorR1, LOW); digitalWrite(motorR2, HIGH); } if((irsL == LOW)&&(irsR == HIGH)) { digitalWrite(motorL1, LOW); digitalWrite(motorL2, HIGH); digitalWrite(motorR1, HIGH); digitalWrite(motorR2, LOW); } if((irsL == HIGH)&&(irsR == HIGH)) { digitalWrite(motorL1, LOW); digitalWrite(motorL2, HIGH); digitalWrite(motorR1, LOW); digitalWrite(motorR2, HIGH); } } if(ultrasonicDistance < 10) { digitalWrite(motorL1, LOW); digitalWrite(motorL2, HIGH); digitalWrite(motorR1, HIGH); digitalWrite(motorR2, LOW); } } }</p><p>/*---------------------------CHECKING THE ULTRSONIC SENSOR-------------------------*/</p><p>void viewDistance() //function to view distance on serial monitor { //to check if the ultra sonic sensor code working properly if((millis() - timeSerialDelay) >= serialPeriod) { Serial.print("Distance"); Serial.print(ultrasonicDistance); Serial.print("cm"); Serial.println(); timeSerialDelay = millis(); } }</p>
