サーボモータ（SG90）を使って数字を描くことに挑戦しているが、サーボが数字に応じた動きをしない

Question

###前提・実現したいこと
rduinoとサーボモータ（SG90）3つを使って、数字を描くロボットアームを作っています。PlotClockというデジタル時計を実現したプロトタイプが存在し、以下がその参考にしているサイトです。ハードウェアはThingiverseからそのまま3Dプリンタで印刷しました。

ハードウェア
参考にしているコード

###発生している問題・エラーメッセージ
数字の「1」と「2」を描くためのコードをgithubの参考コードから抜粋し、サーボは動きます。しかしながら、その動きが、「１」も「2」も同じで数字が判別できない状態に陥っています。「３」や「６」もうまくかけていない状態で、困っています。

現状のアームの動き
こんな風に描きたい

異なる数字のコードをArduinoに書き込んでも、この動画にあるような動きがひたすら続きます。

数字の「１」を描くためのコード
「1」を描く部分は参考コードの189,190行目から抜粋しました。
###該当のソースコード

// delete or mark the next line as comment when done with calibration
#define CALIBRATION

// When in calibration mode, adjust the following factor until the servos move exactly 90 degrees
#define SERVOFAKTORLEFT 650
#define SERVOFAKTORRIGHT 650


// Zero-position of left and right servo
// When in calibration mode, adjust the NULL-values so that the servo arms are at all times parallel
// either to the X or Y axis
#define SERVOLEFTNULL 1900
#define SERVORIGHTNULL 984

#define SERVOPINLEFT  3
#define SERVOPINRIGHT 4

// length of arms
#define L1 35
#define L2 55.1
#define L3 13.2

// origin points of left and right servo
#define O1X 22
#define O1Y -25
#define O2X 47
#define O2Y -25

#include <Servo.h>

Servo servo1;  //
Servo servo2;  //
Servo servo3;  //

volatile double lastX = 75;
volatile double lastY = 47.5;


void setup() {
  // put your setup code here, to run once:
  drawTo(75.2, 47);

  servo2.attach(SERVOPINLEFT);  //  left servo
  servo3.attach(SERVOPINRIGHT);  //  right servo
  delay(1000);
}

void bogenGZS(float bx, float by, float radius, int start, int ende, float sqee) {
  float inkr = 0.05;
  float count = 0;

  do {
    drawTo(sqee * radius * cos(start + count) + bx,
           radius * sin(start + count) + by);
    count += inkr;
  }
  while ((start + count) <= ende);
}


void drawTo(double pX, double pY) {
  double dx, dy, c;
  int i;

  // dx dy of new point
  dx = pX - lastX;
  dy = pY - lastY;
  //path lenght in mm, times 4 equals 4 steps per mm
  c = floor(4 * sqrt(dx * dx + dy * dy));

  if (c < 1) c = 1;

  for (i = 0; i <= c; i++) {
    // draw line point by point
    set_XY(lastX + (i * dx / c), lastY + (i * dy / c));

  }

  lastX = pX;
  lastY = pY;
}

void set_XY(double Tx, double Ty)
{
  delay(1);
  double dx, dy, c, a1, a2, Hx, Hy;

  // calculate triangle between pen, servoLeft and arm joint
  // cartesian dx/dy
  dx = Tx - O1X;
  dy = Ty - O1Y;

  // polar lemgth (c) and angle (a1)
  c = sqrt(dx * dx + dy * dy); //
  a1 = atan2(dy, dx); //
  a2 = return_angle(L1, L2, c);

  servo2.writeMicroseconds(floor(((a2 + a1 - M_PI) * SERVOFAKTORLEFT) + SERVOLEFTNULL));

  // calculate joinr arm point for triangle of the right servo arm
  a2 = return_angle(L2, L1, c);
  Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5°
  Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI);

  // calculate triangle between pen joint, servoRight and arm joint
  dx = Hx - O2X;
  dy = Hy - O2Y;

  c = sqrt(dx * dx + dy * dy);
  a1 = atan2(dy, dx);
  a2 = return_angle(L1, (L2 - L3), c);

  servo3.writeMicroseconds(floor(((a1 - a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL));

}

double return_angle(double a, double b, double c) {
  // cosine rule for angle between c and a
  return acos((a * a + c * c - b * b) / (2 * a * c));
}


void loop() {
  // put your main code here, to run repeatedly

  // Servohorns will have 90° between movements, parallel to x and y axis
  drawTo(-3, 29.2);
  delay(500);
  drawTo(74.1, 28);
  delay(500);

}

//1を書く
void number(float bx, float by, int num, float scale) {

  drawTo(bx + 10 * scale, by + 20 * scale);
    drawTo(bx + 10 * scale, by + 0 * scale);
}

数字の「２」を描くためのコード
void loop()までは数字の「１」を描くためのコードと同様
「2」を描く部分は参考コードの196~198行目から抜粋しました。

void number(float bx, float by, int num, float scale) {


  bogenUZS(bx + 8 * scale, by + 14 * scale, 6 * scale, 3, -0.8, 1);
  drawTo(bx + 1 * scale, by + 0 * scale);
  drawTo(bx + 12 * scale, by + 0 * scale);
}

###補足情報(言語/FW/ツール等のバージョンなど)
ハードウェアをサーボにつける前にキャリブレーションが必要かもしれないのですが、方法がわからず、キャリブレーションなどはせずにそのままハードウェアを装着しました。

＜現状＞
1や2を描くためのコードが読み込まれておらず、現状ではキャリブレーションのみを行なっている可能性があると気がつきました。
他の製作者のキャリブレーション様子

Comments

[8524ba23]

サーボはプログラムで指定した位置に動作していますか？不明であれば、描かれた画像または動画をアップしてはいかがでしょうか？

[退会済みユーザー]

動画を追加させていただきました。ご提案ありがとうございます。

[退会済みユーザー]

追記で失礼します。現状の動きはキャリブレーションのみの可能性があるかもしれません。

Answer 1

動画確認しました。
アームの動作角度がオリジナルと異なりますね。
また、ゴム止め？しているためか、明らかにバックラッシュ大きすぎます。
まずは機構（ハード）を正しく構成し、直線を引くような単純な動作ができるか確認すべきだと思います。

Comments

[退会済みユーザー]

ご回答ありがとうございます。
ゴム止めに関しては、ネジだけでゴムを止めないとサーボの腕の部分がハードウェアとずれてきてしまうという問題があったため止めていました。
ハードを正しく構成するとはどういうことでしょうか。可能であれば、詳しくお聞かせ願います。

[8524ba23]

サーボとアームの取付位置、角度などをオリジナルと同じにすべきということです。
オリジナルのアームは２つの「く」の字が対向して四角形を形作っていますが、質問者様のは２つの「く」の字が同じ向きで四角形がつぶれたような形をしています。
また、動画のようにサーボとアームにずれが生じてしまうと、ペン先の位置も本来の位置からずれてしまいます。

[退会済みユーザー]

ご回答ありがとうございます。
２つの「く」の字が対向して四角形になるように構成したものを動画としてあげ直しました。

[ElecDove]

can110さんがおっしゃっているように，
>直線を引くような単純な動作ができるか確認すべきだと思います。
を先に検証すべきだと思います．drawTo関数で直線が引けるので，たとえば水平線を平行に引けるか，とか，垂直線を引けるか，とか，それができたら斜め線が引けるか，と言った具合に．
また，映像見る限りでは微妙にハード組み立てが甘いように思えました．オリジナルと間違い探しをしてみてください

[退会済みユーザー]

みなさまありがとうございました。