Arduino Mega 2560 LESSON:9 サーボ

Arduino Mega 2560に戻る。

概要

　サーボを使った課題です。

　接続図は以下のとおりです。

　完成図

　またPCB図も作っておきました。

　3DPreview図

　コードは以下のようなものです。

//www.elegoo.com
//2018.12.19
#include <Servo.h>
Servo myservo;

void setup(){
  myservo.attach(9);
  myservo.write(90);// move servos to center position -> 90°
} 
void loop(){
  myservo.write(90);// move servos to center position -> 90°
  delay(500);
  myservo.write(30);// move servos to center position -> 60°
  delay(500);
  myservo.write(90);// move servos to center position -> 90°
  delay(500);
  myservo.write(150);// move servos to center position -> 120°
  delay(500);
}

　サーボライブラリを使った角度の調整です。サーボの位置を瞬時に角度を変えていくという内容のコードですね。サーボの舵の部分を自分に向けて置いた場合の表現で以後、話を進めます。左とか右とか言う表現をしますが、自分に舵を向けた状態から見て左と右とか言ってるんだなぁって思って下さい。

　サーボは赤と黒にあたる線で電源を供給します。赤は+5Vに黒にあたる茶色の線は、GNDへ接続します。サーボの信号線である橙色の線では、パルス波を受け取り角度を調整しています。ライブラリを使わずにまずはパルス波を送ってみましょう。

// SG90サーボ制御
// Servoライブラリ不使用
// タイマー割り込み処理なし

const int servoPin = 9;  // サーボ信号線：D9

void setup() {
  pinMode(servoPin, OUTPUT);
  digitalWrite(servoPin, LOW);
}

void loop() {

  // サーボ信号をHIGHにする
  digitalWrite(servoPin, HIGH);

  // 1500マイクロ秒待つ（中央位置）
  delayMicroseconds(1500);

  // サーボ信号をLOWにする
  digitalWrite(servoPin, LOW);

  // 次のパルスまで待つ
  delayMicroseconds(18500);
}

　このように1500μsecのONと18500μsecのOffを繰り返すパルス波を送ると概ね真ん中の位置でキープしますが、少し不安定であるためサーボがピクピク震えます。1000μsecのONと19000μsecだと左に振れた状態をキープします

• 1500μsecのONと18500μsecのOff：真ん中
• 1000μsecのONと19000μsecのOff：左
• 2000μsecのONと18000μsecのOff：右

　以下のようにすると、左右にサーボがうごきます。左も右も45度くらいしか動かないので、完全に横に向いた状態にはならないですね。

// SG90サーボ制御
// Servoライブラリ不使用
// タイマー割り込み処理なし
// 左右振り

const int servoPin = 9;  // サーボ信号線：D9

void setup() {
  pinMode(servoPin, OUTPUT);
  digitalWrite(servoPin, LOW);
}

void loop() {

  for(int i = 1000; i < 2000; i++){
    // サーボ信号をHIGHにする
    digitalWrite(servoPin, HIGH);

    // 1500マイクロ秒待つ（左位置から右位置へ）
    delayMicroseconds(i);

    // サーボ信号をLOWにする
    digitalWrite(servoPin, LOW);

    // 次のパルスまで待つ
    delayMicroseconds(19000 - i);
  }
  for(int i = 2000; i > 1000; i--){
    // サーボ信号をHIGHにする
    digitalWrite(servoPin, HIGH);

    // 1500マイクロ秒待つ（右位置から左位置へ）
    delayMicroseconds(i);

    // サーボ信号をLOWにする
    digitalWrite(servoPin, LOW);

    // 次のパルスまで待つ
    delayMicroseconds(19000 - i);
  }

}

　サーボライブラリを使わなくても動かすこと自体はできますが、これでは途中で処理を入れる都度、パルス幅が変わって位置が不安定になっていきます。そこで割り込みという技術を使います。

　以下のようにライブラリを使って真ん中に設定するとものすごく安定しています。舵をさわってもほとんど振動を感じません。

#include <Servo.h>

Servo myservo;

void setup() {
  myservo.attach(9);
  myservo.writeMicroseconds(1500);
}

void loop() {
}

　上記のコードは非常に安定します。

void setup() {
  pinMode(servoPin, OUTPUT);
  digitalWrite(servoPin, LOW);
}

void loop() {
  noInterrupts();

  digitalWrite(9, HIGH);
  delayMicroseconds(1500);
  digitalWrite(9, LOW);

  interrupts();

  delayMicroseconds(18500);
}

　上記のようなコードにすると少し改善されます。舵を触るとかすかに振動を感じます。チャットGPTによるとこれは、

　Arduino Mega2560の標準環境では、Timer0のオーバーフロー割り込みが代表例です。Timer0は通常、約1.024msごとにオーバーフローし、millis() や micros() などの時間管理に使われます。とのことでしたので、delayMicrosecondsだけで制御してもTime0のオーバーフローによる割り込みが入っていたのが、noInterrupts();という命令によって割り込みが禁止された期間が作られたことによって少しだけ安定したのではないかという見立てです。なんだかよくわからないことを言われているように感じるでしょうけれど、とにかく小手先の割り込み禁止という手法をもってしてもサーボライブラリの安定性に負けているコードであったということです。

　では、どうすればいいのかという解決編に近づいていきたいと思います。

#include <avr/interrupt.h>

const uint8_t servoPin = 9;

// HIGH期間：1500µs（中央位置）
const uint16_t pulseWidthUs = 1500;

// 信号周期：20000µs（50Hz）
const uint16_t periodUs = 20000;

// 次の割り込みでHIGHにするか、LOWにするか
volatile bool nextHigh = true;

void setup() {
  pinMode(servoPin, OUTPUT);
  digitalWrite(servoPin, LOW);

  noInterrupts();

  // Timer5の設定を初期化
  TCCR5A = 0;
  TCCR5B = 0;
  TCNT5 = 0;

  // 16MHz / 8 = 2MHz
  // 1カウント = 0.5µs
  OCR5A = pulseWidthUs * 2 - 1;//(*(volatile uint16_t *)(0x128))= 2999 と同じ。
  //OCR5Aは0x128レジスタを示す名前定義

  // CTCモード（OCR5A一致でカウンタをクリア）
  TCCR5B |= (1 << WGM52);//WGM52 = 3　00000001→00001000 (*(volatile uint8_t *)(0x121)) = 8 と同じ
  // 00000000 OR  00001000 = 00001000 TCCR5Bは0x121のレジスタを示す名前定義です。

  // 比較一致A割り込みを有効化
  TIMSK5 |= (1 << OCIE5A);//OCIE5A = 1　00000001→00000010 (*(volatile uint8_t *)(0x73)) = 2 と同じ
  // 00000000 OR  00000010 = 00000010 TIMSK5は0x73のレジスタを示す名前定義です。

  // プリスケーラ8でタイマー開始
  TCCR5B |= (1 << CS51);//CS51 = 1 (*(volatile uint8_t *)(0x121)) = 10  と同じ
  // 00001000 OR  00000010 = 00001010 

  interrupts();
}

// Timer5比較一致A割り込み
ISR(TIMER5_COMPA_vect) { //ISR(__vector_47)

/*
void __vector_47(void)
    __attribute__((signal, used, externally_visible));

void __vector_47(void) {
    // 割り込み処理
}
属性	コンパイラへの指示
signal	AVRの割り込み処理関数としてコンパイルする
used	通常のコードから呼ばれていなくても、この関数を生成する
externally_visible	外部から参照される可能性がある関数として扱う

1.TCNT5 が0からカウントを開始する。
2.TCNT5 == OCR5A になる。
3.比較一致Aの割り込み要求が発生する。
4.割り込みが許可されていれば、CPUが __vector_47() を実行する。
5.CTCモードにより TCNT5 が0に戻り、再びカウントを始める。
*/

  if (nextHigh) {
    digitalWrite(servoPin, HIGH);

    // 次はHIGH期間の終了時に割り込み
    OCR5A = pulseWidthUs * 2 - 1;

    nextHigh = false;
  }
  else {
    digitalWrite(servoPin, LOW);

    // 次はLOW期間の終了時に割り込み
    OCR5A = (periodUs - pulseWidthUs) * 2 - 1;

    nextHigh = true;
  }
}

void loop() {
  // サーボ信号はTimer5割り込みが生成する
  // ここでは何もしない
}

■<avr/interrupt.h>

C:\Users\(UserID)\AppData\Local\Arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include\avr

Arduino Mega 2560 LESSON:9 サーボ interrupt.h

■<avr/io.h>

C:\Users\(UserID)\AppData\Local\Arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include\avr

Arduino Mega 2560 LESSON:9 サーボ io.h

■<avr/iom2560.h>

C:\Users\(UserID)\AppData\Local\Arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include\avr

Arduino Mega 2560 LESSON:9 サーボ iom2560.h

■<avr/iomxx0_1.h>

C:\Users\(UserID)\AppData\Local\Arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include\avr

Arduino Mega 2560 LESSON:9 サーボ iomxx0_1.h

　上記のプログラムのように待ち時間をクロックによってカウントするタイマーを用いて、そのタイマーが一致したら割り込み処理を発生させる方式にすると、安定してサーボの舵を変更できます。

　さらに発展させて

const uint8_t servoPin = 45;  // OC5B
const uint16_t pulseWidthUs = 1600;
const uint16_t periodUs = 20000;

void setup() {
  pinMode(servoPin, OUTPUT);

  noInterrupts();

  // Timer5を初期化
  TCCR5A = 0; //レジスタ0x120
  TCCR5B = 0; //レジスタ0x121
  TCNT5 = 0; //レジスタ0x124

  // 16MHz / 8 = 2MHz（1カウント = 0.5µs）
  // Fast PWMモード14：TOP = ICR5
  ICR5 = periodUs * 2 - 1;    // 39999 ICR5:レジスタ0x126

  // HIGH時間を1500µsに設定
  OCR5B = pulseWidthUs * 2;   // 3000 OCR5B:レジスタ0x12A

  // OC5Bを非反転PWM出力に設定
  TCCR5A |= (1 << COM5B1);//COM5B1=5 TCCR5A=00100000

  // Fast PWMモード14（WGM53:0 = 1110）
  TCCR5A |= (1 << WGM51); //WGM51=1 TCCR5A=00100010←00100000 Or 00000010
  TCCR5B |= (1 << WGM53) | (1 << WGM52); //WGM52=3, WGM53=4 TCCR5B=00011000←00000000 Or 00001000 Or 00010000

  // 分周比8でタイマー開始
  TCCR5B |= (1 << CS51); //CS51=1 TCCR5B=00011010←00011000 Or 00000010 0.5μsec

/*
Fast PWMモード14（WGM53:0 = 1110）
TOP = ICR5、OC5Bは非反転PWM出力

1. TCNT5 = 0 で新しいPWM周期が始まり、D45がHIGHになる。
2. TCNT5 = OCR5B（3000）で比較一致し、D45がLOWになる。
3. TCNT5 = ICR5（39999）までカウントする。
4. 次のタイマークロックでTCNT5が0に戻り、再びD45がHIGHになる。
5. 以後、同じ動作を繰り返す。

クロック：
CPUクロック = 16MHz
分周比 = 8（CS52:0 = 010）
Timer5クロック = 16MHz / 8 = 2MHz
1カウント = 0.5µs

PWM周期 = (39999 + 1) × 0.5µs = 20000µs
HIGH時間 = 3000 × 0.5µs = 1500µs
LOW時間 = 18500µs
*/
  interrupts();
}

void loop() {

  OCR5B = 1500 * 2;
  delay(5000);

  for(int i = 1500; i > 999; i--){
    OCR5B = i * 2;
    delay(1);
  }

  for(int i = 1000; i < 2001; i++){
    OCR5B = i * 2;
    delay(1);
  }

  for(int i = 2000; i > 999; i--){
    OCR5B = i * 2;
    delay(1);
  }

  for(int i = 1000; i < 2001; i++){
    OCR5B = i * 2;
    delay(1);
  }

  for(int i = 2000; i > 1499; i--){
    OCR5B = i * 2;
    delay(1);
  }


}

　更にピンの状態変更もハードウェアでやる方式もあります。

TIMSK5 |= (1 << TOIE5);  // Timer5オーバーフロー割り込みを許可

　を追加すれば、上記に以下の割り込み関数が動くようにすることもできます。

ISR(TIMER5_OVF_vect) { … }

　44ピン以外にも3つのピンが同じようなことができることに対応しています。Mega2560はね。

Arduinoピン 出力比較チャネル    比較レジスタ 
D46         OC5A                OCR5A
D45         OC5B                OCR5B
D44         OC5C                OCR5C

　Arduino IDEで.inoファイルで記述した場合はArduino.hが読み込まれるのでinterrupt.hのヘッダの読み込みは省略できますが、Cppファイルに記述した場合は省略することはできません。

　サーボの勉強をするというこのレッスンでは割り込みというWindowsプログラミングでも滅多に登場しないコードが登場していることに気を付けたいところですね。

Mega2560はTimer5ではD44,45,46他にもTimerが3種類あって3ピンづつあって全部で12種類のピンで同じことができるようになっています。

タイマー    Arduino Mega2560の出力ピン
Timer1      D11、D12、D13
Timer3      D2、D3、D5
Timer4      D6、D7、D8
Timer5      D44、D45、D46

タイマー自体は他にも2基あり全部で6基体制です。使いたい場合はさらに調べるなりGPTに聞くなりしてみて下さい。

　

Arduino Mega 2560に戻る。