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

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


//Servo installed
#include <Servo.h>

Servo myservo;  // create servo object to control a servo
Servo myservo1; 
Servo myservo2; 
//Servo myservo1;
// twelve servo objects can be created on most boards 

int pos = 90;    // variable to store the servo position

//Sonar installed

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 = 10;
long previoustime = 0;

void setup() 
//Servo setup
{
    strip.begin();
  strip.show();
  myservo.attach(10);
  myservo1.attach(9);
myservo2.attach(5);
 // myservo1.attach(10);// attaches the servo on pin 9 to the servo objec

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

    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);
    
//    for( int i = 0; i<NUM_LIGHTS; i++){
//      strip.setPixelColor(i, low);
 //       strip.show();
}

void loop() {
for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos += 1);              // tell servo to go to position in variable 'pos'
    delay(45);                       // waits 15ms for the servo to reach the position
  }
 for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
delay(30);  
 }
 for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
delay(30);  
 // for (pos = 20; pos <= 160; pos += 1) { // goes from 180 degrees to 0 degrees
 //   myservo2.write(pos);              // tell servo to go to position in variable 'pos'
//delay(30);                       // waits 15ms for the servo to reach the position
 // }

 } 
  for (pos = 160; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
   delay(45);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 160; pos >= 20; pos -= 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
delay(45);                       // waits 15ms for the servo to reach the position
  }
    for (pos = 160; pos >= 20; pos -= 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
delay(45);                       // waits 15ms for the servo to reach the position
  }
 // {
 // for (pos = 160; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
//    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
//delay(45);                       // waits 15ms for the servo to reach the position
 // }




//sonar loop

if( millis()- previoustime > 1000){
previoustime = millis();
 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); 
 sonarafstand(cm);
delay (20);

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 (500);
 
//  delay(100); 
}

}

void sonarafstand(int afstand){
 if(cm > 10 && cm < 50 ){
 //servo loop   

  for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(45);                       // waits 15ms for the servo to reach the position
  }
 for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
delay(45);  
 }
  for (pos = 20; pos <= 160; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
delay(45);  
 // for (pos = 20; pos <= 160; pos += 1) { // goes from 180 degrees to 0 degrees
 //   myservo2.write(pos);              // tell servo to go to position in variable 'pos'
//delay(30);                       // waits 15ms for the servo to reach the position
 // }

 } 
  for (pos = 160; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
   delay(45);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 160; pos >= 20; pos -= 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo to go to position in variable 'pos'
delay(45);                       // waits 15ms for the servo to reach the position
  }
   for (pos = 160; pos >= 20; pos -= 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
delay(45);                       // waits 15ms for the servo to reach the position
  }
 // {
 // for (pos = 160; pos >= 20; pos -= 1) { // goes from 180 degrees to 0 degrees
//    myservo2.write(pos);              // tell servo to go to position in variable 'pos'
//delay(45);                       // waits 15ms for the servo to reach the position
 // }
 
 }else {
 
 
 }
}