ステップモーター2台同時制御 |
ARDUINOでL6470ドライバーを使い、ステッピングモーター2台を同時制御します。次の動画は簡単な例ですが、2つのモーターを同時に制御しています。
SPI接続について |
L6470使用ステッピングモータードライバー(秋月電子通商)を用いて、2つのステッピングモーターを制御します。SPI接続はARDUINOに対して、1つのデバイスを接続して使用したことはありますが、複数デバイスを接続したことはなく、正直あまり理解していませんでした。
今回、2つの L6470 を2つ接続する為に調べてみました。下図の通り、クロックとデータ入出力はバス接続し、制御対象デバイスの選択は、SS(Slave Select)信号で行います。
対象デバイス出力(SS信号)をHigh→Lowに切り換えることで対象を選択できるので、思ったよりもシンプルでした。
配線接続 |
配線は次の図の通り、L6470ドライブキットを2台接続します。SPIデータ送受信信号線をバス接続とし、デバイス選択信号とデバイスBUSY信号は分けてArduino UNOに接続します。
リンク
プログラム |
Arduino Unoプログラムを掲載します。4pin をデバイス選択信号(Slave Select)、3pin をBUSY信号として定義し、制御対象のモーターに応じデバイス選択するロジックを追加しています。
#include <SPI.h> // ピン定義。 #define PIN_SPI_SCLK 13 // クロック(SPI CLK、SCLK) #define PIN_SPI_MISO 12 // マスタ入力/スレーブ出力(MISO) #define PIN_SPI_MOSI 11 // マスタ出力/スレーブ入力(MOSI) #define PIN_SPI_SS01 10 // スレーブ選択(Slave Select) #define PIN_MT_BSY01 9 // BUSY #define PIN_SPI_SS02 4 // スレーブ選択(Slave Select) #define PIN_MT_BSY02 3 // BUSY #define PIN_5 5 // リミットSW(原点側(左端)) #define PIN_6 6 // リミットSW(右端) #define PIN_7 7 // LED出力 #define PIN_8 8 // LED出力 // 対象デバイス初期値 int dvs_select=0; // 入力値初期化 int sw_sts_5 = LOW; int sw_sts_6 = LOW; // 初回セットアップ void setup() { // 各PIN入出力定義 pinMode(PIN_SPI_MOSI, OUTPUT); pinMode(PIN_SPI_MISO, INPUT); pinMode(PIN_SPI_SCLK, OUTPUT); pinMode(PIN_SPI_SS01, OUTPUT); pinMode(PIN_MT_BSY01, INPUT); pinMode(PIN_SPI_SS02, OUTPUT); pinMode(PIN_MT_BSY02, INPUT); pinMode(PIN_5, INPUT ); pinMode(PIN_6, INPUT ); pinMode(PIN_7, OUTPUT ); pinMode(PIN_8, OUTPUT ); SPI.begin(); SPI.setDataMode(SPI_MODE3); SPI.setBitOrder(MSBFIRST); dvs_select=0; SPI_DISCONNECT(); L6470_rst_device(); // L6470リセット L6470_set_parameter(); // L6470パラメータ設定 delay(500); dvs_select=1; get_origin(); // 原点復帰 dvs_select=0; L6470_data_transfer(0xd8,0,0); // 原点情報初期化 L6470_wait_not_busy(100); // 16000step=20.8cm → 16000 X 5 / 20.8 = 3846 dvs_select=0; L6470_data_transfer(0x41,3,3200); // 移動量・方向指定移動 L6470_wait_not_busy(800); L6470_data_transfer(0x70,0,0); // 原点位置移動 L6470_wait_not_busy(300); dvs_select=1; L6470_data_transfer(0x41,3,1600); // 移動量・方向指定移動 L6470_wait_not_busy(300); dvs_select=1; L6470_data_transfer(0x41,3,6400); // 移動量・方向指定移動 dvs_select=2; L6470_data_transfer(0x40,3,6400); // 移動量・方向指定移動 dvs_select=0; L6470_wait_not_busy(500); dvs_select=2; L6470_data_transfer(0x41,3,1600); // 移動量・方向指定移動 L6470_wait_not_busy(500); dvs_select=0; L6470_data_transfer(0x40,3,8000); // 移動量・方向指定移動 } // シリアルデータ送信 void snd_str(String snd_dat){ String snd_msg = " " + snd_dat; Serial.println(snd_msg); return; } // メイン処理 void loop(){ } // 原点復帰 void get_origin(){ digitalWrite( PIN_7 , LOW ); digitalWrite( PIN_8 , LOW ); // 反-原点方向(右)移動 if(digitalRead( PIN_5 )){ L6470_data_transfer(0x51,3,8000); // 速度指定移動(中速) delay(50); while(digitalRead( PIN_5 )){ delay(500); } L6470_data_transfer(0xb0,0,0); // 回転停止、保持トルク有 L6470_wait_not_busy(500); } // 原点方向(左)移動 L6470_data_transfer(0x50,3,8000); // 速度指定移動(中速) while(!digitalRead( PIN_5 )){} L6470_data_transfer(0xb0,0,0); // 回転停止、保持トルク有 L6470_wait_not_busy(500); // 反-原点方向(右)移動 L6470_data_transfer(0x51,3,500); // 速度指定移動(低速) delay(50); while(digitalRead( PIN_5 )){} L6470_data_transfer(0xb0,0,0); // 回転停止、保持トルク有(機械原点) L6470_wait_not_busy(500); // 反-原点方向(右)移動(移動量指定) // 600X20.8/16000=0.78(cm) L6470_data_transfer(0x41,3,600); // 移動量・方向指定移動 L6470_wait_not_busy(100); digitalWrite( PIN_7 , HIGH ); digitalWrite( PIN_8 , HIGH ); } // 初期設定 void L6470_set_parameter(){ L6470_data_transfer(0x05,2,0x0e); // [R, WS] 加速度default 0x08A (12bit) (14.55*val+14.55[step/s^2]) L6470_data_transfer(0x06,2,0x0e); // [R, WS] 減速度default 0x08A (12bit) (14.55*val+14.55[step/s^2]) L6470_data_transfer(0x07,2,0x0e); // [R, WR] 最大速度default 0x041 (10bit) (15.25*val+15.25[step/s]) L6470_data_transfer(0x08,2,0x01); // [R, WS] 最小速度default 0x000 (1+12bit) (0.238*val[step/s]) L6470_data_transfer(0x15,2,0x3ff); // [R, WR] μステップからフルステップへの切替点速度default 0x027 (10bit) (15.25*val+7.63[step/s]) L6470_data_transfer(0x09,1,0x50); // [R, WR] 停止時励磁電圧default 0x29 (8bit) (Vs[V]*val/256) L6470_data_transfer(0x0a,1,0x50); // [R, WR] 定速回転時励磁電圧default 0x29 (8bit) (Vs[V]*val/256) L6470_data_transfer(0x0b,1,0x50); // [R, WR] 加速時励磁電圧default 0x29 (8bit) (Vs[V]*val/256) L6470_data_transfer(0x0c,1,0x50); // [R, WR] 減速時励磁電圧default 0x29 (8bit) (Vs[V]*val/256) // 0x00 : 400 step/rot , 0x01 : 800 step/rot , 0x02 : 1600 step/rot , 0x03 : 3200 step/rot , // 0x04 : 6400 step/rot , 0x05 : 12800 step/rot , 0x06 : 25600 step/rot , 0x07 : 51200 step/rot //ステップモードdefault 0x07 (1+3+1+3bit) L6470_data_transfer(0x16,1,0x03); } // デバイスリセット void L6470_rst_device(){ L6470_data_send_u(0x00); //nop命令 L6470_data_send_u(0x00); L6470_data_send_u(0x00); L6470_data_send_u(0x00); L6470_data_send_u(0xc0); } // 送信データ加工 void L6470_data_transfer(int add,int bytes,long val){ int data[3]; L6470_data_send(add); for(int i=0;i<=bytes-1;i++){ data[i] = val & 0xff; val = val >> 8; } if(bytes==3){ L6470_data_send(data[2]); } if(bytes>=2){ L6470_data_send(data[1]); } if(bytes>=1){ L6470_data_send(data[0]); } } // データ送信(BUSY待機) void L6470_data_send(unsigned char add_or_val){ L6470_wait_not_busy(10); SPI_DVS_SELECT(); SPI.transfer(add_or_val); // アドレスもしくはデータ送信。 SPI_DISCONNECT(); } // データ送信(直ぐ送信) void L6470_data_send_u(unsigned char add_or_val){ SPI_DVS_SELECT(); SPI.transfer(add_or_val); // アドレスもしくはデータ送信。 SPI_DISCONNECT(); } void SPI_DVS_SELECT(){ if(dvs_select==1){ digitalWrite(PIN_SPI_SS01, LOW); // ~SSイネーブル。 digitalWrite(PIN_SPI_SS02, HIGH); // ~SSディスエーブル。 }else if(dvs_select==2){ digitalWrite(PIN_SPI_SS01, HIGH); // ~SSディスエーブル。 digitalWrite(PIN_SPI_SS02, LOW); // ~SSイネーブル。 }else{ digitalWrite(PIN_SPI_SS01, LOW); // ~SSイネーブル。 digitalWrite(PIN_SPI_SS02, LOW); // ~SSイネーブル。 } } void SPI_DISCONNECT(){ digitalWrite(PIN_SPI_SS01, HIGH); // ~SSディスエーブル。 digitalWrite(PIN_SPI_SS02, HIGH); // ~SSディスエーブル。 } // BUSY解除後、指定時間待機 void L6470_wait_not_busy(long time){ if(dvs_select==1){ while(!digitalRead(PIN_MT_BSY01)){} }else if(dvs_select==2){ while(!digitalRead(PIN_MT_BSY02)){} }else{ while(!digitalRead(PIN_MT_BSY01) && !digitalRead(PIN_MT_BSY02)){} // BUSY解除待機 } delay(time); }
まとめ |
使い方によっては、XYテーブルも作れそうですが、少し今回使用しているステッピングモータのパワー不足が心配です。