Arduino とセンサーを使う4軸ウィンチ(2)

ペリフェラル
image_print

Aarduino CNCボードを使う4軸ウィンチの続きです。

この項ではワイヤーの終了(最後と最初)を示すために金属球をセンサーに採用した話と、後半にレーザー距離計を取り付ける話を書いてあります。

実際に試すとレーザー距離計は海中が汚れていたりすると、途端に誤動作をし、信頼できないことがわかり、現在、採用していません。
が空間で使う場合は有効かもしれないので消さずに置いてあります。

システム全体像

ここで一度、システム全体の概要を見たいと思います。以下の図のようになります。

Raspiのプログラム起動から見ていきます。

  1. ユーザーはRaspiのプログラムからArduinoへSTARTコマンドを送ります。(USBシリアル経由)
  2. Arduinoはモーターでカメラ台をDOWNします。
  3. ユーザーは適当なところで(水深計を見ながら)STOPコマンドで停止させます。
  4. もしビデオならば撮りっぱなしでOKだし、写真ならここで司令を送ります。
  5. ユーザーはArduinoにREWINDコマンドを送ります。
  6. Arduinoはモーターでカメラ台をUPします。
  7. UPが終わったらArduinoはRaspiにRENDを返します。

Arduinoのプログラム

さて、ここで測距センサー付きカメラ台をコントロールするArduinoのプログラムのアルゴリズムを考えてみます。
(しかし、アルゴリズムの書き方ってオレが学生の40年以上前から進歩していないのかよ。けっ)

  1. Raspiから設定コマンドが来ることがあるので文言を解析して設定する。(Depth)
    ただし、設定はすべてデフォルト値をもつものとする
  2. RaspiからSTARTコマンドが来たら、モーターを回してカメラ台を下げる(DOWN)
  3. もし、ワイヤーの限界まで下げた(Senser)が、指定の深さよりも深い場合、END with エラー(コード2)を返す。モーターを止める。
  4. STOPコマンドが来たら、モーターを止める。
  5. (カメラがRaspiの指定により撮影する)
  6. RASPIからREWINDコマンドが来たら、モーターを回してカメラ台を上げる(UP)
  7. ワイヤーが最短になったこと(Senser)が検知したらモーターを止める。

上記のふたつの「もし」は即応性が求められます。なぜならば、このイベントが起きたら即座にモーターの回転を止めることが好ましいからです。

ワイヤーのセンサーとして巻取り終了と、終わりは真鍮のボールを取り付けてます。

なお、プログラムとしての全容はGithubで公開しています。

ふたつのシリアルポート

もし、レーザー測距センサーを使う場合、Arduinoにおいて、ふたつのシリアルポートが必要です。
ひとつは元々USBを使うシリアルです。こちらは普段Arduino IDEに接続していますが、最終的にはRaspberry Piに接続してコマンドの送受信に使いたいものです。
具体的にはSerial.available関数でコマンドを知ります。
Arduino標準のシリアル通信は誤解されていることが多いように思います。多くの人がSerial.availableを出した時にArduinoのシリアルはデータを受け取ろうとしていると思っているようです。
よくよく考えてみてください。それではデータを取りこぼします。ArduinoはSerialをオープンした時点でデータを受け取ると割り込み処理をしてバッファーにデータを受け取っています。
したがって必要な時にデータの有無を調べ、評価することが正しいコードの書き方のようです。

もうひとつはセンサーを繋ぐシリアルです。
こちらはGPIOピンから作り出すしかありません。
ArduinoにはSoftwareSerialライブラリーがあり、ピンを指定することでシリアル通信できます。

幸いにしてGRBLボードの移動リミッターに使うXYZ(デジタルポート9,10,11)は使っていませんし、調べると別にプルアップ(電圧を与えられている)わけでもないようです。
したがって、ポート9, 10をセンサーのシリアル通信に使うことにします。

繋ぐ測距センサーの条件は以下のものです。

  • UART 5V通信
  • 耐水
  • ケーブルがあるもの
  • 安価

いろいろ探していて、Wit MotionのWT-VL53R-TTLという製品を入手しました。
ただし、提供されるソフトウェアはバグだらけで使い物にならないようです。とくに設定を変えると指定どおりにならず、あの世へ飛んでいきます。
せっかく買ったセンサーを設定がおかしくなっただけで諦めたくはありません。
WIt Motionのサポートに連絡したら、かなり親切でした。
16進数を送受信するツールをくれて指示どおりのコマンドを送って、なんとかリセットできました。
いろいろやっていて変更できたのは、デフォルトではサンプリング速度20Hzですが、なんとか2Hzにはできました。
このセンサーには「距離が◯◯になったらアラームを上げる」という便利な機能がついているのですが、他のセンサーを使うかもしれないので今回は測距データから判断することにします。

このセンサーを上のボードに繋いでなんとか測距データが出るように書いてみた習作は次のとおりです。通常のシリアルも文字を書き込めばエコーされます。



#include <SoftwareSerial.h>

#define RX_PIN 9
#define TX_PIN 10
#define BUFFLEN 50

SoftwareSerial dSerial(RX_PIN, TX_PIN);

char cStr[BUFFLEN];
unsigned short usCnt = 0;

void setup() {
  // Serial baud rate 9600bps
  Serial.begin(9600);
  delay(200);
  
  // dSerial.begin(9600);
  // dSerial.begin(19200);
  // dSerial.begin(38400);
  // dSerial.begin(57600);
  dSerial.begin(115200);
  dSerial.flush();
  
  delay(200);
  Serial.println("Serial/Software Serial Ready.");
}

void loop() {
  // Echo back string
  if (Serial.available() > 0){
    String data = Serial.readString();
    Serial.print("Echo: ");
    Serial.println(data);
  }
  
  while (dSerial.available())
    {
      cStr[usCnt] = dSerial.read();
      usCnt=usCnt+1;
      // 距離が信頼できるか? 0ならOK他なら疑わしい
      if ((cStr[0] == 'S') && (cStr[1] == 't')&&(cStr[2] == 'a') )
      {
        if ((cStr[usCnt-2] == ' ') && (cStr[usCnt-1] == ','))
        {
          int state;
          sscanf(cStr, "State;%d ,", &state);
          Serial.print("State:");
          Serial.println(state);
          usCnt=0; // reset counter 
        }
      }
      else if ((cStr[0] == 'd') && (cStr[1] == ':'))
      {
        if ((cStr[usCnt-2] == '\r') && (cStr[usCnt-1] == '\n'))
        {
          int distance;
          sscanf(cStr, "d: %d\r\n", &distance);
          Serial.print("Distance:");
          Serial.print(distance);
          Serial.print(" mm");
          Serial.print("\r\n");
          usCnt=0; // reset counter
        }
      }
      else if (usCnt > 2) {
        usCnt=usCnt-1;
        memcpy(&cStr[0], &cStr[1], usCnt);
      }
    }

} 

出力はこんな感じです。なんとミリメートルで出るんですね。

なお、statusは大事で0でないと正しく測定できていません。
数メートルなら正確なのですが、極端に距離が近い、極端に遠い場合、数値は信頼できません。

 

ソフトウェアシリアルとGPIOの割り込み

ここまではシリアルがふたつ必要だということでSoftwareSerialライブラリーを使っていました。ここでワイヤーの始まりと終わりを示すボールセンサー検知の割り込み(Interruption)を設定するとコンパイルエラーになります。
センサー検知割り込みを避けようかとも思いました。
しかしやってみると周期的にセンサーを読み取っているのではどうしても反応が遅くなりますし、チャタリング(バウンシング)の問題も潰さねばなりません。
するとやはり検知したら割り込み処理でフラグセットするのが最速となります。

具体的にはsetupに記述すべき次のコードです。

 attachPCINT(digitalPinToPCINT(Sense_PIN), BallSense_ISR, FALLING);

ソフトウェアシリアルとGPIOの割り込みは共存できないことは、一部では知られているようです。

これは悩みました。
私なりに最も簡単だと思われる解決策が以下のものです。

ソフトウェアシリアルのライブラリーとしてNeoSWSerialを使います。
AltSoftSerialも有名なのですが、ピン8を固定で使います。CNCボードではピン8はEnableピンとして固定されており、変更はできません。

NeoSWSerialをライブラリーとしてインストールし、この部分のコメントをはずします。

次にsetup()関数にこんな記載をします。

 pinMode(Sense_PIN, INPUT_PULLUP);

  attachPCINT(digitalPinToPCINT( RX_PIN) , myISR, CHANGE); // シリアル用
  attachPCINT(digitalPinToPCINT(Sense_PIN), Sense_ISR, FALLING); // センサー用

NeoSWSerialが使うRX_ピンとGPIOをセンサーとして使うSense_ピンに割り込みルーチンを設定します。
RX_Pin割り込みルーチンのmyISRは次のとおりです。


void myISR()
{
  NeoSWSerial::rxISR( *portInputRegister( digitalPinToPort( RX_PIN ) ) );
}

とりあえずこれで。ソフトウェアシリアルとセンサーピンの割り込みはコンパイルできて、動作するようです。

 

タイトルとURLをコピーしました