   pinMode(PILKAS, OUTPUT);
   pinMode(GELTONAS, OUTPUT);
 
+  pinMode(THROTTLE_PIN1, OUTPUT);
+  pinMode(THROTTLE_PIN2, OUTPUT);
+
+  pinMode(STEERING_ENABLE, OUTPUT);
+
+  pinMode(SPI_LATCH, OUTPUT);
+  pinMode(SPI_DATA, OUTPUT);
+Serial.begin(9600);
+Serial.println("Start servo debug");
+
+
+sendControl();
+  
   analogWrite(PILKAS, center[0]);
   analogWrite(GELTONAS, center[1]);
 
+myservo.attach(9);  // attaches the servo on pin 9 to the servo object
+
+ //center
+ 
+myservo.write(110); 
   delay(1000);
 
   digitalWrite(TAKEOVER, HIGH);

#define PILKAS 6
#define GELTONAS 5


//pilkas geltonas
uint8_t center[] = {113, 138};


//  delay(500);

 


 
}





void loop() {
//  center[0]++;
//  center[1]++;
  

//  analogWrite(PILKAS, center[0]);
//  analogWrite(GELTONAS, center[1]);

//  delay(10);

//myservo.write(pos);              // tell servo to go to position in variable 'pos'
//Serial.print("Servo at ");
//Serial.println(pos);

//for (short i=30;i<110;i++) {
//myservo.write(i);              // tell servo to go to position in variable 'pos'
//Serial.print("Servo at ");
//Serial.println(i);
// delay(1500);

//myservo.write(129);


delay(1000);
 myservo.write(85);
 delay(100);
 Aux(1);
 delay(100);
 myservo.write(40);
 delay(1000);
 myservo.write(30);
 delay(1000);
 myservo.write(40);
 delay(1000);
 myservo.write(85);
 

//delay(2000);
//Aux(1);
//delay(200);

//digitalWrite(THROTTLE_PIN2, LOW);
//digitalWrite(THROTTLE_PIN1, HIGH); // forward

//delay(100);
//digitalWrite(STEERING_ENABLE, HIGH);

//delay(5000);

//digitalWrite(STEERING_ENABLE, LOW);
//delay(100);
//digitalWrite(STEERING_ENABLE, HIGH);
//delay(10);
//digitalWrite(THROTTLE_PIN1, LOW);
//digitalWrite(THROTTLE_PIN2, HIGH); // back
//delay(500);
//
//digitalWrite(STEERING_ENABLE, LOW);
//delay(100);
//digitalWrite(THROTTLE_PIN1, LOW);
//digitalWrite(THROTTLE_PIN2, LOW); // back

}

void Aux(bool state) {
  if (state) {
    control = control | CTRL_AUX;
  } else {
    control = control & ~CTRL_AUX;
  }
 sendControl();
}

void sendControl(void) {
   digitalWrite(SPI_LATCH, LOW);
   shiftOut(SPI_DATA, SPI_CLK, MSBFIRST, control);  
   digitalWrite(SPI_LATCH, HIGH);
}
