Embedded Files
TinkerCADで改造したモデルがこれです。モータ用電源に９V電池を用意しました。L293Dの８番ピン（モータへの電源）につなぎます。モータの回し方はINPUTの２ピンを使って回転方向を指定し、ENABLEピンでPWMの大きさ（速度）を決めています。モータのギア比、エンコーダの窓の数が分からないのでそこは適当です。
ArduinoUnoの割込みはピン２およびピン３に割り付けられているので、エンコーダのチャネルA、BをそれぞれArduinoのピン２，３へ接続します。
割込みの設定は attachInterrupt(interrupt, function, mode) で行います。attachInterrupt(0, doEncoderA, CHANGE); は０番の割込み（第３引数のCHANGEによる指定により、このピンへの状態変化で割込みがかかる。）がかかるとdoEncoderAという関数を起動します。Arduino Due で高分解能なエンコーダを読み取ってみた の方向の判断 の説明にあるようにチャネルA、Bそれぞれの信号が変化したときにdoEncoderA、doEncoderBを起動し、そのときのチャネルの信号が等しいとき（HIGH・HIGH、あるいはLOW・LOWのとき）チェネルAの割込みの場合には
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?1:-1;
この式で、1:-1のところで1を選択するので、カウントアップされ、等しくないときには-1を選択しカウントダウンされます。
チャネルBも同様に行うことで、エンコーダが回転すると信号の変化時にカウントし、回転数を計測することが出来ます。
// motor control pin
const int motorDirPin1 = 7; // Input 1
const int motorDirPin2 = 8; // Input 2
const int motorPWMPin = 9; // Enable(PWM)
// encoder pin
const int encoderPinA = 2;
const int encoderPinB = 3;
int encoderPos = 0;
// encoder value change motor turn angles
const float ratio = 360./188.611/34.;
// 360. -> 1 turn
// 188.611 -> Gear Ratio
// 48. -> Encoder: Countable Events Per Revolution (Motor Shaft)
// P control
float Kp = 30; //位置の比例ゲイン
float targetDeg = 360; //目標値を360度とする。
void doEncoderA()
{
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?1:-1;
}
void doEncoderB()
{
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?-1:1;
}
void doMotor(bool dir, int vel)
{
if(dir){
digitalWrite(motorDirPin1, LOW);
digitalWrite(motorDirPin2, HIGH);
}else{
digitalWrite(motorDirPin1, HIGH);
digitalWrite(motorDirPin2, LOW);
}
analogWrite(motorPWMPin, 200);
// analogWrite(motorPWMPin, dir?(255 - vel):vel);
}
void setup()
{
Serial.begin(9600);
pinMode(encoderPinA, INPUT_PULLUP);
attachInterrupt(0, doEncoderA, CHANGE);
pinMode(encoderPinB, INPUT_PULLUP);
attachInterrupt(1, doEncoderB, CHANGE);
pinMode(motorDirPin1, OUTPUT);
pinMode(motorDirPin2, OUTPUT);
}
void loop()
{
float motorDeg = float(encoderPos)*ratio;
float error = targetDeg - motorDeg;
float control = Kp*error;
//digitalWrite(EnablePin, 255);
doMotor((control>=0)?HIGH:LOW, min(abs(control), 255));
Serial.print("encoderPos : ");
Serial.print(encoderPos);
Serial.print(" motorDeg : ");
Serial.print(float(encoderPos)*ratio);
Serial.print(" error : ");
Serial.print(error);
Serial.print(" control : ");
Serial.print(control);
Serial.print(" motorVel : ");
Serial.println(min(abs(control), 255));
}
エンコーダのケーブル並びは次のようになっています。（TinkerCADでは２種類のエンコーダ付モータがあるが、そのうちの１つ。もう１つは並びが違っています。
モータ正、負は負にGNDをつなぎ、正にモータに与えるPWM信号を入れます。エンコーダ（接地）、（電源）は（接地）をGNDにつなぎ、（電源）にArduinoの5Vをつなぎます。チャネルA、Bはエンコーダのパルスが出るのでこれをカウントして回転数を求めます。
エンコーダ付きモータを購入しました。このモータはTinkerCADのエンコーダ付きモータ（2種）の配線とは並びが違うようです。そこで適当に動かして下記の並び（エンコーダのGND、電源とチャネルA、Bは逆かもしれません）が分かりました。このエンコーダを使われるときには参考にしてください。
赤：モータ電源
黒：モータGND
緑：エンコーダGND
青：エンコーダ電源
黄：チャネルA
白：チャネルB
エンコーダ付モータを2つ以上制御したい場合には割り込みが足りません。通常はピン2と3しか割り込みに使えないからです。ですが、そこは考えている人がいて PinChangeInterrupt というライブラリがあります。このライブラリは通常の割り込みピン以外のデジタルピンを割り込み対応に出来るというすぐれものです。これを使うにはArduino IDEで次のようにします。
Arduino IDEのツール→ライブラリを管理でPinChangeInterruptを検索
するといくつか出てくるがPinChangeInterruptを探し出してそれをインストールする
これで使えるようになります。
次のように2つのエンコーダ付モータを接続し、その下のプログラムで動くと思います。（まだ実装していません。） 残念ながらTinkerCADではライブラリを自由に組み込めないので下記のモデルは実行できません。
割り込みでは引数を使えないので、下のプログラムでは関数ポインタを使ってLoop処理が出来るようにしてみました。関数ポインタを配列に入れることで、Loop処理毎に別の関数を呼び出せるようにしています。
#include "PinChangeInterrupt.h"
const int maxMotorNum = 2; //max number of motors
// encoder pin
const int encoderPinA[] = {2, 4}; //interrupt pins for encoder phase A
const int encoderPinB[] = {3, 5}; //interrupt pins for encoder phase B
// motor control pin
const int motorDirPin1[] = {7, 12}; // Input 1
const int motorDirPin2[] = {8, 11}; // Input 2
const int motorPWMPin[] = {9, 10}; // Enable(PWM)
int encoderPos[] = {0, 0}; //encoder counts
// function pointers
void (*funcA[maxMotorNum])() = {&enc1A, &enc2A}; //interrupt functions for phase A
void (*funcB[maxMotorNum])() = {&enc1B, &enc2B}; //interrupt functions for phase B
// encoder value change motor turn angles
const float ratio = 360. / 188.611 / 34.;
// 360. -> 1 turn
// 188.611 -> Gear Ratio
// 48. -> Encoder: Countable Events Per Revolution (Motor Shaft)
// P control
float Kp[maxMotorNum] = {30, 30}; //位置の比例ゲイン
float targetDeg[][maxMotorNum] = {{0, 0}, //目標値を360度とする。
{40, 0},
{40, 40},
{40, 0},
{ -40, 0},
{ -40, -40},
{ -40, 0}
};
const int minStep = 1; //minimum pwm step for inching
int start = false; //demo start: true
int demoLoop = 0;
const int maxDemoLoop = 7;
float inpositionError = 0.5;
//encoder for motor1
void enc1A() { //for phase A
doEncoderA(0);
}
void enc1B() { //for phase B
doEncoderB(0);
}
//encoder for motor2
void enc2A() { //for phase A
doEncoderA(1);
}
void enc2B() { //for phase B
doEncoderB(1);
}
//position calculation
void doEncoderA(int num)
{
encoderPos[num] += (digitalRead(encoderPinA[ num ]) == digitalRead(encoderPinB[ num ])) ? 1 : -1;
}
void doEncoderB(int num)
{
encoderPos[ num ] += (digitalRead(encoderPinA[ num ]) == digitalRead(encoderPinB[ num ])) ? -1 : 1;
}
//move a motor
void doMotor(int num, bool dir, int vel)
{
if (dir) {
digitalWrite(motorDirPin1[num], LOW);
digitalWrite(motorDirPin2[num], HIGH);
} else {
digitalWrite(motorDirPin1[num], HIGH);
digitalWrite(motorDirPin2[num], LOW);
}
analogWrite(motorPWMPin[num], vel);
// analogWrite(motorPWMPin, dir?(255 - vel):vel);
}
void setup()
{
Serial.begin(9600);
for (int i = 0; i < maxMotorNum ; i++) {
pinMode(encoderPinA[i], INPUT_PULLUP);
pinMode(encoderPinB[i], INPUT_PULLUP);
pinMode(motorDirPin1[i], OUTPUT);
pinMode(motorDirPin2[i], OUTPUT);
attachPCINT(digitalPinToPCINT(encoderPinA[i]), funcA[i], CHANGE); //assign function to an encoder phase A interrupt
attachPCINT(digitalPinToPCINT(encoderPinB[i]), funcB[i], CHANGE); //assign function to an encoder phase B interrupt
}
// attachInterrupt(0, funcA[0], CHANGE);
// attachInterrupt(1, funcB[0], CHANGE);
// attachPCINT(digitalPinToPCINT(encoderPinA[1]), funcA[1], CHANGE);
// attachPCINT(digitalPinToPCINT(encoderPinB[1]), funcB[1], CHANGE);
}
void loop()
{
if (Serial.available() > 0) {
// read incoming serial data:
char inChar = Serial.read();
if (inChar == "f") { //joint1 +
doMotor(0, HIGH, minStep);
}
if (inChar == "v") { //joint1 -
doMotor(0, LOW, minStep);
}
if (inChar == "j") { //joint2 +
doMotor(1, HIGH, minStep);
}
if (inChar == "m") { //joint2 -
doMotor(1, LOW, minStep);
}
if (inChar == "s") { //demo start
start = true;
}
if (inChar == "t") { //jdemo stop
start = false;
}
Serial.print("*** ");
Serial.println(inChar);
}
if (start) {
float motorDeg[maxMotorNum] = {float(encoderPos[0]) * ratio, float(encoderPos[1]) * ratio};
float error[maxMotorNum] = {targetDeg[demoLoop][0] - motorDeg[0], targetDeg[demoLoop][1] - motorDeg[1]};
float control[maxMotorNum] = {Kp[0] * error[0], Kp[1] * error[1]};
for (int i = 0; i < maxMotorNum; i++) {
if(abs(error[i]) < inpositionError) control[i] = 0;
doMotor(i, (control[i] >= 0) ? HIGH : LOW, min(abs(control[i]), 255));
}
for (int i = 0; i < maxMotorNum; i++) {
Serial.print("J");
Serial.print(i);
Serial.print("= ");
Serial.print("encoderPos : ");
Serial.print(encoderPos[i]);
Serial.print(" motorDeg : ");
Serial.println(float(encoderPos[i])*ratio);
}
Serial.println("");
int inposition = 0;
for (int i = 0; i < maxMotorNum; i++) {
if(abs(error[i]) < inpositionError) inposition++;
}
if(inposition >= 2){
if(++demoLoop > maxDemoLoop) demoLoop = 0;
}
}
}
エンコーダ付モータ参考URL
Page updated
Google Sites
Report abuse