| ステップモーター２台同時制御 |
ARDUINOでL6470ドライバーを使い、ステッピングモーター２台を同時制御します。次の動画は簡単な例ですが、２つのモーターを同時に制御しています。
| SPI接続について |
L6470使用ステッピングモータードライバー（秋月電子通商）を用いて、２つのステッピングモーターを制御します。SPI接続はARDUINOに対して、１つのデバイスを接続して使用したことはありますが、複数デバイスを接続したことはなく、正直あまり理解していませんでした。
今回、２つの L6470 を２つ接続する為に調べてみました。下図の通り、クロックとデータ入出力はバス接続し、制御対象デバイスの選択は、SS（Slave Select）信号で行います。
対象デバイス出力（SS信号）をHigh→Lowに切り換えることで対象を選択できるので、思ったよりもシンプルでした。
| 配線接続 |
配線は次の図の通り、L6470ドライブキットを２台接続します。SPIデータ送受信信号線をバス接続とし、デバイス選択信号とデバイスBUSY信号は分けてArduino UNOに接続します。
リンク
| プログラム |
Arduino Unoプログラムを掲載します。４pin をデバイス選択信号（Slave Select）、３pin を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テーブルも作れそうですが、少し今回使用しているステッピングモータのパワー不足が心配です。