#include <Servo.h> 

Servo waist,front,back;

#define wCenter 80
#define fCenter 70
#define bCenter 70
#define wSwing 40
#define fSwing 40
#define bSwing 30
#define FORWARD 0
#define BACKWARD 1
#define LEFT 2
#define RIGHT 3
#define STOP 4
int wPos,fPos,bPos,cycle,dir;
long start;

void setup() 
{ 
  Serial.begin(9600);
  waist.attach(13);
  front.attach(12);
  back.attach(11);
  wPos=wCenter;
  fPos=fCenter;
  bPos=bCenter;
  dir=FORWARD;
  cycle=0;
  start=millis();
  Serial.println("iWaist,iFront,iBack");
} 

void doBackward(int cycle) {
  if (cycle<12) wPos=wCenter+wSwing;
  else if (cycle<37) wPos=wCenter-wSwing;
  else if (cycle<62) wPos=wCenter+wSwing;
  else if (cycle<87) wPos=wCenter-wSwing;
  else          wPos=wCenter+wSwing;
  if (cycle<25) fPos=fCenter+fSwing;
  else if (cycle<50) fPos=fCenter-fSwing; 
  else if (cycle<75) fPos=fCenter+fSwing;
  else fPos=fCenter-fSwing; 
  if (cycle<25) bPos=bCenter-bSwing;
  else if (cycle<50) bPos=bCenter+bSwing; 
  else if (cycle<75) bPos=bCenter-bSwing;
  else bPos=bCenter+bSwing; 
}

void doForward(int cycle) {
  doBackward(100-cycle);
}

void doLeft(int cycle) {
  doBackward(100-cycle);
  if (wPos>wCenter) wPos=wCenter;
}

void doRight(int cycle) {
  doBackward(100-cycle);
  if (wPos<wCenter) wPos=wCenter;
}

void doStop(){
  wPos=wCenter;
  fPos=fCenter;
  bPos=bCenter;
}

void posUpdate() {
  cycle=(cycle+1)%100;  // 100 step cycle
  switch(dir) {
    case FORWARD: doForward(cycle);  break;
    case BACKWARD: doBackward(cycle); break;
    case LEFT: doLeft(cycle); break;
    case RIGHT: doRight(cycle); break;
    case STOP: doStop(); break;
  }
}

void loop() 
{ 
  posUpdate();
  waist.write(wPos);
  front.write(fPos);
  back.write(bPos);
  long iWaist=analogRead(0);
  long iFront=analogRead(1);
  long iBack=analogRead(2);
  Serial.print(iWaist,10);
  Serial.print(",");
  Serial.print(iFront,10);
  Serial.print(",");
  Serial.print(iBack,10);
  Serial.println();
  delay(20);
  long t=millis()-start;
  if (t <10000) dir=FORWARD;
  else if (t < 20000) dir=BACKWARD;
  else if (t < 30000) dir=LEFT;
  else if (t < 40000) dir=RIGHT;
  else dir=STOP;
}