//Sonar
int rxPin = 0; // this is simular to the int = setting, however this is linked to the library
int txPin = 1;
int trig = 16; 
int echo = 17; 
int trig1 = 18; 
int echo1 = 19; 
long lecture_echo; 
long cm;
int timeout = 1000;

#include <Adafruit_NeoPixel.h>
#define PIN 3
#define NUM_LIGHTS  20

Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LIGHTS, PIN, NEO_GRB + NEO_KHZ800);


//Servo
#include <Servo.h>

Servo myservo;  // create servo object to control a servo
Servo myservo1; 
Servo myservo2; 



void setup() 
{
  
  pinMode(trig, OUTPUT); 
  digitalWrite(trig, LOW); 
  pinMode(echo, INPUT);  
  
  pinMode(trig1, OUTPUT); 
  digitalWrite(trig1, LOW); 
  pinMode(echo1, INPUT); 
  
  Serial.begin(9600); 

//servo pins    
  myservo.attach(10);
  myservo1.attach(9);
myservo2.attach(5);



  
}
void loop() 
{ 

      
 digitalWrite(trig, HIGH); 
 delayMicroseconds(10); 
 digitalWrite(trig, LOW); 
 lecture_echo = pulseIn(echo, HIGH,  timeout*1000); 
 cm = lecture_echo / 58; 
  
 Serial.print ("sonar 1 in cm: "); 
 Serial.println(cm); 


digitalWrite(trig1, HIGH); 
delayMicroseconds(10); 
digitalWrite(trig1, LOW); 
lecture_echo = pulseIn(echo1, HIGH, timeout*1000); //timeout in micro seconden
cm = lecture_echo / 58; 

Serial.print ("sonar 2 in cm: "); 
Serial.println(cm); 
  
  delay(timeout); 

 if(cm > 0 && cm < 50 ){

  myservo.write(45);              // tell servo to go to position in variable 'pos'
    delay(500);
    myservo1.write(0);              // tell servo to go to position in variable 'pos'
    delay(500);
      myservo2.write(180);              // tell servo to go to position in variable 'pos'
    delay(500);
 myservo2.write(90);              // tell servo to go to position in variable 'pos'
    delay(500); 
    myservo2.write(180);              // tell servo to go to position in variable 'pos'
    delay(500);
     myservo2.write(90);              // tell servo to go to position in variable 'pos'
delay(500);

      strip.begin();
  strip.show(); 
       
       strip.setPixelColor(0, 229, 142, 13); 
    strip.setPixelColor(1, 0, 0, 255); 
    strip.setPixelColor(2, 0, 0, 255); 
    strip.setPixelColor(3, 229, 142, 13); 
       strip.setPixelColor(4, 229, 142, 13); 
    strip.setPixelColor(5, 0, 0, 255); 
    strip.setPixelColor(6, 0, 0, 255); 
    strip.setPixelColor(7, 0, 0, 255); 
  strip.setPixelColor(8, 0, 0, 0);
 strip.setPixelColor(9, 0, 0, 255); 
    strip.setPixelColor(10, 229, 142, 13); 
    strip.setPixelColor(11, 0, 0, 255); 
    strip.setPixelColor(12, 0, 0, 0); 
   strip.setPixelColor(13, 0, 0, 255); 
      strip.setPixelColor(14, 0, 0, 255); 
 strip.setPixelColor(15, 0, 0, 255); 
    strip.setPixelColor(16, 229, 142, 13); 
    strip.setPixelColor(17, 229, 142, 13); 
   strip.setPixelColor(18, 0, 0, 255);   
      strip.setPixelColor(19, 0, 0, 255);

  for( int i = 0; i<NUM_LIGHTS; i++){
  //strip.setPixelColor(i, high);
        strip.show();
    }   
    delay(100); 

}

else
{  myservo.write(90);              // tell servo to go to position in variable 'pos'
    delay(500);
    myservo1.write(90);              // tell servo to go to position in variable 'pos'
    delay(500);
      myservo2.write(90);              // tell servo to go to position in variable 'pos'
    delay(500);

    
   strip.begin();
  //strip.show();

   strip.setPixelColor(0, 229, 142, 13); 
    strip.setPixelColor(1, 229, 142, 13); 
    strip.setPixelColor(2, 229, 142, 13); 
    strip.setPixelColor(3, 229, 142, 13); 
       strip.setPixelColor(4, 229, 142, 13); 
    strip.setPixelColor(5, 229, 142, 13); 
    strip.setPixelColor(6, 229, 142, 13); 
    strip.setPixelColor(7, 229, 142, 13); 
  strip.setPixelColor(8, 0, 0, 255);
 strip.setPixelColor(9, 229, 142, 13); 
    strip.setPixelColor(10, 229, 142, 13); 
    strip.setPixelColor(11, 229, 142, 13); 
    strip.setPixelColor(12, 0, 0, 255); 
   strip.setPixelColor(13, 229, 142, 13); 
      strip.setPixelColor(14, 229, 142, 13); 
 strip.setPixelColor(15, 229, 142, 13); 
    strip.setPixelColor(16, 229, 142, 13); 
    strip.setPixelColor(17, 229, 142, 13); 
   strip.setPixelColor(18, 229, 142, 13); 
    strip.setPixelColor(19, 229, 142, 13);

   for( int i = 0; i<NUM_LIGHTS; i++){
    //  strip.setPixelColor(i, high);
        strip.show();
    }   
    delay(100); 
}
}