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

ペリフェラル
2023.11.15
image_print

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

ふたつのシリアルポート

Arduinoにはふたつのシリアルポートが必要です。
ひとつは元々USBを使うシリアルです。こちらは普段Arduino IDEに接続していますが、最終的にはRaspberry Piに接続してコマンドの送受信に使いたいものです。

もうひとつはセンサーを繋ぐシリアルです。
こちらは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でないと正しく測定できていません。
数メートルなら正確なのですが、極端に距離が近い、極端に遠い場合、数値は信頼できません。

システム全体像

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

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

  1. トリガーはフライトコントローラーからのAUXピンのUpです。
  2. それを検知すると、RaspiのプログラムはArduinoへSTARTコマンドを送ります。（USBシリアル経由）
  3. Arduinoはモーターでカメラ台をDOWNします。
  4. 深さセンサーが水底より2メートルを検知したら、Arduinoはモーターを止めます。
  5. ArduinoはRaspiにENDを返します。
  6. RaspiはGoProにhttpでシャッターコマンドを送ります。
  7. 数秒、シャッターを待ちます。
  8. RaspiはArduinoにREWINDコマンドを送ります。
  9. Arduinoはモーターでカメラ台をUPします。
  10. UPが終わったらArduinoはRaspiにRENDを返します。

Arduinoのプログラム

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

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

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

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

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

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

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

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

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

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

ソフトウェアシリアルのライブラリーとしてNeoSWSerialを使います。
AltSoftSerialも有名なのですが、ピン８を固定で使います。CNCボードではピン８は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 ) ) );
}

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

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

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