ステッピングモータートルク保持休憩（リミットスイッチで原点復帰）

Question

２相ステッピングモーターをL6470ドライバーとarduinoで動かしています。
arduinoは初めてで、いろんなところから拾って書いてみました。
動きとしては、電源ON→ステッピングモーターが正転→リミットスイッチに当たる（switch ON）→モーターが550秒100のスピードで逆転→トルク保持のまま100ミリ秒休憩→モーターが550秒100のスピードで正転→トルク保持のまま100ミリ秒休憩→の繰り返し

スイッチONからモーター正転逆転はできたのですが、「トルク保持のまま休憩」ができません。
delayを使うとスイッチの反応を邪魔してしまうので、このような書き方をしました。
どうかお助けください。。。

/***********************

回転

************************/
#include <Arduino.h>
#include <SPI.h>

/*************

設定の部 
*/
long max_speed = 7000;// 70000 固定

long int dd = 200;// 微増・微減の量 400
int dt = 10; // 微増・微減にかける時間　ミリ秒

int t_interval = 10;// (休止する時間 みり秒）
int t_hold = 170; //　最高速を維持する時間　ミリ秒）


int steps; 

/*
設定　終わり
**************
****/


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

#define PIN_L6470_SS PIN_SPI_SS

// 関数プロトタイプ
void L6470_write(unsigned char data);
void L6470_init(void);
void L6470_run(long speed);

/***********************
動作に関わる変数。
************************/

long speed = 1000; // スピード初期値
long int n_dir = 0 ;// カウントアップ　奇数で正回転　偶数で逆回転
long offTime;


int c_step = max_speed / dd;

const int limit_sw = 7;
bool switchState = false;
int counter = 0;
int milsec = 0;
bool speedSwitch = false;

void setup()
{
 // ピン設定。
 pinMode(PIN_SPI_MOSI, OUTPUT);
 pinMode(PIN_SPI_MISO, INPUT);
 pinMode(PIN_SPI_SCK, OUTPUT);
 pinMode(PIN_L6470_SS, OUTPUT);

 // りみとすいっち
 pinMode(limit_sw, INPUT_PULLUP );
 digitalWrite(PIN_L6470_SS, HIGH);
 // SPI開始。
 SPI.begin();
 
 // シリアル通信。
 Serial.begin(9600);
 
 // L6470の初期化。
 L6470_init();
 
 Serial.println("Start...");


}


void loop()
{
 


int value = digitalRead(limit_sw);



if(value == 0){
  counter ++;
  if(counter > 50){
    switchState = true;
    counter = 50;
  }
}
if (switchState == true){
  milsec++;
  Serial.println(milsec);
  if(milsec > 550){
    speedSwitch = !speedSwitch;
    milsec = 0;

  }

  int speed;
  if(speedSwitch == true){
    speed = 100;

  }

  else{
    speed = -100;
  
  }
 
  L6470_run( speed );

  
}
    
 

else{
  
   L6470_run( 400 );//速度

}

}


void L6470_write(unsigned char data)
{
 // ~SSイネーブル。
 digitalWrite(PIN_L6470_SS, LOW);
 // SPI転送。
 SPI.transfer(data);
 // ~SSディスエーブル。
 digitalWrite(PIN_L6470_SS, HIGH);
}

void L6470_init()
{
 // MAX_SPEED設定。
 /// レジスタアドレス。
 L6470_write(0x07);
 // 最大回転スピード値(10bit)　００２０

 L6470_write(0x00);
 L6470_write(0x90);
 
 // KVAL_HOLD設定。
 /// レジスタアドレス。
 L6470_write(0x09);
 // モータ停止中の電圧設定(8bit)
 L6470_write(0xFF);
 
 // KVAL_RUN設定。
 /// レジスタアドレス。
 L6470_write(0x0A);
 // モータ定速回転中の電圧設定(8bit)
 //L6470_write(0xFF);
 L6470_write(0x64);
 
 // KVAL_ACC設定。
 /// レジスタアドレス。
 L6470_write(0x0B);
 // モータ加速中の電圧設定(8bit)
 L6470_write(0xFF);
 
 // KVAL_DEC設定。
 /// レジスタアドレス。
 L6470_write(0x0C);
 // モータ減速中の電圧設定(8bit)
 L6470_write(0xFF);
 
 // OCD_TH設定。
 /// レジスタアドレス。
 L6470_write(0x13);
 // オーバーカレントスレッショルド設定(4bit)
 L6470_write(0x0F);
 
 // STALL_TH設定。
 /// レジスタアドレス。
 L6470_write(0x14);
 // ストール電流スレッショルド設定(4bit)
 L6470_write(0x7F);
}

void L6470_run(long speed)
{
 unsigned short dir;
 unsigned long spd;
 unsigned char spd_h;
 unsigned char spd_m;
 unsigned char spd_l;

//spd = speed;

 // 方向検出。
 
 if (speed < 0)
 {
  dir = 0x50;
  spd = -1 * speed;
 }
 else
 {
  dir = 0x51;
  spd = speed;
 }


 // 送信バイトデータ生成。
 spd_h = (unsigned char)((0x0F0000 & spd) >> 16);
 spd_m = (unsigned char)((0x00FF00 & spd) >> 8);
 spd_l = (unsigned char)(0x00FF & spd);
 
 // コマンド（レジスタアドレス）送信。
 L6470_write(dir);
 // データ送信。
 L6470_write(spd_h);
 L6470_write(spd_m);
 L6470_write(spd_l);
 
  Serial.print( " current spd = ");
  Serial.println( spd );
}

Comments

[y_waiwai]

いまいちよーわかりませんが、トルク保持休憩、ってモータを停止させるってことでしょうか

[fukufuku999]

早速ありがとうございます！ひとまず、停止させたいです。

Answer 1

状態マシンって考え方を知っておくとよいかと思います。

例えば、

//プログラム①
const int LED1=4;
const int LED2=5;
void setup(){
  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
}

int state=0;
//あるいは
//int state =1;
void loop(){
  if( state == 0 ){
    digitalWrite(LED1,HIGH);
  }else if(state == 1){
    digitalWrite(LED2,HIGH);
  }
}

というプログラム、なにが起こるかわかりますね? stateが0のときには、if文はstate==0が真のときの動きしかしませんから、

void loop(){
    digitalWrite(LED1,HIGH);
}

と同じこと。多分LED1が点灯するのでしょう。一方、state=1の方を生かせば

//int state=0;
//あるいは
int state =1;
void loop(){
    digitalWrite(LED2,HIGH);
}

と同じ。LED2が点灯。

では、ちょっと変えて、

//プログラム②
const int LED1=4;
const int LED2=5;
unsigned long t0;
void setup(){
  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
  t0=millis();
}

int state=0;
void loop(){
  unsigned long t=millis();
  if( state == 0 ){
    digitalWrite(LED1,HIGH);
    if(t-t0>500){
      digitalWrite(LED1,LOW);
      t0=t;
      state=1;
    }
  }else if(state == 1){
    digitalWrite(LED2,HIGH);
    if(t-t0>300){
      digitalWrite(LED2,LOW);
      t0=t;
      state=0;
    }
  }
}

としたらどうでしょう。なんだかプログラムが長くなった気もしますが、stateが0の間は

void loop(){
  unsigned long t=millis();
  digitalWrite(LED1,HIGH);
  if(t-t0>500){
    digitalWrite(LED1,LOW);
    t0=t;
    state=1;
  }
}

だけ考えれば良くて、LED1を点灯しておいて前回t0を更新した時間から500m秒経ったらLED1を消す。その時間をt0に記録して、stateを1にするという動作です。stateが1になったら今度は

void loop(){
  unsigned long t=millis();
  digitalWrite(LED2,HIGH);
  if(t-t0>300){
    digitalWrite(LED2,LOW);
    t0=t;
    state=0;
  }
}

LED2を点灯しておいて前回t0を更新した時間から300m秒経ったらLED2を消す。その時間をt0に記録して、stateを0にします。
結果として、プログラム②はLED1の点灯を500m秒、LED2の点灯を300m秒で交互に点灯するプログラムです。
もうちょっとひねって、以下はどうなるでしょう。

//プログラム③
const int LED1=4;
const int LED2=5;
const int SW1=8;
unsigned long t0;
void setup(){
  pinMode(LED1,OUTPUT);
  pinMode(LED2,OUTPUT);
  pinMode(SW1,INPUT_PULLUP);//追加
  t0=millis();
}

int state=0;
void loop(){
  unsigned long t=millis();
  if( state == 0 ){
    //およそプログラム②と同じ
    digitalWrite(LED1,HIGH);
    if(t-t0>500){
      digitalWrite(LED1,LOW);
      //t0=t;
      state=1;
    }
  }else if(state == 1){
    digitalWrite(LED2,HIGH);
    if(digitalRead(SW1)==LOW){//変えた
      digitalWrite(LED2,LOW);
      t0=t;
      state=0;
    }
  }
}

LED1が500m秒点灯して消灯/LED2点灯、LED2の点灯時にスイッチをON(スイッチはピンとGNDの間に入っているとして)にするとLED2が消灯してLED1が500m秒点灯する、ということになります。

このような構造を応用すると、

int state=0; //これを「状態変数」と呼ぶ
void loop(){
  if(state==0){//状態0だったら
    //処理0
    if(他の処理に遷移する条件){
      //この状態の後始末
      //次の状態への準備
      //次の状態へ、状態変数の変更
    }
  } else if(state==1){//状態1だったら
    //処理1
    if(他の処理に遷移する条件){
      //この状態の後始末
      //次の状態への準備
      //次の状態へ、状態変数の変更
    }
  } else if(state==2){//状態2だったら
    //処理2
    if(他の処理に遷移する条件){
      //この状態の後始末
      //次の状態への準備
      //次の状態へ、状態変数の変更
    }
  } else if...(状態をいくつも繋げられる)
}

という定形バターンで「次々と変わっていく状態」を記述できます。(switch文で書くことが多いのですが、知らない可能性を想定してif～else ifで書いています。知っているなら適宜置き換えてください)

今回であれば、

int state=0;
void loop(){
  if(state==0){
    正転
    if(リミットスイッチオン){
      時間取得
      state=1;
    }
  }else if(state==1){
    逆転
    if(550m秒経過){
      時間取得
      state=2;
    }
  }else if(state==2){
    ブレーキ
    if(100m秒経過){
      時間取得
      state=3;
    }
  }else if(state==3){
    正転
    if(550m秒経過){//もしかしたらif(550m秒経過 or リミットスイッチON)とした方がいいかも
      時間取得
      state=4;
    }
  }else if(state==4){
    ブレーキ
    if(100m秒経過){
      時間取得
      state=1;
    }
  }
}

とすれば、とりあえず目的は達成出来るのではないでしょうか。
ブレーキの2箇所はそのつもりになれば一つにまとめられるでしょうが、とにかく動かす、ということで放置しています。

Answer 2

ぐぐると出てくるページですが、

マイコンの実験：モータドライバ(L6470)の実験

L6470_Stop(mode)

　　モータを停止させる処理関数です。
Hizはモータを停止させた後、モータの出力をOFF(ハイインピーダンス)にします。
mode：停止モードを指定します
0 = SoftStop 減速しながらモータを停止させます、保持トルク有り。
1 = HardStop 即時にモータを停止させます、保持トルク有り。
2 = SoftHiz 減速しながらモータを停止させます、保持トルク無し。
3 = HardHiz 即時にモータを停止させます、保持トルク無し。

ってな記述がありますが。

Comments

[fukufuku999]

ありがとうございます。
これをどこに入れたらいいかわからりません。。