25 | 1月 | 2020

位置決めスライダープログラム

　Arduino UNOから、SPIインタフェイスを介し、L6470ドライブキットにコマンド送信しステッピングモータを制御します。キットにも取扱説明書が付属していますが、素人の私には簡単には判りませんので、“北の国から電子工作”というサイトで公開されているARDUINOプログラムを勉強させて頂きました。有難うございます。
先回投稿“位置決めスライダーの製作①”で掲載した動画のArduino UNOのプログラムを参考に掲載します。

#include <SPI.h>

// ピン定義。
#define PIN_SPI_MOSI 11
#define PIN_SPI_MISO 12
#define PIN_SPI_SCK 13
#define PIN_SPI_SS 10
#define PIN_BUSY 9

#define PIN_5 5
#define PIN_6 6
#define PIN_7 7
#define PIN_8 8

int sw_sts_5 = LOW;                   // 初期入力状態設定
int sw_sts_6 = LOW;                   // 初期入力状態設定


// 起動時実行
void setup()
{
  delay(1000);
  pinMode(PIN_SPI_MOSI, OUTPUT);
  pinMode(PIN_SPI_MISO, INPUT);
  pinMode(PIN_SPI_SCK, OUTPUT);
  pinMode(PIN_SPI_SS, OUTPUT);
  pinMode(PIN_BUSY, 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);
  
  digitalWrite(PIN_SPI_SS, HIGH);

  L6470_rst_device();                       // L6470リセット
  L6470_set_parameter();                    // L6470パラメータ設定
  delay(500);

  get_origin();                             // 原点復帰動作
  L6470_data_transfer(0x60,3,(long)2307);   // 絶対値移動指令(目盛30mm位置)  
}

// メインループ
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);

  L6470_data_transfer(0x41,3,600);          // 反-原点方向（右）長さ指定移動
  L6470_wait_not_busy(100);
  
  L6470_data_transfer(0xd8,0,0);            // 原点情報初期化
  L6470_wait_not_busy(800);

  digitalWrite( PIN_7 , HIGH );
  digitalWrite( PIN_8 , HIGH );
}


// 初期設定
void L6470_set_parameter(){
  L6470_data_transfer(0x05,2,0x0e);  
  L6470_data_transfer(0x06,2,0x0e);   
  L6470_data_transfer(0x07,2,0x0e);    
  L6470_data_transfer(0x08,2,0x01);  
  L6470_data_transfer(0x15,2,0x3ff); 
  L6470_data_transfer(0x09,1,0x50);   
  L6470_data_transfer(0x0a,1,0x50);   
  L6470_data_transfer(0x0b,1,0x50);   
  L6470_data_transfer(0x0c,1,0x50);   

  // マクロステップ指定（ 0x03 : 1/8 ）
  // 実測値：16000step=20.8cm → 15385step=20.0cm　→  77step=1mm(→15400step=20.0cm)
  // 計算値：1/8マイクロステップ→400*8(STEP/ROT)→3200(STEP/ROT))/40(mm/rot)=80(STEP/ROT)
  L6470_data_transfer(0x16,1,0x03);
}

// L6470リセット
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);
}

// L6470送信データ加工
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]);
  }  
}

// L6470データ送信(BUSY解除待機)
void L6470_data_send(unsigned char add_or_val){
  while(!digitalRead(PIN_BUSY)){}
  
  digitalWrite(PIN_SPI_SS, LOW);          // ~SSイネーブル。
  SPI.transfer(add_or_val);              // アドレスもしくはデータ送信。
  digitalWrite(PIN_SPI_SS, HIGH);         // ~SSディスエーブル。
}

// L6470データ送信(BUSY状態関係なし)
void L6470_data_send_u(unsigned char add_or_val){
  digitalWrite(PIN_SPI_SS, LOW);          // ~SSイネーブル。
  SPI.transfer(add_or_val);               // アドレスもしくはデータ送信。
  digitalWrite(PIN_SPI_SS, HIGH);         // ~SSディスエーブル。
}

// BUSY解除待機
void L6470_wait_not_busy(long time){
  // BUSY解除待機
  while(!digitalRead(PIN_BUSY)){}
  delay(time);
}

スライダー移動量の計算

　Arduino UNOプログラムから、L6470に対し、移動量指定する際の考え方を書いておきます。今回、プログラム104行目のマイクロステップ指定で “１／８”に細分化しています。表１に関連データと表２に表１データをもとに１mm移動するのに必要なSTEP数を計算します。
この計算では、80 [STEP/mm]となりました。

表１：スライダーの移動量を計算する為のデータ
No	項　目	内　容
１	L6470マイクロステップ設定	１／８
２	ステッピングモーター分解能	400 [STEP/回転]
３	タイミングベルトピッチ	２[mm]
４	プーリー歯数	20

表２：１mm移動するのに必要なSTEP数
No	項　目	内　容
１	モーター１回転当りSTEP数	８X 400 [STEP/回転]＝3200 [STEP/回転] （マイクロステップ設定考慮）
２	モーター１回転当り移動量	２[mm] X 20 = 40 [mm/回転]
３	１mm当り STEP数	3200[STEP/回転] ÷ 40[mm/回転] ＝ 80 [STEP/mm]

ただ、実際に動かしてみると「目盛」と一致しません。プーリー端数やタイミングベルト伸びなどを疑いましたが、原因は「目盛」がパソコンから印刷しているため、実スケールと異なっていることでした。
参考までに実測方法について記載します。いきなり多く移動させるとオーバーランなどリスクがあるので、少しずつ移動量を増やし、“0” [mm]の位置から、16000 [STEP]移動させた時に「目盛」の 20.8 [cm] の位置に移動しましたので、次の様に計算します。
→ 16000 [STEP] ÷ 208 [mm] = 76.9 [STEP/ｍｍ]

　市販の定規で改めて確認し、表２の計算が正しいことは確認出来ました。ただ、「目盛」を貼り直すと黒い塗装が剥がれてしまうなどの問題もあるので、実測値に基づき移動量計算をすることにしています。

主要パーツ

　主要パーツについて記載します。
タイミングベルトのセットが意外に安く助かりました。中国製の３Dプリンター等が安く販売されていますが、その補修パーツの様です。

L6470ﾄﾞﾗｲﾌﾞｷｯﾄ	AE-L6470DRV	秋月電子
ｽﾃｯﾋﾟﾝｸﾞﾓｰﾀｰ	ST-42BYH1004-5013
ﾘﾐｯﾄSW（ﾏｲｸﾛSW）	SS-10GL13
ﾌﾗｯﾄｹｰﾌﾞﾙ	DG01032-0012-01
KeeYees GT2 ﾀｲﾐﾝｸﾞﾍﾞﾙﾄ	―	AMAZON
ﾐﾆｶﾞｲﾄﾞﾚｰﾙ	MRS15-400	ｽｶﾞﾂﾈ工業
ﾐﾆｶﾞｲﾄﾞﾚｰﾙ	MR-15CS	ｽｶﾞﾂﾈ工業