/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>

Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;
Servo updown;

void setup(){
  servo_1.attach(9); 
  servo_2.attach(10); 
  servo_3.attach(11); 
  servo_4.attach(6); 
  updown.attach(5);
  servo_1.write(90);      
  servo_2.write(90);
  servo_3.write(90);
  servo_4.write(90);
  updown.write(0);
  delay(100);
}

void _init(){
  servo_1.write(90);      
  servo_2.write(90);
  servo_3.write(90);
  servo_4.write(90);
  delay(1000);
}

void forward(){
  float time = -0.5;
  int steps = 0;
  while(true){
    steps++;
    time += 0.5;
    if(time == 1.0){
      servo_1.write(45);
      servo_3.write(45);
      servo_2.write(90);
      servo_4.write(90);
      delay(250);
    }else{
      servo_1.write(90);
      servo_3.write(90);
      servo_2.write(45);
      servo_4.write(45);
      delay(250);
    }
    if(time == 1.0) time = -0.5;
    if(steps == 20) break;
  }
}

void backward(){
  float time = -0.5;
  int steps = 0;
  while(true){
    steps++;
    time += 0.5;
    if(time == 1.0){
      servo_1.write(135);
      servo_3.write(135);
      servo_2.write(90);
      servo_4.write(90);
      delay(250);
    }else{
      servo_1.write(90);
      servo_3.write(90);
      servo_2.write(135);
      servo_4.write(135);
      delay(250);
    }
    if(time == 1.0) time = -0.5;
    if(steps == 20) break;
  }
}

void upAndDown(){
  for(int i=0;i<181;i++){
    updown.write(i);
    delay(10);
  }
  for(int i=180;i>=00;i--){
    updown.write(i);
    delay(10);
  }
}

void turn(){

}


void loop(){
  forward();  
  _init();
  backward();
  _init();
  upAndDown();
  _init();
  turn();
  _init();

}