Finalnocomments_edit.ino

//Start :
//define the pins that we will use for the first ultrasonic sensor
//----------------------------------------------------------------------------------------------------------------------
#define trigPin1 9                                   //pin number 9 in arduino MEGA2560
#define echoPin1 8                                   // we'll use this pin to read the signal from the first sensor
#define LED_first_ping 22                            // I/O digital or analogique ( we will use pin 22 to command an LED (on/off))
//----------------------------------------------------------------------------------------------------------------------

//define the pins that we will use for the second ultrasonic sensor
//----------------------------------------------------------------------------------------------------------------------
#define trigPin2 11
#define echoPin2 10
#define LED_second_ping 24
//----------------------------------------------------------------------------------------------------------------------

//define the pins that we will use for the third ultrasonic sensor
//----------------------------------------------------------------------------------------------------------------------
#define trigPin3 13
#define echoPin3 12
#define LED_third_ping 26
//----------------------------------------------------------------------------------------------------------------------

//used variables
//----------------------------------------------------------------------------------------------------------------------
double duration, distance, UltraSensor1, UltraSensor2, UltraSensor3; //we'll use these variable to store and generate data
float x0, y0, z0, x1, y1, z1, x2, y2, z2;
char data;
String SerialData = "";
double CentreX, CentreY, CentreZ, Px, Py, Pz, Qx, Qy, Qz, Rx, Ry, Rz, PQx, PQy, PQz, PRx, PRy, PRz;
float UltraSensor1S, UltraSensor1T, UltraSensor2S,UltraSensor2T, UltraSensor3S, UltraSensor3T;
float average1, average2, average3;
float C0, C1, C2, Plane, Final;
//----------------------------------------------------------------------------------------------------------------------

//Make the setup of your pins
//----------------------------------------------------------------------------------------------------------------------
void setup()
{ // START SETUP FUNCTION
  Serial.begin (9600);                              // we will use the serial data transmission to display the distance value on the serial monitor
  
  // setup pins first sensor
  pinMode(trigPin1, OUTPUT);                        // from where we will transmit the ultrasonic wave
  pinMode(echoPin1, INPUT);                         //from where we will read the reflected wave
  
  //setup pins second sensor
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  //setup pins third sensor
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);

  x0 = 0;
  y0 = 0;
  z0 = 0;
  x1 = 0;
  y1 = 3;
  z1 =5;
  x2 = 0;
  y2 = -3;
  z2 = 5;




}// END SETUP FUNCTION

//write the code in the loop function
void loop()
{
  UltraSensor1=0;
  UltraSensor2=0;
  UltraSensor3=0;
  // START THE LOOP FUNCTION
 for (int i = 0; i<10; i++) {  
    SonarSensor(trigPin1, echoPin1);              // look bellow to find the difinition of the SonarSensor function

   if(distance>=UltraSensor1+0.5)
   {
    UltraSensor1T = UltraSensor1S;
    UltraSensor1S = UltraSensor1;
  UltraSensor1 = distance;                       // store the distance in the first variable
   }
   else if(distance>=UltraSensor1S)
   {
    UltraSensor1T = UltraSensor1S;
    UltraSensor1S=distance;
   }
   else if(distance>=UltraSensor1T)
   {
        UltraSensor1T=distance;
   }
   average1=(UltraSensor1+UltraSensor1S+UltraSensor1T)/3; 
   x0 = average1;
   delay(70);
  
  }
   
  for (int i = 0;i<10; i++) {
    SonarSensor(trigPin2, echoPin2);              // call the SonarSensor function again with the second sensor pins

  if(distance>=UltraSensor2+0.5)
   {
    UltraSensor2T = UltraSensor2S;
    UltraSensor2S = UltraSensor2;
  UltraSensor2 = distance;                       // store the distance in the second variable
   }
   else if(distance>=UltraSensor2S)
   {
    UltraSensor2T = UltraSensor2S;
    UltraSensor2S=distance;
   }
   else if(distance>=UltraSensor2T)
   {
    UltraSensor2T=distance;
   }
   average2=(UltraSensor2+UltraSensor2S+UltraSensor2T)/3;
   x1 = average2;
   
   delay(70);
  }
  
for (int i = 0;i<10; i++) {
    SonarSensor(trigPin3, echoPin3);              // call the SonarSensor function again with the second sensor pins

  if(distance>=UltraSensor3+0.5)
   {
    UltraSensor3T = UltraSensor3S;
    UltraSensor3S = UltraSensor3;
  UltraSensor3 = distance;                       // store the distance in the second variable
   }
   else if(distance>=UltraSensor3S)
   {
    UltraSensor3T = UltraSensor3S;
    UltraSensor3S=distance;
   }
   else if(distance>=UltraSensor3T)
   {
    UltraSensor3T=distance;
   }
   average3=(UltraSensor3+UltraSensor3S+UltraSensor3T)/3;
   x2 = average3;
   delay(70);
  }
   
  while (Serial.available())
  {
    delay(10);
    data = Serial.read();
    SerialData += data;
  }


  // display the distances on the serial monitor for the first sensor
  //----------------------------------------------------------------------------------------------------------------------
  Serial.println();
  Serial.println();
  Serial.print("distance measured by the first sensor: ");
  Serial.print(x0);
  Serial.println(" cm");
  Serial.print(x0);
  Serial.print(",0,0");
  Serial.println();
  //----------------------------------------------------------------------------------------------------------------------

  //display the distance on the serial monitor for the second sensor
  //----------------------------------------------------------------------------------------------------------------------
  Serial.print("distance measured by the second sensor: ");
  Serial.print(x1);
  Serial.println(" cm");
  Serial.print(x1);
  Serial.print(",3,5");
  Serial.println();
  //---------------------------------------------------------------------------------------------------------------------

  //display the distance on the serial monitor for the third sensor
  //----------------------------------------------------------------------------------------------------------------------
  Serial.print("distance measured by the third sensor: ");
  Serial.print(x2);
  Serial.println(" cm");
  Serial.print(x2);
  Serial.print(",-3,5");
  Serial.println();
  //----------------------------------------------------------------------------------------------------------------------


  CentreX = 0;
  CentreY = 0;
  CentreZ = 3.333;   // Center of the three sensors, will be used in the final equation
  
  
  C0 = 30;
  C1 = -(5*x1) + (5*x2);
  C2 = (6*x0) - (3*x1) - (3*x2); //Plane = C0(x-X0) + C1(y-YO) + C2(z-Z0) note (X0,Y0,Z0) = (x0,0,0)  //Cross product of PQ and PR
  
  Plane = abs(C0*(x0));//abs(C0*(CentreX-x0)+C1*(CentreY-y0)+C2*(CentreZ-z0));
  Serial.print("C0 is:");
  Serial.println(C0);
  Serial.print("C1 is:");
  Serial.println(C1);
  Serial.print("C2 is:");
  Serial.println(C2);
  Serial.print("The value of the the plane is:");
  Serial.println(Plane);
  
  Final  = (Plane-C2*CentreZ)/(sqrt(pow(C0,2)+pow(C1,2)+pow(C2,2)));
  Serial.print("The average distance of the sensors to the plane is:");
  Serial.print(Final);
  Serial.println("cm");
  if (Final<3) {
    Serial.print("The sensors are too close to the surface, move back!"); //Note, for this case the delay has made to be 50ms
  }
  delay(2000);
}//END LOOP FUNTION

// SonarSensor function used to generate and read the ultrasonic wave
void SonarSensor(int trigPinSensor, int echoPinSensor) //it takes the trigPIN and the echoPIN
{
  //START SonarSensor FUNCTION
  //generate the ultrasonic wave
  //----------------------------------------------------------------------------------------------------------------------
  digitalWrite(trigPinSensor, LOW);// put trigpin LOW
  delayMicroseconds(2);// wait 2 microseconds
  digitalWrite(trigPinSensor, HIGH);// switch trigpin HIGH
  delayMicroseconds(10); // wait 10 microseconds
  digitalWrite(trigPinSensor, LOW);// turn it LOW again
  delayMicroseconds(50);
  //----------------------------------------------------------------------------------------------------------------------

  //read the distance
  //----------------------------------------------------------------------------------------------------------------------
 
  duration = pulseIn(echoPinSensor, HIGH);//pulseIn funtion will return the time on how much the configured pin remain the level HIGH or LOW; in this case it will return how much time echoPinSensor stay HIGH
  distance = (duration / 2) / 29.1; // first we have to divide the duration by two
  
}// END SonarSensor FUNCTION

/****************************----------------------- END PROGRAM -----------------------****************************/