 
 //  delay(500);
 
+ 
+
+
+ 
 }
 
+
+
+
+
 void loop() {
-  center[0]++;
-  center[1]++;
+//  center[0]++;
+//  center[1]++;
   
 
-  analogWrite(PILKAS, center[0]);
-  analogWrite(GELTONAS, center[1]);
+//  analogWrite(PILKAS, center[0]);
+//  analogWrite(GELTONAS, center[1]);
 
-  delay(10);
+//  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);
+}

#define PILKAS 6
#define GELTONAS 5


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

  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);
