#include <Servo.h> // this program uses the Servo library

Servo LeftServo;   // create servo object to control both servos
Servo RightServo;  // a maximum of eight servos can be created                                                                      
                                             
#define FAST  50   // try to change these values during the test
#define SLOW   5
#define LED_Yellow 13 
#define SENSOR_1  2
#define SENSOR_2  3

int kolesoL = 0;
int kolesoP = 0;
int oddelovac = 0;

long ciara = 0;           // variable to store the value coming from the sensor
long svetlo = 0;           // variable to store the value coming from the sensor

const int pingPin = 11;


void setup() 
{ 
 Serial.begin(115200);
  pinMode(LED_Yellow, OUTPUT );
  
   LeftServo.attach(9);    // attaches the servo on pin 9 to the servo object 
   RightServo.attach(10);  // attaches the servo on pin 10 to the servo object 
  
} 
 
void loop() { 
  
    svetlo = analogRead(SENSOR_1);     // read the value from the sensor
    ciara = analogRead(SENSOR_2); 
    
    
    
    long duration, cm;
    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);
    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH);
    cm = microsecondsToCentimeters(duration);
    delay(300);
  
  
  
//    Serial.print("Sensor = ");  
//    Serial.println(ciara, DEC);  
//    Serial.print(", ");  
//    Serial.println(svetlo, DEC);  
//    Serial.print(", ");  
//    Serial.println(cm, DEC);  
//  
  while (Serial.available() > 2) {
      oddelovac = Serial.read();
      if(oddelovac ==1){
          kolesoL = Serial.read()-128;
          kolesoP = Serial.read()-128;
  
        Serial.write(1);   
        Serial.write(cm);
        Serial.write(svetlo/10);
        Serial.write(ciara/10);
        LeftServo.write(kolesoL);    // test varying speed of movement
        RightServo.write(kolesoP);
   }  
}
      
      
 

}
  
  long microsecondsToCentimeters(long microseconds)
  {
    return microseconds / 29 / 2;
  }