第三章 モーターを動かす


モーターをデジタルで動かす


モーターをどのように回しているか
使っているモーターは直流モーターです。
乾電池を一個つなげれば、回転します。二個直列につなげれば高速回転します。三個直列につなげると、さらに高速回転しますが、変な臭いがし始めて壊れるかもしれません。
　今度は乾電池一個よりも遅く回転させるにはどうしたら良いでしょうか?
例えば半分のスピードにするために半分の電圧にすることを考えます。
電圧を下げるために抵抗を直列につなぐと考える人もいるでしょう。使用条件に依存しますが、0.5A流れるとして半分の電圧がモーターにかかるとすると、
2R = 1.5(V)/0.5(A) = 3、 従ってR=1.5Ω、抵抗で消費される電力 P=IV = 0.5*0.75=0.375、従って1.5Ωで1/2Wの抵抗が必要になります。
1/4のスピードにするには4R=3になり、R=0.75Ωとなります。
では、連続的にスピードを変えるにはどうするかというと、消費電力が大きく、抵抗値の小さい可変抵抗が必要になります。そんなの売っていません。さらにこれをコンピュータでコントロールするには？　と考えるとだんだんと複雑になります。従って、ここでこの延長を考えるのをやめます。

モーターを乾電池１個つなぐ場合を考えます。
  1. １０秒間だけモーターに電流を流します。それなりに回ります。
  2. 次に５秒間だけモーターに電流を流し、後の５秒は流しません。それなりに回り、止まります。
  3. １秒電流を流し、１秒電流を止めるを繰り返し、１０秒間続ける。回ったり止まったりする。
  4. 1/10秒電流を流し、1/10秒電流を止めるを繰り返し、１０秒間続ける。動いたり止まったりする暇が無いので、以前より遅く回る。
  5. 30/100秒電流を流し、70/100秒電流を止めるを、繰り返すとさらに遅く回る。
  6. 10/100秒電流を流し、90/100秒電流を止めるを、繰り返すとさらに遅く回る。
  7. 0秒電流を流し、1秒電流を止めるを繰り返すとモーターは止まる。
つまり、電源電圧は同じでも、電流を流す時間でスピードをコントロールすることができることが分かります。これがパルス幅変調(PWM)でモーターのスピードを制御する原理です。
実際には、逆回転ができないと意味が無いので、CPUボード内蔵のTB6552というICはこれができるHブリッジ回路を採用しています。しかし、パワーが足りないのでさらに外部にTA7291Pを用いてモーターを回します。


ヘッダーファイルとライブラリ
これ以後で使うヘッダーファイルとライブラリをダウンロードして用います。

ダウンロード

注意：まだ完成していませんのでバグがあっても不思議はありません。

パルス幅変調を用いてスピードをコントロールする

MCPWMクラスは二つのモーターをパルス幅変調で制御します。モーターはそれぞれ、Ach(左のモーターにする予定)とBch(右のモーターにする予定)と名付けられています。
まだ、モーターに接続しません。
下のプログラムを動かすと、写真の位置のピンからPWM波形がでるので、
オシロスコープで確認しましょう。
ここでは、setBackwardメソッドを使っていますが、setForwardメソッドも
使ってみましょう。この時は、もう一つの方のピンから波形が出ます。



PWMのテスト
void main(void)
{
  startInit("**** MCPWM test ****\n");
  MCPWM pwm;
  pwm.stop(pwm.Ach);
  pwm.setBackward(pwm.Bch);
  pwm.setBackward(pwm.Ach);
  while(true) {
    for(UINT i=0x1000; i<0xf000; i++) {
      for(int j=0; j<100; j++);
        pwm.setHighTime(pwm.Ach, i);
        pwm.setHighTime(pwm.Bch, i);
    }
    for(UINT i=0xf000; i>0x1000; i--) 
    {
      for(int j=0; j<100; j++);
        pwm.setHighTime(pwm.Ach, i);
        pwm.setHighTime(pwm.Bch, i);
    }
  }
}

 1 
 2 
 3 Motor Control Pulse Width Modulation
 4 モーターをコントロールするMCPWMオブジェクトを作る。
 5 念のためAchの出力を止めておく。(意味が無いかもしれない)
 6 Bchを逆回転に設定する。
 7 Achを逆回転に設定する。
 8 永久ループ
 9   AchのHightimeを変更する。
10   BchのHightimeを変更する。
11 
12 
13 
14 上記for文の逆動作をする
15
16
17
18
19
20
21


重要なMCPWMクラスのメンバー
フィールド
  const static int Ach=0;
  const static int Bch=1;
  const static char Forward=1;
  const static char Backward=2;
コンストラクタ
  MCPWM();
メソッド
  void start(int pin);               PWM発生開始
  void stop( unsigned int pin);      PWM停止
  void Stop();                       両チャンネルとも停止
  void setForward(unsigned int pin); 前進設定
  void setForward();                 両チャンネルとも前進
  void reStart();                    Stop()した場合の再開
ハードウェア
　Port6, bit0　Ach  
  Port6, bit1  Bch        の2チャンネルをサポートしている

ひたすら直進する

モータードライバー回路及びモーターを繋いで動かします。
うまくいったら逆方向もやってみましょう

左右の値は、モーターの特性、タイヤの微妙な直径で異なります。

ひたすら直進する
void main()
{
  startInit("\n****** 直進テスト *********\n");
  MCPWM robo;
  robo.setForward();
  robo.setHighTime(robo.Ach, 0x4000); // 左
  robo.setHighTime(robo.Bch, 0x4000); // 右
  while(true);
}


上記プログラムを実行した時の信号をオシロスコープで観察しました。
１メモリ4msです。
0x4000=16384μs=16.384ms
つまり、highの時間が16.384msです。

その場で回転させる

左右のモーターのスピードを逆にするとその場で回転します(setForward()とsetBackward())。ここでは、90度、つまり直角に回転するようにプログラムしてみましょう。
残念ながら簡単すぎるので、サンプルプログラムは提示しません。自分で考えて下さい。
ヒント：Timerクラスオブジェクトを用いて直角に移動する時間を設定する。

マインドストームNXTのモーターを動かす

マインドストームNXTの入出力ポートの配線は以下のようになっています。

割り込みを使用しない方法でやってみる
配線：CN10-11(P63)、CN10-12(P62)をモータードライバTA7291Pに入力する。
　　　モータードライバの出力はNXTのモーターにつなぐ
　　　NXTのモーターのエンコーダー1をCN11-1(P86)につなぐ
　　　NXTのモーターのエンコーダー2をCN11-5(P85)につなぐ
　　　NXTのモーターへの電源は4.3Vなのでそれらしい電源をつなぐ
　　　グラウンドをつなぐ

マインドストームNXTのモーターを動かす
#include "common.h"
#include "primary.h"
#include "NXTMotor.h"
#include "LegoMotor.h"
#include "interruptClass.h"
extern int wkCount, wkCount2, wkThermo, wkcnt[];
extern BYTE intKind;

BYTE getSW();        //スイッチ読み込み
void startInit(char* str);

void main()
{
  startInit("*** LegoMotor test ***\n");
  CPU.setInput(8); 
  int target=720;
  LegoMotor nxt; 
  int speed=0x4000;
  nxt.LegoMotorSpeed(speed);
  while(true)
  {
    nxt.move(target);
    nxt.move(0);
  }
}
void startInit(char* str)
{
  init();
  LED(1);  // 緑
  while(!getSW() );
  LED(0);
  printf(str);
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15
16 720は1回転
17 LegoMotorのモーターオブジェクトを作る、エンコーダはゼロ位置
18 
19 動作モータースピードを設定する(正逆同じ、絶対値)
20 永久ループ
21 
22 エンコーダが720まで正回転で移動する。
23 エンコーダが0まで逆回転で移動する
24 
25


LegoMotor.h
#ifndef _LEGOMOTOR
#define _LEGOMOTOR
#include "MCPWM.h"
#include "interruptClass.h"

class LegoMotor : public MCPWM, public interruptClass
{
private:
  long speed;
  int  counter;
  int  state;
public:
  static int levelPort, levelBit;
  LegoMotor();
  LegoMotor(int port, int bit, int intr);
  void resetMove();
  void move(int target);
  int getState();
  void setSpeed(long speed);
};

#endif


 1 ヘッダーの多重再読み込み禁止
 2 
 3 
 4 
 5 
 6 MCPWMとinterruptClassから派生させる
 7 
 8 
 9 スピードの絶対値を記憶する
10 エンコーダーの値
11 エンコーダーの状態
12 
13 チェックするレベルのポートとビット番号
14 コンストラクタ
15 初期設定
16 エンコーダーの値をリセットする
17 モーターをtargetまで移動させる
18 
19 モーターのスピードを設定する
20


LegoMotor.cppはライブラリに入っていますから、参考程度です。
LegoMotor.cpp
#include "LegoMotor.h"
#include "interruptClass.h"
#include "Port.h"
#include "cpu.h"
#include "Timer.h"

extern cpu CPU;
extern int wkcnt[];

int LegoMotor::levelPort, LegoMotor::levelBit;

LegoMotor::LegoMotor()
{
  counter=0;
}

void LegoMotor::resetMove(){
  counter = 0;    //エンコーダー
}
void LegoMotor::move(int target)
{
  Timer tm;
  int med, slow;
  int length;
  length=target - counter;
  if(length>0) {
    LegoMotorSpeed(speed);
    med=length-50;
    slow=speed/20*15;
    while(target > counter)
    {
      getState();
      if(med<counter) LegoMotorSpeed(slow);
    }
  }
  else {
    med= -length+50;
    slow=-speed/20*15;  
    LegoMotorSpeed(-speed);
    while(target < counter)
    {
      getState();
      if(med>counter) LegoMotorSpeed(slow);
    }
  }
  LegoMotorSpeed(0);
  int oldCounter=counter;
  tm.mark();
  do {
    getState();
    if(tm.timeout(100)){
      if(oldCounter==counter) break;
      else {
        oldCounter=counter;
        tm.mark();
      }
    }
  } while(true);
}

int LegoMotor::getState()
{
    int data = (CPU.readPort(CPU.port8)>>6)&3;
    switch(state)
    {
      case 0:
        if(data==1) counter--;
        else if(data==2) counter++;
        break;
      case 1:
        if(data==0) counter++;
        else if(data==3) counter--;
        break;
      case 2:
        if(data==0) counter--;
        else if(data==3) counter++;
        break;
      case 3:
        if(data==1) counter++;
        else if(data==2) counter--;
        break;
    }
    state = data;
    return state;
}
void LegoMotor::setSpeed(long speed)
{
  this->speed = speed;
}

 1 
 2 割り込み処理クラス
 3 ポートクラス
 4 ＣＰＵクラス
 5 
 6 CPUクラスの実体は外にある
 7 wkcntの実体は外にある
 8 
 9 クラス変数
10 
11 
12
13 デフォールトのポートとビット。立ち上がり割り込み
14 wkp3割り込みを用いる。
15 
16
17
18
19 割り込みの設定、割り込みエッジ指定
20 回転方向測定のオブジェクトを作る
21 回転方向測定のポートの記憶
22 回転方向測定のビットの記憶
23 回転方向測定のビットを入力に設定する
24 割り込みを可能にする
25
26
27
28 
29 
30 現在の位置よりもcountが大きい場合に、
31 正回転でモーターを動かす
32 
33 エンコーダーがcount未満の時モーターを動かす
34 
35
36 
37 
38 
39 逆回転でモーターを動かす
40 エンコーダーがcountを超えている時モーターを動かす
41
42
43
43
44 
45 設定位置に到達したのでモーター停止
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79 
80 
81 
82 
83 
84 
85 
86 
87 オブジェクトのspeed変数にセットする
88


割り込みを使用する方法でやってみる
配線：CN10-11(P63)、CN10-12(P62)をモータードライバTA7291Pに入力する。
　　　モータードライバの出力はNXTのモーターにつなぐ
　　　NXTのモーターのエンコーダー1をCN11-1(P86)につなぐ
　　　NXTのモーターのエンコーダー2をCN11-5(P85)につなぐ
　　　NXTのモーターへの電源は4.3Vなのでそれらしい電源をつなぐ
　　　グラウンドをつなぐ
までは割り込みなしと同じです。
追加：NXTのモーターのエンコーダー1をCN11-1(P86)に繋がっている線をさらにCN10-18(P52)に繋ぐ
　　　NXTのモーターのエンコーダー2をCN11-5(P85)に繋がっている線をさらにCN10-17(P53)に繋ぐ
　　　この二つの線が割り込んで、その時のレベルをチェックすることで回転を方向などを判断します。
割り込みベクターを記述しているファイルintprg.cppを取り替えます。
ダウンロード

NXTのモーターを割り込みで動かす
#include "common.h"
#include "primary.h"
#include "NXTMotor.h"
#include "LegoMotor.h"
#include "interruptClass.h"
extern int wkCount, wkCount2, wkThermo, wkcnt[];
extern BYTE intKind;

BYTE getSW();        //スイッチ読み込み
void startInit(char* str);

void main()
{
  startInit("*** NXTMotor test ***\n");
  NXTMotor nxt;
  int speed=0x4000;
  nxt.LegoMotorSpeed(speed);
  int i=0;
  while(true)
  {
    nxt.setSpeed(0x4000);
    nxt.move(360);
    printf("%d %5d\n", i, wkcnt[2]);
    nxt.move(0);
    printf("%d %5d\n", i,wkcnt[2]);
    i++;
  }
}

void startInit(char* str)
{
  init();
  LED(1);  // 緑
  while(!getSW() );
  LED(0);
  printf(str);
}

 1 
 2 
 3 
 4 
 5 
 6 ここでは使わない
 7 ここでは使わない
 8 
 9 
10 
11 
12 
13 
14 
15 NXTMotorのモーターオブジェクトを作る、エンコーダはゼロ位置
16 
17 動作モータースピードを設定する(正逆同じ、絶対値)
18 
19 永久ループ
20 
21 
22 エンコーダが360まで正回転で移動する。
23 
24 エンコーダが0まで逆回転で移動する
25 
26
27
28 
29 
30 
31 
32 
33 
34 
35
36 
37




NXTMotor.h
#ifndef _NXTMOTOR
#define _NXTMOTOR
#include "MCPWM.h"
#include "interruptClass.h"

class NXTMotor : public MCPWM, public interruptClass
{
private:
  long speed; 
public:
  static int levelPort, levelBit0, levelBit1;
  NXTMotor();
  NXTMotor(int port, int bit, int intr);
  void init(int port, int bit0, int bit1,
     int intr0, int edge0, int intr1, int edge1);
  void resetMove();
  void move(int count);
  void setSpeed(long speed);
};

#endif

 1 ヘッダーの多重再読み込み禁止
 2 
 3 
 4 
 5 
 6 MCPWMとinterruptClassから派生させる
 7 
 8 
 9 スピードの絶対値を記憶する
10 
11 割り込み設定値
12 コンストラクタ
13 コンストラクタ
14 
15 初期設定
16 エンコーダーの値をリセットする
17 モーターをcountまで移動させる
18 モーターのスピードを設定する
19 
20


重要なNXTMotorクラスのメンバー
フィールド
  long speed;
  int levelPort, levelBit0, levelBit1;
コンストラクタ
  NXTMotor();
  NXTMotor(int port, int bit, int intr);
メソッド
  void init(int port, int bit0, int bit1,
     int intr0, int edge0, int intr1, int edge1);
  void resetMove();
  void move(int count);
  void setSpeed(long speed);

第四章に行く　　　　ホームに戻る