SG90でキッチリ0/180度を出してみた

ライブラリを使ったサーボ操作（SG90）の真面目な実験結果メモです。

結論　キッチリと0/180度で停止させる設定方法
１　軸受け凸の辺りを０度とみなして、とりあえずサーボホーンを取り付ける。
２　以下のプログラムを　servo2.attach(9);　で動かし、サーボホン０度の角度ズレを手で補正する。（ギアかみ合わせを変更し0に”近い”角度に持っていく）
３　関数 servo2.attach(9,xxx, yyy ); に変更し、正確な位置で止まるようにxxx,yyy パラメータを補正していく。

実用角度に合わない場合は、以上の補正パラメータが取得できてから、サーボホーンを90度単位で変更するが吉（と思う）

－－－－－－－－－－－－－－－－－－－－－
関数を参考としたサイトはこちら

※　micro:bitでの角度設定は、南が90度、西が0度、東が180度になってる。

先ずは、サーボをちゃんと立てて固定する。

凸が下側に来るようにダンボーへセットした。
この位置がArduinoのライブラリ「Servo.h」で規定する「0度」あたり
反対側（上空）が「180度」あたりになる。

通電してない状態で、サーボホーンを手で回すと実際には200度近く回る。

その結果だろうか、真下が0度ではないのだ。上画のような位置（左側）までサーボ軸は回ってしまう。

このズレの件は一旦置いておいて、
ライブラリ使い　180度 => 90度 => 0度　と動くように、以下のように書いてみる。

#include "Servo.h"
Servo servo2;

void setup( ) {
}

void loop( ) {
  servo2.attach( 9 );   //　※　servo2.attach(9,480,2460); が私のこのサーボでは正解です
  servo2.write(180);
  delay(1500);
  servo2.write(90);
  delay(500);
  servo2.write(0);
  delay(500);

  servo2.detach( );
  delay(2000);
}

※の部分 今回は、パラメータ1個だけ。
これだと、ライブラリ内部の値が用いられてしまうようで、少々問題がある。

まずは実際に動かした結果、基準の0度が結構ズレていたのでサーボホーンを外し、概ね真下・真上になるように調整し直した。
が、このプログラムでは、イマイチ正確に上下で止めることはできなかった。
結局、キッチリ上下が出ない原因は、サーボ個々のバラツキなんだろう。

そのために、※のattach( ) 引数には、第2第3の引数が使えるオプションがある。

あれこれ試した結果、このサーボの固有値を見つけることができた。（目視の範囲ですが）

servo2.attach( 9, 480, 2460 );

このパラメータはPWMのマイクロ秒値だそうです。
筐体側面にメモ書きしておかんとイカンね。
参考ですがデータシート上は 500 ~ 2400 μsだそうです。

これで見事に上下が垂直に揃いましたとさ。

いや、めでたしめでたし。

 

PlotClockのパラメータ設定作業

さて、午前中のグダグダタイムを終え、フキの佃煮を作ったので本日のメインイベント「PlotClockのパラメータ調整」

上写真が現在の状態。アームとサーボ・ホーンはマスキングテープで固定している。
（ネットで拾ったSTLデータがアームにホーンをはめ込める奴でなかったのが間違い。）

まずは角度指定でどう動くのかやってみた。左サーボは反対側に向けて動くんだがいいんだろうか？

ネットを見ると左右のアームが水平垂直になるときの数値を得ろと言っている。（こちらのほうが分かり易い）
そもそも、サンプルプログラムには「調整（Calibrate）」モードがある。
が、盤面の左右をペンを持つアームが忙しく動き回るのを目視確認し、パラメータを毎回書き込んでコンパイル＆調整を繰り返す必要がある。
結構セワシくて面倒くさい。ここからが一番の関門と言われている。

　

上は「左サーボ　左向け！」　と　「右サーボ 右向け！」　状態

先ずは上のような状態になる「SERVOxxxxNULL」の値を探す。
「左サーボ　左向け！」　と　「右サーボ 右向け！」　状態になる値を探す。
それだけだと850/700になったのだが、次の「SERVOFAKTORxxxx」値設定作業をやると無理が出てくるので変更せざるをえ無くなった。

続いて「SERVOFAKTORxxxx」の値を探す。

「左サーボ 前向け！」　と　「右サーボ　前向け！」の値を探す。
ところが、この段でサーボホーンの取付角度をやり直したり色々・・・試行錯誤があり、
結局先ほど求めた「SERVOxxxxNULL」の値も変えざるを得なくなる。

はい、正直よく判ってなかったので、めちゃ時間がかかりました。

（左のサーボは少し動き始めている）

上の写真は「左サーボ　左向け！」　と　「右サーボ　前向け！」　の状態ですね。

結構試行錯誤が大変

ソースを見ればわかるが、サーボの動作はPWMのマイクロセコンド値を直接発行し、動作させている。
余り delay( )数値が小さいとサーボ動作が間に合わないので、500ms程は入れること。

Youtubeにアップした動作の動画

もう疲れた・・・

＝＝＝＝＝＝設定時ソース＝＝＝＝＝＝＝＝＝＝＝＝＝

//-------ピンアサイン-------------
#define SERVOPINLIFT  2   // Liftサーボ
#define SERVOPINLEFT  3   // 左サーボ
#define SERVOPINRIGHT 4   // 右

// 左右サーボホーン　左/右　向け！ 
// 各サーボ本体のゼロ度の値　（凸の尖った向きがゼロ度）
#define SERVOLEFTNULL 1625  // 左サーボ　左向け！
#define SERVORIGHTNULL 700 // 右サーボ 右向け！

// 各サーボ　前向け！
#define SERVOFAKTORLEFT 650  //　左サーボ 前向け！
#define SERVOFAKTORRIGHT 1700 // 右サーボ　前向け！

// lift positions of lifting servo
// 垂直方向の高さ調整　水平が1400 数値が大きいほど下に下る
#define LIFT0 1400  // 1080 机の面高 on drawing surface
#define LIFT1 1350  // 925 文字描画のスキップ高　between numbers
#define LIFT2 1200  // 725 消去パッドへ入れる高さ　going towards sweeper

// speed of liftimg arm, higher is slower
#define LIFTSPEED 1500

// ★length of arms
#define L1 35
#define L2 55.1 //org 55.1 実測は44だったが・・・
#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"

int servoLift = 1500;

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

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

int last_min = 0;
//============================================
void setup() { 
  Serial.begin(9600);
  servo1.attach(SERVOPINLIFT);  //  lifting servo
  servo2.attach(SERVOPINLEFT);  //  left servo
  servo3.attach(SERVOPINRIGHT);  //  right servo
  delay(1000); 
} 

void loop() {  // 
  chousei();
  delay(2000);
}

//----------サーボの角度調整確認--------------------
void chousei(){
  servo2.writeMicroseconds(650);  // SERVO FAKTOR LEFT      左サーボ　前向け！
  delay(500);
  servo3.writeMicroseconds(1700); // SERVO FAKTOR RIGHT  右サーボ　前向け！
  delay(500);
  servo2.writeMicroseconds(1625); // SERVO LEFT　NULL　  左サーボ　左向け！
  delay(500);
  servo3.writeMicroseconds(700);  // SERVO RIGHT NULL      右サーボ　右向け！
}

//============================================
void lift(char lift) {
  switch (lift) {
  case 0: //850
     if (servoLift >= LIFT0) {
      while (servoLift >= LIFT0) 
      {
        servoLift--;
        servo1.writeMicroseconds(servoLift);        
        delayMicroseconds(LIFTSPEED);
      }
    } 
    else {
      while (servoLift <= LIFT0) {
        servoLift++;
        servo1.writeMicroseconds(servoLift);
        delayMicroseconds(LIFTSPEED);
      }
    }
    break;
  case 1: //150
    if (servoLift >= LIFT1) {
      while (servoLift >= LIFT1) {
        servoLift--;
        servo1.writeMicroseconds(servoLift);
        delayMicroseconds(LIFTSPEED);
      }
    } 
    else {
      while (servoLift <= LIFT1) {
        servoLift++;
        servo1.writeMicroseconds(servoLift);
        delayMicroseconds(LIFTSPEED);
      }
    }
    break;
  case 2:
    if (servoLift >= LIFT2) {
      while (servoLift >= LIFT2) {
        servoLift--;
        servo1.writeMicroseconds(servoLift);
        delayMicroseconds(LIFTSPEED);
      }
    } 
    else {
      while (servoLift <= LIFT2) {
        servoLift++;
        servo1.writeMicroseconds(servoLift);        
        delayMicroseconds(LIFTSPEED);
      }
    }
    break;
  }
}

void bogenUZS(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 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;
}

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

}

 

暇

 

いや～

ここまで空白が多いカレンダーも珍しい。
日曜日に入ってる2件は「メール発信日」と「団体が行う他人担当行事」で、あとは2日前のお仕事だけ

暑いしArduino勉強も進むし良いんだけども。

キョウイク・キョウヨウ　大事ですね。