ROBOT de CONTROL MANUAL / autónoma (mediante la técnica de la fusión de sensores) (7 / 12 paso)

Paso 7: El código: - para el SENSOR PING

 const int serialPeriod = 250; // a period of 250ms = a frequency of 4 Hz unsigned long timeSerialDelay = 0;const int UltraloopPeriod = 20; // a period of 20ms = a frequency of 50 Hz unsigned UltraLoopDelay = 0; const int sensor_1 = 10; // input/output from the sensor_1 const int sensor_2 = 11; const int sensor_3 = 12; 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); }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 ultrasonic sensor }void motorStart() //function to drive the motor according to sensed distance { int irsL = digitalRead(sensor_2); //IR left sensor int irsR = digitalRead(sensor_3); //IR right sensor if(ultrasonicDistance > 10) //this part code works to prevent side collisions even { //when there is no obstacle in front of the robot if((irsL == LOW)&&(irsR== LOW)) { 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) //this part of code works when there is some { //obstacle is infront of the robot and also if((irsL == HIGH)&&(irsR == LOW)) //to prevent side collision { 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((irsL == LOW)&&(irsR == LOW)) { digitalWrite(motorL1, HIGH); digitalWrite(motorL2, LOW); digitalWrite(motorR1, LOW); digitalWrite(motorR2, HIGH); } } }/*---------------------------CHECKING THE ULTRSONIC SENSOR-------------------------*/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(); } }