Arduinoを使う4軸ウィンチ(モーター制御編)

4つのモーターで水平昇降

4軸のステッピングモーターで昇降できるウィンチを作ることにしました。
なぜ普通のブラシモーターを使わないかというとステッピングモーターってトルクが強く、静止時にギアがなくても保持していることができて、比較的ゆっくり回せるからです。
巷には3Dプリンターのおかげで出回っています。
こんな感じで作ります。
Arduinoで細かい制御をして、Raspberry Piからのコマンドで動くことにします。

と、ん？

こういうボードはどっかで見たよな。

CNC用のモーター制御基板

あ、あれだ。

CNCマシン用のボード。
とくにArduinoに乗っけるピンが完備したボードを”シールド”と呼ぶ風習があります・
このボードはCNCマシンのキットに使われていて、GRBLというオープンソースで駆動します。
何年か前にさんざんいじってたヤツ。
とてもポピュラーで、ボードは激安で売られています。

とりあえず4個モーターがついたキットを購入。モーターは4401タイプなのだそうだ。
ボードのバージョンは無印のV3.0です。

ボードチューニング

4401は12Vで200カウントで一回転する。最大電流は1.7Aです。
モータードライブモジュールはA4988という最大で2A程度がドライブできます。Arduino Uno R３もコネクターは12Vまでサポートしているので電源は12Vで決まりです。
ドローン基盤から取れるということですね。どうするかは未定だけど。

A4988ボードはのっかっている極小のボリュームで最大出力電流をコントロールできます。
GNDとボリュームの中心にテスターをあて、電圧を測ります。もっとも右にまわして1.5Vくらいのはずです。
最大にすると結構発熱するので、4401モーターを使っている限りにおいては、中間（0.7Vくらい？）にしておいたほうがいいと思います。
MS0, MS1, MS2はドライブの精度をあげる場合に設定しますが、今回は回転数で数えるくらいの精度でいいので全部LOWにします。

なお、すべてのロジックは5V仕様となります。
ドローン系(Raspberry PI)は3.3V仕様ですから、接続時に5V-3.3Vのレベルコンバーターを挟む必要がありますが、以下のようにUSBインターフェースを使えば、そんな悩みはないです。

GRBL使っていた時にはArduinoとコントローラーのボードのピン配列なんか考えずに、言われたとおり繋いでいればよかったんですが、自分でコード書くならばそうもいきません。

とはいってもシールドのピンレイアウトは比較的どうでもいいです。
大事なものはこれ。

ここEnable, Direction, Stepが重要なピンです。

しかし！ここにはX,Y,ZしかなくAドライブについてはポートがありません！
それはこの動画(https://www.youtube.com/watch?v=UZcyh9qgwyQにあるとおり、

とジャンパーすればXドライブと同じようにAドライブは動くのです。
Aドライブはあくまでもフォロワーとして動くのですね。

A4988の電流制限（ポテンショメーターとGND間の電圧調整）はか習うやります。私の場合は0.45-0.46Vくらいで調整しています。
これをやらないと、ちょっとモーターに負荷がかかっただけで過電流となりモーターが逆回転を始めます。A4988のドキュメントをさらっと見ても記載は見つけられませんでしたが、現象としてワイヤーがひっかかりモーターに負荷がかかると勝手に逆回転する現象に遭遇しています。対策がこの電流調整でした。

なお、モーターケーブルの差し込みはこの方向にしました。反対でも当然動作しますが、統一しましょう。

モーターテスト

ボードに12V電源を繋いで、

Arduinoプログラムでのピンの定義

const int stepXPin = 2; //X-軸　ステップ
const int stepYPin = 3; //Y.軸　ステップ
const int stepZPin = 4; //Z.軸　ステップ

const int dirXPin = 5; // X-軸　方向　1-CW 0-CCW
const int dirYPin = 6; // Y-軸　方向
const int dirZPin = 7; // z-軸　方向

const int enPin=8; // スピンドル エネイブル

とりあえずX軸用モーターが回るプログラム

const int stepXPin = 2; //X-軸　ステップ
const int stepYPin = 3; //Y.軸　ステップ
const int stepZPin = 4; //Z.軸　ステップ

const int dirXPin = 5; // X-軸　方向　1-CW 0-CCW
const int dirYPin = 6; // Y-軸　方向
const int dirZPin = 7; // z-軸　方向

const int enPin=8; // スピンドル エネイブル

const int stepsPerRev=200; // 200カウントで1回転
int pulseWidthMicros = 100;  // microseconds　パルス幅
int millisBtwnSteps = 1000;  // 動作待ち時間

boolean direction = HIGH;

void setup() {
  Serial.begin(9600);

  pinMode(stepXPin, OUTPUT);
  pinMode(stepYPin, OUTPUT);
  pinMode(stepZPin, OUTPUT);
  
  pinMode(dirXPin, OUTPUT);
  pinMode(dirYPin, OUTPUT);
  pinMode(dirZPin, OUTPUT);
  
  pinMode(enPin, OUTPUT);
  digitalWrite(enPin, LOW);

  Serial.println("End of Initialization");
}


void loop() {
 // directionのロジックはわかりやすくするためダサダサです。
  if (direction == HIGH){
    digitalWrite(dirXPin, HIGH); // CW
    direction = LOW;
  } else {
    digitalWrite(dirXPin, LOW);
    direction = HIGH;
    
  }
  delay(100);

  for (int i = 0; i < stepsPerRev; i++) {
      digitalWrite(stepXPin, HIGH);
      delayMicroseconds(pulseWidthMicros);
      digitalWrite(stepXPin, LOW);
      delayMicroseconds(millisBtwnSteps);
  }

  delay(1000);
  
}

高度化させるためにいろいろなステッピングモーターコントロールプログラムを調べました。
困ったことにオープンソースの3DプリンターやCNCはモーターを一時点でひとつしか動かさないようで、stepperdriverやAccelStepperといったライブラリーは連続してモーターを回せない(360度回すと一瞬止まる）し動きがカクカク、同時に複数のモーターを動かせないようです。（なにか方法があるかもしれません。）

単なる昇降なので、自前でコードを書くほうがわかりやすく安心です。
単体ドライブのコードとの違いはdelayです。delaymaicrosecondsで待つと、モーター4台が同時に回りません。おかしな動きをします。
上の方法を拡張した次をみてください。

/*
 * X,Y,Z stepper Motor test
 * A Stepper should be controlled by jumper pins.
 * by Tsukasa Takao 2023
 */

const int stepXPin = 2; //X-軸　ステップ
const int stepYPin = 3; //Y.軸　ステップ
const int stepZPin = 4; //Z.軸　ステップ

const int dirXPin = 5; // X-軸　方向　1-CW 0-CCW
const int dirYPin = 6; // Y-軸　方向
const int dirZPin = 7; // z-軸　方向

const int enPin=8; // スピンドル エネイブル

const int stepsPerRev=200; // 200カウントで1回転
int pulseWidth = 3;  // ミリで十分。microsecondsは駄目


void setup() {
  Serial.begin(9600);

  pinMode(stepXPin, OUTPUT);
  pinMode(stepYPin, OUTPUT);
  pinMode(stepZPin, OUTPUT);
  
  pinMode(dirXPin, OUTPUT);
  pinMode(dirYPin, OUTPUT);
  pinMode(dirZPin, OUTPUT);

  pinMode(enPin, OUTPUT);
  digitalWrite(enPin, LOW);
  Serial.println("End of Initialization");
}

void rotate(int count){
  Serial.println("Start Rotation");
  for (int i = 0; i < stepsPerRev*count; i++) {
      digitalWrite(stepXPin, HIGH);
      digitalWrite(stepYPin, HIGH);
      digitalWrite(stepZPin, HIGH);
      delay(pulseWidth);
      digitalWrite(stepXPin, LOW);
      digitalWrite(stepYPin, LOW);
      digitalWrite(stepZPin, LOW);
      delay(pulseWidth);    
  }
   Serial.println("End Rotation");
}

void moveDown(){
   digitalWrite(dirXPin, HIGH);
   digitalWrite(dirYPin, LOW);
   digitalWrite(dirZPin, HIGH);
   rotate(10);
}

void moveUp(){
   digitalWrite(dirXPin, LOW);
   digitalWrite(dirYPin, HIGH);
   digitalWrite(dirZPin, LOW);
   rotate(10);
}

void loop() {
  moveDown();
  delay(1000);
  moveUp();
  delay(1000);
}

レイアウト（プログラム上、A軸はX軸のフォロワーなので出てこない）

いろいろ調べても同時にステッピングモーターを回したい人はあまりいないらしく、おかしな動きも自己解決するしかありませんでした。

システム全体像

ウィンチ制御システムのRaspi側のプログラム起動から見ていきます。

1. ユーザーはRaspiのプログラムからArduinoへSTARTコマンドを送ります。（USBシリアル経由)
2. Arduinoはモーターでカメラ台をDOWNします。
3. ユーザーは適当なところで（水深計を見ながら）STOPコマンドで停止させます。もし、ケーブルが伸び切ったらセンサーで検知し止まります。
4. もしビデオならば撮りっぱなしでOKだし、写真ならウィンチはこのまあカメラを操作します。
5. ユーザーはArduinoにREWINDコマンドを送ります。
6. Arduinoはモーターでカメラ台をUPします。
7. UPが終わったらセンサーで検知して、自動的に止まります。

ワイヤーの終了（最後と最初）

で巻取りを終わらねばなりません。そのために金属球をセンサーに採用しました。

が空間で使う場合は有効かもしれないので消さずに置いてあります。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)が検知したらモーターを止める。

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

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

Z+のポート11をセンサーに使うことにします。

通信

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

最終的にウィンチの制御プログラム(boat_winch.ino)は次のとおり。

/*
 * Arduino Camera platform driver
 *  
 * PinchangeInterrupt module is required. Please install from library manager.
 * 
 *   by Tsukasa Takao@Umineco ltd. 2023/11, 2024/6, 2025/2
 */
#include 

#define DEBUG

// Ball sensor
#define ballSensePin 11 // Motor senser pin
volatile boolean ballSense = HIGH; // By program and Interruption Handler

// CNC motor drive board
const int stepXPin = 2; // X-axis step pin
const int stepYPin = 3; // Y-axis step pin
const int stepZPin = 4; // Z-axis step pin

const int dirXPin = 5; // X-axis direction pin
const int dirYPin = 6; // Y-axis direction pin
const int dirZPin = 7; // Z-axis direction pin

const int enPin = 8; // spindle enable pin

const int stepsPerRev = 200; // 200カウントで1回転
int pulseWidth = 3; // microsecond モーターパルス幅　テスト結果の最小値

String Cmd; // コマンド

#define START "start"
#define STOP "stop"
#define REWIND "rewind"

// 1回転
void rotate(int count){
  for (int i = 0; i < stepsPerRev*count; i++) { if (ballSense == LOW){ // By some reason, stop rotation ! break; } digitalWrite(stepXPin, HIGH); digitalWrite(stepYPin, HIGH); digitalWrite(stepZPin, HIGH); delay(pulseWidth); digitalWrite(stepXPin, LOW); digitalWrite(stepYPin, LOW); digitalWrite(stepZPin, LOW); delay(pulseWidth); } } void moveDown(){ #ifdef DEBUG Serial.println("directin DOWN"); #endif digitalWrite(dirXPin, HIGH); digitalWrite(dirYPin, LOW); digitalWrite(dirZPin, LOW); } void moveUp(){ #ifdef DEBUG Serial.println("directin UP"); #endif digitalWrite(dirXPin, LOW); digitalWrite(dirYPin, HIGH); digitalWrite(dirZPin, HIGH); } void Ball_ISR(){ ballSense = LOW; } void setup() { // Sensor setup pinMode(ballSensePin, INPUT_PULLUP); // always HIGH except detected. // Motor setup pinMode(stepXPin, OUTPUT); pinMode(stepYPin, OUTPUT); pinMode(stepZPin, OUTPUT); pinMode(dirXPin, OUTPUT); pinMode(dirYPin, OUTPUT); pinMode(dirZPin, OUTPUT); pinMode(enPin, OUTPUT); // Serial baud rate 9600bps Serial.begin(9600); delay(200); attachPCINT(digitalPinToPCINT(ballSensePin), Ball_ISR, FALLING); digitalWrite(enPin, LOW); // allways enable Serial.println("Serial Ready."); } void loop(){ // command process if (Serial.available() > 0){
    Cmd  = Serial.readString();
    Cmd.trim(); // remove blank, CR, LF
    ballSense = HIGH; // restart 
  }

  // command process, but stop is emergency.
  if (Cmd == START){
    #ifdef DEBUG
    Serial.println("Start !");
    #endif
    moveDown();
  } else if (Cmd == REWIND){
    #ifdef DEBUG
    Serial.println("Rewind !");
    #endif
    moveUp();
  } else if (Cmd == STOP){
    ballSense = LOW;
  } else {
    ballSense = LOW;
  }
  
  if (ballSense == HIGH){
    rotate(2);
  }  
} // end loop