#define encoderr 2
#define encoderl 3

#define echo1 64
#define echo2 66
#define echo3 68
#define trig1 65
#define trig2 67
#define trig3 69
#define echo4 41
#define echo5 31
#define trig4 43
#define trig5 33

#define motorr_p 7     // motor - and + conectors
#define motorr_m 5
#define motorl_p 8
#define motorl_m 4

 long int dispulser = 0;  // enchoder pulses till now 
 long int dispulsel = 0;


long duration, distance  ;
int rightsensor, midsensor, leftsensor ,right2sensor, mid2sensor, left2sensor;

 void ultra_sonics(int trigPin, int echoPin)
{
  digitalWrite(trigPin, 0);
  delayMicroseconds(2);

  digitalWrite(trigPin, 1);
  delayMicroseconds(10);
  digitalWrite(trigPin, 0);
  duration = pulseIn(echoPin, 1 , 26000);
  distance = distance;
  distance = duration * 0.034/2;
}


void enr()
{
    dispulser ++;
}

void enl()
{
    dispulsel ++ ;
}

void check_ul_sen()
{
  ultra_sonics(trig1, echo1);
  leftsensor = distance;

  ultra_sonics(trig2, echo2);
  rightsensor = distance;
  
  ultra_sonics(trig3, echo3);
  midsensor = distance; 
  
  ultra_sonics(trig4, echo4);
  right2sensor = distance;
  
  ultra_sonics(trig5, echo5);
  left2sensor = distance;
  

  

  
}

void setup() {
  // put your setup code here, to run once:
  pinMode(motorl_p , OUTPUT);
  pinMode(motorl_m , OUTPUT);
  pinMode(motorr_p , OUTPUT);
  pinMode(motorr_m , OUTPUT);

  pinMode(trig1, OUTPUT);
  pinMode(trig2, OUTPUT);
  pinMode(trig3, OUTPUT);
  pinMode(trig4, OUTPUT);
  pinMode(trig5, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(echo2, INPUT);
  pinMode(echo3, INPUT);
  pinMode(echo4, INPUT);
  pinMode(echo5, INPUT);

  Serial.begin(115200);
    
  attachInterrupt(digitalPinToInterrupt(encoderr), enr, RISING); 
  attachInterrupt(digitalPinToInterrupt(encoderl), enl, RISING); 

}

void loop() {
  // put your main code here, to run repeatedly:
  check_ul_sen();
  //analogWrite(motorl_p , 255);
  //analogWrite(motorl_m , 0);

  //analogWrite(motorr_p , 255);
 // analogWrite(motorr_m , 0);

  Serial.print('L');
  Serial.print(leftsensor);
  Serial.print(" R ");
  Serial.print(rightsensor);
  Serial.print(" M ");
  Serial.print(midsensor);
  Serial.print(" L2 ");
  Serial.print(left2sensor);
  Serial.print(" R2 ");
  Serial.println(right2sensor);
  Serial.print(" enl ");
  Serial.print(dispulsel);
  Serial.print(" enr ");
  Serial.println(dispulser);
  
  delay(300);
  
  


}