サーボモーターをループさせて動かしたい

Question

目標座標へ２つのサーボを動かし、元の座標地までもどして、また目標座標へサーボを動かしてを繰り返すプログラムを作りたいのですが目標座標へ動いて止まってしまいます。どのように改善したら良いでしょうか？回答お願いします。

#include<stdio.h>
#include<math.h>
#include<Servo.h>
Servo servo1,servo2;
double a,b,c,x,y,pos1,pos2,pos3,pos4,pos5,A,B,C,D,radian1,radian2;
void setup(){
servo1.attach(9);
servo2.attach(7);
y=5;
b=5;
c=5;

a=sqrt(xx+yy);
pos1=((bb+cc-aa)/(2bc));
radian1=acos(pos1);
A=radian1180/M_PI;//M_PI（円周率）
B=180-A;//二個目のモーターの角度
pos2=(y/x);
pos5=((bb+aa-cc)/(2ba));
D=acos(pos5);
radian2=atan(pos2);
C=radian2180/M_PI-D;//一つ目のモーターの角度
}
void loop(){
for ( x = 0; x < 5; x += 1 ) {
servo1.write(C);
delay(1000);
servo2.write(B);
delay(1000);
}
for ( x = 5; x > 0; x -= 1 ) {
servo1.write(C);
delay(1000);
servo2.write(B);
delay(1000);
}
}

Answer 1

「目標座標へ２つのサーボを動かし、元の座標地までもどして、また目標座標へサーボを動かしてを繰り返すプログラムを作りたい」
これには、少なくとも以下の４つの値が必要です。
１つ目のサーボの元の座標
１つ目のサーボの目標座標
２つ目のサーボの元の座標
２つ目のサーボの目標座標

しかし、質問に書かれた繰り返し(loop)のコード（下に整形して引用）には、２つの値（BとC）しか使われていません。そして、loopの中でBとCの値は変わりません。
「１つ目のサーボをCの位置へ動かす、１秒待ち、２つ目のサーボをBの位置に動かす、１秒待ち」を同じB、Cで繰り返すのですから、サーボは同じ位置に止まってしまって当然です。

void loop(){ 
    for ( x = 0; x < 5; x += 1 ) { 
        servo1.write(C); 
        delay(1000); 
        servo2.write(B); 
        delay(1000); 
     }   
     for ( x = 5; x > 0; x -= 1 ) { 
        servo1.write(C); 
        delay(1000); 
        servo2.write(B); 
        delay(1000); 
     } 
}

ここからは私の推測ですが、質問者がやりたいのは、loopの中でxの値を差分１で０->5->0という変化をさせ、ｘの値が変わるたびにBとCの値をsetup関数で計算して、２つのサーボを動かすということでしょう。
この推測が正しいとすれば、loopのコードを以下のように変更すれば良いはずです。

void loop(){ 
    for ( x = 0; x < 5; x += 1 ) { 
        setup(); // B,Cの値を再計算
        servo1.write(C); 
        delay(1000); 
        servo2.write(B); 
        delay(1000); 
     } 
     for ( x = 5; x > 0; x -= 1 ) { 
        setup(); // B,Cの値を再計算
        servo1.write(C); 
        delay(1000); 
        servo2.write(B); 
        delay(1000); 
     } 
}

但し、上記のコードでは、サーボ１とサーボ２が代わる代わる動くので、カクカクとした動きになってしまいます。
サーボ１の動作とサーボ２の動作の間にある時間待ちを無くした下記のコードのほうが、質問者の望むものかもしれません。

void loop(){ 
    for ( x = 0; x < 5; x += 1 ) { 
        setup(); // B,Cの値を再計算
        servo1.write(C); 
        servo2.write(B); 
        delay(1000); 
     } 
     for ( x = 5; x > 0; x -= 1 ) { 
        setup(); // B,Cの値を再計算
        servo1.write(C); 
        servo2.write(B); 
        delay(1000); 
     } 
}