はじめに
組み込んだ機能をひとつづづスケッチを入力しながら動作確認をして行きます。
ロボットに組み込んだ機能は、
音を鳴らす
距離を測る
サーボモーターを動かす
LEDを光らせる
ジャイロセンサーを読み取る
等があります。
ひとつひとつスケッチを使って確認して行きます。
１．音を鳴らす。
・スケッチを読み込む
Arduino IDEを起動し、
ファイル→スケッチ例→02.Digital→ToneMelody
を開きます。
ファイル→名前を付けて保存を選択し、ToneMelodyPin15としてスケッチを保存します。
・プログラムを修正する
元のプログラムは、Pin8にスピーカーを接続することを想定していますが、このロボットは圧電スピーカーをPin15に接続しているので、ピンの番号を修正します。
Linｅ38、44付近の tone() と noTone() の最初の引数の 8を15に変更します。
・動作を確認する
コンパイルして起動すると、起動時に一回有名なフレーズを演奏します。
２. 距離を測る
・スケッチを読み込む
Arduino IDEを起動し、
ファイル→スケッチ例→06.Sensors→Ping
を開きます。
ファイル→名前を付けて保存を選択し、PingPin16 としてスケッチを保存します。
・プログラムを修正する
元のプログラムは、Pin7に超音波距離センサーを接続することを想定していますが、このロボットはPin16に接続しているので、ピンの番号を修正します。
Linｅ25付近の const int pingPin = 7; を16に変更します。
・動作を確認する
このプログラムはセンサーから読み取った数値をコンソールに出力するので、シリアルモニタを開いて確認します。
シリアルモニタは、ツールバーの虫メガネマークを押すと起動します。
または、ツール→シリアルモニタ で起動します。
シリアルモニタにセンサが読み取った数値が表示されれば成功です。単位はｃｍです。
文字が正しく表示されない場合は、シリアルモニタの速度設定があっていないのかもしれません。速度が 9600 bps になっていることを確認して下さい。
３．サーボモーターを動かす
・スケッチを読み込む
Arduino IDEを起動し、
ファイル→スケッチ例→Servo→Sweep
を開きます。
ファイル→名前を付けて保存を選択し、SweepPin9 としてスケッチを保存します。
・プログラムを修正する
これはたまたま首のサーボモーターと同じ番号なので変更なしでそのまま使えます。
・動作を確認する
起動すると首を左右９０度に振り続けます。
このままでは電池を使って行き、モーターも消耗するので動作確認後は適当なスケッチに入れ替えて下さい。
これを応用して、８つのサーボモータを制御することで、二足歩行させることができるようにします。
指定するピンとサーボモーターとの関係は、
Ｐｉｎ2 右脛
Ｐｉｎ3 右もも
Ｐｉｎ4 左脛
Ｐｉｎ５ 左もも
Ｐｉｎ６ 腰
Ｐｉｎ７ 右肩
Ｐｉｎ８ 左肩
Ｐｉｎ９ 首
となっています。
違う場所のサーボモーターを動かす場合は、Linｅ18付近の attach() の引数を9から変更します。
8個のサーボモーターを同時に動かすには、配列を使用して、 複数 attach（）し複数指示を出せば良いのです。
Servo myservo[8]; // 配列でサーボを８個定義する。
（中略）
for(int i=0; i <8; i++)
{
myservo[i].ａｔｔａｃｈ（2+i）； // Pin2から９をサーボモーターの制御に割り当てる
}
（中略）
for(pos = 0; pos <=180; pos +=1) {
for(int i=0; i <8; i++)
{
myservo[i].write(81+pos/10); // サーボがロックしないように90度中心で角度を1/10にした。
delay(15);
}
}
（中略）
for(pos = 180; pos >=0; pos -=1) {
for(int i=0; i <8; i++)
{
myservo[i].write(81+pos/10); // サーボがロックしないように90度中心で角度を1/10にした。
delay(15);
}
}
これで、８個同時に１８度の範囲で同じように動くはずです。
（！）実機で確認する場合は、転ぶかもしれないので最初は機体を寝かせて確認して下さい。
ちょっと中休み
ここまでの知識があれば、プログラムを少し変えるだけでロボットを動かすことができるようになります。
このロボットの脚は平行リンク機構なので、ふくらはぎとももの左右のサーボモータを同じ角度に設定すれば、スクワットを行います。
重心のバランスが取れていれば、ジャイロは使わなくても倒れません。
（おもちゃの人形をバランスを取って立たせたのと同じです。）
Arduino IDEには、他にも沢山のサンプルがあるので、試して勉強してみて下さい。
これから
ここまでは、Arduino IDEに最初から組み込まれているスケッチを修正して確認して来ましたが、ここからは外部ライブラリをインストールして使う必要があるので少し難しくなります。
ライブラリと言うのは、作ったソフトウェアを他の人が利用しやすいように、ソフトウェアの部品としてまとめたものです。
例えばサーボ―モータで使った Servo クラスは標準に取り込まれたライブラリで、
#include
を利用する前に付ける事によって、ソースコード上でライブライが呼び出して使えるようになっています。
外部ライブラリと言うのは、まだ標準には取り込まれていないライブラリで、自分で個別にダウンロードしてインストールすることで利用できるようになるライブラリのことです。
外部のライブラリは、Windows10では、ドキュメントフォルダのArduinoフォルダのLibrariesに格納します。
具体的には以下のフォルダになります。
C:\Users\XYZ\Documents\Arduino\libraries
（！XYZはユーザー名です。）
（ライブラリのインストールについては、英語が得意な人は本家を参照してください：http://www.arduino.cc/en/Guide/Libraries）
外部ライブラリのアンインストールは、格納されたフォルダに作成されたファイルを削除し、Auduino IDEを再起動するとアンインストールされています。
４．フルカラーLEDをつける
・ライブラリをダウンロードする
フルカラーLEDは、外部ライブラリのAdafruit_NeoPixelライブラリを使用します。
Adafruit_NeoPixelライブラリは、Arduino IDEを起動しメニューバーから
スケッチ→ライブラリをインクルード→ライブラリを管理
で表示されるライブラリマネージャの検索フィルタに NeoPixel と入力して検索し、一覧表示された中から Adafruit_NeoPixel の行を押し インストール ボタンが表示されたら、それを押します。
バージョンが表示され、 INSTALLED と表示されていればインストール終了です。
インストールされたライブラリは、ライブラリフォルダを見れば作成されてインストールされています。
・スケッチを読み込む
Arduino IDEを起動し、ファイル→スケッチ例→Adafruit_NeoPixel→simple を開きます。
ファイル→名前を付けて保存を選択し、neopixelSimplePin14 としてスケッチを保存します。
・プログラムを修正する
他の修正と同様に、ピン番号がロボットに合うようにプログラムを修正します。元のプログラムは、Pin6にフルカラーLEDを接続することを想定していますが、このロボットはPin14に接続しているので、ピンの番号を修正します。
Linｅ11付近の #define PIN 6 の定義を #define PIN 14 に変更します。
Line14付近の #define NUMPIXELS 16を #define NUMPIXELS 1に変更します。
このLEDは一本のピンで、何個ものフルカラーLEDを制御できますが、頭の上に一つしか載せていないので１に設定します。
Line 20付近の Adafruit_NeoPixel() の引数の一部 NEO_GRB を NEO_RGB に変更します。
これはフルカラーLEDの色の指定順番を表しており、赤（Red）、緑（Green）、青（Blue）の順番に指定することをライブラリの関数に設定しています。
また、Line25行付近の三行をコメントアウトまたは削除します。これはArdｕｉｎo Nanoには不要な設定です。
//#if defined (__AVR_ATtiny85__)
// if (F_CPU == 16000000) clock_prescale_set(clock_div_1);
//#endif
・動作を確認する
フルカラーLEDがグリーンに点灯すれば成功です。
色は Line40行付近の pixels.Color(0,150,0) で指定されているので、 pixels.Color（）の引数を
pixels.Color(15,0,0) なら 赤
pixels.Color(0,15,0) なら 緑
pixels.Color(0,0,15) なら 青
と変えて、色々な色に変えられるか確認して下さい。
色を混ぜて中間色を出すこともできますので、本当に光の三原色を混ぜると色々な色になるか遊んでみて下さい。
（！）フルカラーLEDは電気をすごく使うので、ロボットで使うときは 明るさは10から15くらいまでにして下さい。
５．ジャイロセンサを読みとり
ジャイロセンサのGY-521に使用されている６軸センサーのMPU-6050のライブライは、ライブラリマネージャで検索しても出て来ません。
その場合は、ライブラリを使わずにゴリゴリ作るか、ライブラリをネットから探してくるかになります。
５．１ ごりごり作る場合
ごりごり作る場合は、とっくんラボさんのページが参考になります。
URL：https://tockn.com/entry/20170311182703
・スケッチを読み込む
ホームページのスケッチをコピーして、新規作成したプロジェクトに貼り付けます。
プロジェクト名は GY-521Test として保存します。
・プログラムを修正する
そのままでは Arduino Nano ではエラーになってしまうので、一か所修正します。
void setup() {
// Wire.begin(26, 25);
Wire.begin();
（中略）
なお修正版のソフトを後ろにつけておきます。
・動作を確認する
このプログラムはセンサーから読み取った数値をコンソールに出力するので、シリアルモニタを開いて確認します。
シリアルモニタにセンサが読み取った数値が表示されれば成功です。
５．２ ライブラリを使用する場合
・ライブラリをダウンロードする
MPU-6050用の外部ライブラリをここからClone or Downlode から Download ZIP を選択し、i2cdevlib-master.zip をダウンロードします。
URL：https://github.com/jrowberg/i2cdevlib
・ライブラリの展開
ダウンロードしたファイルを展開し、i2cdevlib-master.zip\i2cdevlib-master\Arduino の中の I2Cdev と MPU6050 の２つのフォルダをArduinoフォルダのLibrariesに格納します。
・スケッチを読み込む
Arduino IDEを起動し、
ファイル→スケッチ例→ MPU6050 →MPU6080_raw
を開きます。
ファイル→名前を付けて保存を選択し、MPU6080_raw としてスケッチを保存します。
・プログラムを修正する
Line83の Serial.begin(38400); を Serial.begin(9600); に変更します。
・動作を確認する
このプログラムはセンサーから読み取った数値をコンソールに出力するので、Arduino IDEの ツール->シリアルモニタを開いて確認します。
初期化が成功し MPU6050 connection successful と表示され、シリアルモニタにセンサから読み取った数値が表示されれば成功です。
文字化けする場合は、シリアルモニタの通信速度が9600bps になっているかを確認して下さい。
----------------------------------------------------------------
まとめ
これで吉野式ロボットのハードウェアとしては完成です。
上記の動作確認に使用したスケッチを組み合わせることで、二足歩行ロボットが作れます。
ここまで６日で作ってこれたでしょうか。
次回はハードウェア編のまとめとして、調整方法と簡単なロボットの動作プログラムを作成します。
GY-521修正ソースリストリスト
//----------------------------------------------------------------
/* Original とっくんラボ ： 20170311
URL https://tockn.com/entry/20170311182703
Modify 中野島ロボット：〇川 20180105
file name GY-521Test.ino
*/
#include
#define MPU6050_ADDR 0x68 // MPU-6050 device address
#define MPU6050_SMPLRT_DIV 0x19 // MPU-6050 register address
#define MPU6050_CONFIG 0x1a
#define MPU6050_GYRO_CONFIG 0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I 0x75
#define MPU6050_PWR_MGMT_1 0x6b
double offsetX = 0, offsetY = 0, offsetZ = 0;
double gyro_angle_x = 0, gyro_angle_y = 0, gyro_angle_z = 0;
float angleX, angleY, angleZ;
float interval, preInterval;
float acc_x, acc_y, acc_z, acc_angle_x, acc_angle_y;
float gx, gy, gz, dpsX, dpsY, dpsZ;
//I2C書き込み
void writeMPU6050(byte reg, byte data) {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(reg);
Wire.write(data);
Wire.endTransmission();
}
//I2C読み込み
byte readMPU6050(byte reg) {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(reg);
Wire.endTransmission(true);
Wire.requestFrom(MPU6050_ADDR, 1/*length*/);
byte data = Wire.read();
return data;
}
//キャリブレーション
void calcCalibration(){
Serial.print("Calculate Calibration");
for(int i = 0; i < 3000; i++){
int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_ADDR, 14, true);
raw_acc_x = Wire.read() << 8 | Wire.read();
raw_acc_y = Wire.read() << 8 | Wire.read();
raw_acc_z = Wire.read() << 8 | Wire.read();
raw_t = Wire.read() << 8 | Wire.read();
raw_gyro_x = Wire.read() << 8 | Wire.read();
raw_gyro_y = Wire.read() << 8 | Wire.read();
raw_gyro_z = Wire.read() << 8 | Wire.read();
dpsX = *1 * 360 / -2.0 / PI;
acc_angle_x = atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI;
dpsX = ((float)raw_gyro_x) / 65.5; // LSB sensitivity: 65.5 LSB/dps @ ±500dps
dpsY = ((float)raw_gyro_y) / 65.5;
dpsZ = ((float)raw_gyro_z) / 65.5;
//前回計算した時から今までの経過時間を算出
interval = millis() - preInterval;
preInterval = millis();
//数値積分
gyro_angle_x += (dpsX - offsetX) * (interval * 0.001);
gyro_angle_y += (dpsY - offsetY) * (interval * 0.001);
gyro_angle_z += (dpsZ - offsetZ) * (interval * 0.001);
//相補フィルター
angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);
angleZ = gyro_angle_z;
gyro_angle_x = angleX;
gyro_angle_y = angleY;
gyro_angle_z = angleZ;
}
void setup() {
// Wire.begin(26, 25);
Wire.begin();
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(9600);
delay(100);
//正常に接続されているかの確認
if (readMPU6050(MPU6050_WHO_AM_I) != 0x68) {
Serial.println("\nWHO_AM_I error.");
while (true);
}
//設定を書き込む
writeMPU6050(MPU6050_SMPLRT_DIV, 0x00); // sample rate: 8kHz/(7+1) = 1kHz
writeMPU6050(MPU6050_CONFIG, 0x00); // disable DLPF, gyro output rate = 8kHz
writeMPU6050(MPU6050_GYRO_CONFIG, 0x08); // gyro range: ±500dps
writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
writeMPU6050(MPU6050_PWR_MGMT_1, 0x01); // disable sleep mode, PLL with X gyro
//キャリブレーション
calcCalibration();
Serial.println();
offsetX /= 3000;
offsetY /= 3000;
offsetZ /= 3000;
Serial.print("offsetX : ");
Serial.println(offsetX);
Serial.print("offsetY : ");
Serial.println(offsetY);
Serial.print("offsetZ : ");
Serial.println(offsetZ);
}
void loop() {
//加速度、ジャイロから角度を計算
calcRotation();
Serial.print("angleX : ");
Serial.print(angleX);
Serial.print(" angleY : ");
Serial.print(angleY);
Serial.print(" angleZ : ");
Serial.println(angleZ);
}
// End of File.
//----------------------------------------------------------------
購入品情報
この辺りになると、具体的な部品の知識や、センサに合わせたプログラミング知識が必要になってきます。
ここで紹介するArudino関連本は、”くせが強く、中身が濃い”本です。
Day６の内容が難しい、上手くいかないと思ったら、どちらかを覗いてみて下さい。きっと抜け出すヒントがあると思います。
楽天ブックス
「電子オルガン」「電卓」「ブートローダ／スケッチラ I／O books 谷川寛 工学社アルドゥイーノ
楽天市場 by
![Arduino電子工作 「電子オルガン」「電卓」「ブートローダ／スケッチラ （I／O books） [ 谷川寛 ] の詳しい情報を見る / ウェブリブログ商品ポータル](https://cdn-ak.f.st-hatena.com/images/fotolife/n/nakarobo/20180912/20180912130938.gif "Arduino電子工作 「電子オルガン」「電卓」「ブートローダ／スケッチラ （I／O books） [ 谷川寛 ] の詳しい情報を見る / ウェブリブログ商品ポータル")
*1:float)raw_gyro_x) / 65.5;
dpsY = ((float)raw_gyro_y) / 65.5;
dpsZ = ((float)raw_gyro_z) / 65.5;
offsetX += dpsX;
offsetY += dpsY;
offsetZ += dpsZ;
if(i % 1000 == 0){
Serial.print(".");
}
}
}
//加速度、ジャイロから角度を計算
void calcRotation(){
int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
//レジスタアドレス0x3Bから、計14バイト分のデータを出力するようMPU6050へ指示
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_ADDR, 14, true);
//出力されたデータを読み込み、ビットシフト演算
raw_acc_x = Wire.read() << 8 | Wire.read();
raw_acc_y = Wire.read() << 8 | Wire.read();
raw_acc_z = Wire.read() << 8 | Wire.read();
raw_t = Wire.read() << 8 | Wire.read();
raw_gyro_x = Wire.read() << 8 | Wire.read();
raw_gyro_y = Wire.read() << 8 | Wire.read();
raw_gyro_z = Wire.read() << 8 | Wire.read();
//単位Gへ変換
acc_x = ((float)raw_acc_x) / 16384.0;
acc_y = ((float)raw_acc_y) / 16384.0;
acc_z = ((float)raw_acc_z) / 16384.0;
//加速度センサーから角度を算出
acc_angle_y = atan2(acc_x, acc_z + abs(acc_y
