3Dプリンタで作る吉野式二足歩行ロボット「ArduiroidOne」の作り方を公開しています。ArduinoNanoV3を使う場合の記事です。
Arduino Nanoを使う場合
コントローラにArduinoNanoV3とそのCloneを使う場合のArduiroidOneの動作確認手順です。ArduinoNanoには通常はLEDを点滅するだけのプログラム(Blinkスケッチ)が書き込まれています。これにArduiroidOne用のプログラムを書き込む必要があります。
Arduino Nanoは Arduino純正品の他に多くのクローンが安価に提供されています。なおArduino Nanoの設計情報は公開されているため、クローンは違法コピーではありません。
1.接続
SG90サーボモータをArduinoに繋いて行きます。ArduiroidOneを前から見て、左足の下をサーボ番号0をD4として数の通りに接続して行きます。
Aruduino NanoのD0、D1はシリアル通信で利用していて、 D2、D3はセンサからの割り込みで使用するので未使用(予約)しています。サーボモータを接続するのはD4からD11までとなり、D13はAruduino Nano の場合は 基板上のLEDに接続されているので、容易に追加できるサーボモータはD12の一個になります
2.インストール
ArduiroidOneを動かすにはまずスケッチを書き込む必要がありますが、Arduinoの開発環境のインストールがまだの場合は、Arduino IDEをダウンロードしインストールします。
ダウンロードはここから
Windows10の場合は、Windowsインストーラか、圧縮ファイル、Windowsストアアプリから入手できます。一番上の”Windows Instraller. for Windows7 and UP”をクリックしてダウンロードしてインストールするのが簡単です。
画面が変わったら、下の”JUST DOWNLOAD”を押すと無料でダウンロードできます。
(カンパウェアなので、余裕がある時は寄付して上げて下さい)
この記事の執筆時点のArduinoIDEの最新バージョンは1.8.12です。
ダウンロードが終わったら、ダウンロードフォルダに、
arduino-1.8.12-windows.exe
がダウンロードされているはずなので、管理者権限で起動してPCにインストールして下さい。
3.ソフトウェアの設定
Arduino IDEをインストールしたら、ArduiroidOneとPCと接続し、Arduino IDE起動して以下の設定を行います。
Arduino IDEのツールメニューから
ボード:ArduinoNano
プロセッサ:ATmega328P(Old Bootloader)
シリアルポート:COM3
(!)COMの次の数値は環境よって変わりますが、COM1は多くの場合PCの本体で使用されているので、COM3以上のどれかになります。
4.スケッチの書き込み
ArduiroidOneのTest用スケッチをThingiversのArduiroidOneのページからダウンロードするか、リスト(末尾参照)を打ち込んだら以下の手順で書き込みます。
- Aruduino IDEから ArduiroidOneTest.ino を開きます。
- スケッチ→マイコンボードに書き込む を押します。
- ”スケッチをコンパイルしています...”が表示されてコンパイルが終わると、続いて”マイコンボードに書き込んでいます...”と表示されます。
- ”ボードへの書き込みが完了しました。”と表示されたら終了です。
(!)エラーが出て書き込みができない場合は、PCとちゃんと接続されているか確認して下さい。
(できれば、ダウンロードせずにリストを打ち込んで理解しながら進めて欲しいと思います。)
つづく
次回は調整を行います。
ArduiroidOneTestスケッチリスト
// Arduiroid Humanoid Robot // File ArduiroidOneTest.ino // Servo #include <Servo.h> #define MAX_SERVO_NUM 8 #define SERVO_DELAY 300 // 60deg/100mse + 25mse Servo servo[MAX_SERVO_NUM]; int angle[MAX_SERVO_NUM]; // 現在角度 //ArduiroidOne (Front View) // ----- // | 7 | // | D11 | // ----- ----- ----- // | 6 | | 5 | // | D10 | | D09 | // ----- ----- // ----- // | 4 | // | D08 | // ----- // ----- ----- // | 3 | | 1 | // | D07 | | D05 | // ----- ----- // | 2 | | 0 | // | D06 | | D04 | // ----- ------ // RIGHT LEFT const int8_t servoCenter[MAX_SERVO_NUM] = {90,90,90,90,90,90,90,90}; // サーボーの原点座標 const int8_t servoDir[MAX_SERVO_NUM] = {1,1,-1,-1,-1,-1,1,-1}; // 軸の方向 前が+、右が+ const int8_t basicPosition[32][MAX_SERVO_NUM] = { {0,0,0,0,0,0,0,0}, // P00: {0,0,0,0,0,0,0,0}, // P01: {0,0,0,0,0,0,0,0}, // P02: {0,0,0,0,0,0,0,0}, // P03: {0,0,0,0,0,0,0,0}, // P04: {0,0,0,0,0,0,0,0}, // P05: {0,0,0,0,0,0,0,0}, // P06: {0,0,0,0,0,0,0,0}, // P07: {0,0,0,0,0,0,0,0}, // P08: {0,0,0,0,0,0,0,0}, // P09: {0,0,0,0,0,0,0,0}, // P10: {0,0,0,0,0,0,0,0}, // P11: {0,0,0,0,0,0,0,0}, // P12: {0,0,0,0,0,0,0,0}, // P13: {0,0,0,0,0,0,0,0}, // P14: {0,0,0,0,0,0,0,0}, // P15: {0,0,0,0,0,0,0,0}, // P16: {0,0,0,0,0,0,0,0}, // P17: {0,0,0,0,0,0,0,0}, // P18: {0,0,0,0,0,0,0,0}, // P19: {0,0,0,0,0,0,0,0}, // P20: {0,0,0,0,0,0,0,0}, // P21: {0,0,0,0,0,0,0,0}, // P22: {0,0,0,0,0,0,0,0}, // P23: {0,0,0,0,0,0,0,0}, // P24: {0,0,0,0,0,0,0,0}, // P25: {0,0,0,0,0,0,0,0}, // P26: {0,0,0,0,0,0,0,0}, // P27: {0,0,0,0,0,0,0,0}, // P28: {0,0,0,0,0,0,0,0}, // P29: {0,0,0,0,0,0,0,0}, // P30: {0,0,0,0,0,0,0,0} // P31: }; // This angle value range is -90 to 90 deg. but 99 is keep current angle // Setup First Power On at Once void setup() { // put your setup code here, to run once: Serial.begin(57600); while (!Serial); // Servo Test attachServo(); detachServo(); } // End setup // MainLoop void loop() { // if there's any serial available, read it: while (Serial.available() > 0) { // servo ON attachServo(); char command = Serial.read(); switch (command){ case '0': // ToDo set8Pos(0); break; case '1': // ToDo set8Pos(0); break; case '2': // ToDo set8Pos(0); break; case '3': // ToDo set8Pos(0); break; case '4': // ToDo set8Pos(0); break; case '5': // ToDo set8Pos(0); break; case '6': // ToDo set8Pos(0); break; case '7': // ToDo set8Pos(0); break; case '8': // ToDo set8Pos(0); break; case '9': // ToDo set8Pos(0); break; case 'D': // Wait case 'd': delay(SERVO_DELAY); break; } // switch // SERVO Set command if ( command == 'S' || command == 's' ) // 'S' is Servo Command "S,Servo_no,Angle 0-180 deg" { Serial.print(command); Serial.print(','); int servoNo = Serial.parseInt(); Serial.print(servoNo); Serial.print(','); int servoAngle = Serial.parseInt(); Serial.print(servoAngle); Serial.println(); if ( 0 <= servoNo && servoNo < MAX_SERVO_NUM ) { // Single Servo Move servo[servoNo].write(servoAngle); // Wait Servo Move delay(SERVO_DELAY); } } // SERVO Set END // SERVO All command if ( command == 'A' || command == 'a' ) // 'A' is Servo All Command "A,Angle0, Angle1...Angle7" { Serial.print(command); // read Angle from Serial for (int i = 0; i < MAX_SERVO_NUM; i++) { angle[i] = Serial.parseInt(); Serial.print(','); Serial.print(angle[i]); } Serial.println(); // servo Move set8Angle( angle[0], angle[1], angle[2], angle[3], angle[4], angle[5], angle[6], angle[7] ); // Wait Servo Move delay(SERVO_DELAY); } // SERVO All End // Position command if ( command == 'P' || command == 'p' ) // 'P'is a Position Command "P positionNo" for basicPosition { Serial.print(command); Serial.print(','); int positionNo = Serial.parseInt(); Serial.print(positionNo); Serial.print('\n'); // servo Move set8Pos(positionNo); // Wait Servo Move delay(SERVO_DELAY); Serial.print('\n'); } // Position End. // Servo OFF detachServo(); } // End while }// End loop // attach ServoMotors void attachServo() { // attach Pin to ServoMotor servo[0].attach(4); servo[1].attach(5); servo[2].attach(6); servo[3].attach(7); servo[4].attach(8); servo[5].attach(9); servo[6].attach(10); servo[7].attach(11); } // dettach ServoMotors void detachServo() { servo[0].detach(); servo[1].detach(); servo[2].detach(); servo[3].detach(); servo[4].detach(); servo[5].detach(); servo[6].detach(); servo[7].detach(); } // Set 8 ServoMotor angles void set8Angle(int a0, int a1, int a2, int a3, int a4, int a5, int a6, int a7){ servo[0].write(a0); servo[1].write(a1); servo[2].write(a2); servo[3].write(a3); servo[4].write(a4); servo[5].write(a5); servo[6].write(a6); servo[7].write(a7); } // Set 8 ServoMoter Position void set8Pos(int positionNo) { int8_t reverse = false; if(positionNo < 0){ reverse = true; positionNo = abs(positionNo); } if( 32 <= positionNo) return; if(!reverse){ if(basicPosition[positionNo][0] != 99) angle[0] = servoCenter[0] + (basicPosition[positionNo][0] * servoDir[0]); if(basicPosition[positionNo][1] != 99) angle[1] = servoCenter[1] + (basicPosition[positionNo][1] * servoDir[1]); if(basicPosition[positionNo][2] != 99) angle[2] = servoCenter[2] + (basicPosition[positionNo][2] * servoDir[2]); if(basicPosition[positionNo][3] != 99) angle[3] = servoCenter[3] + (basicPosition[positionNo][3] * servoDir[3]); if(basicPosition[positionNo][4] != 99) angle[4] = servoCenter[4] + (basicPosition[positionNo][4] * servoDir[4]); if(basicPosition[positionNo][5] != 99) angle[5] = servoCenter[5] + (basicPosition[positionNo][5] * servoDir[5]); if(basicPosition[positionNo][6] != 99) angle[6] = servoCenter[6] + (basicPosition[positionNo][6] * servoDir[6]); if(basicPosition[positionNo][7] != 99) angle[7] = servoCenter[7] + (basicPosition[positionNo][7] * servoDir[7]); } else {
if(basicPosition[positionNo][2] != 99)
angle[0] = servoCenter[0] + (basicPosition[positionNo][2] * servoDir[0]);
if(basicPosition[positionNo][3] != 99)
angle[1] = servoCenter[1] + (basicPosition[positionNo][3] * servoDir[1]);
if(basicPosition[positionNo][0] != 99)
angle[2] = servoCenter[2] + (basicPosition[positionNo][0] * servoDir[2]);
if(basicPosition[positionNo][1] != 99)
angle[3] = servoCenter[3] + (basicPosition[positionNo][1] * servoDir[3]);
if(basicPosition[positionNo][4] != 99)
angle[4] = servoCenter[4] + (basicPosition[positionNo][4] * (-servoDir[4]));
if(basicPosition[positionNo][6] != 99)
angle[5] = servoCenter[5] + (basicPosition[positionNo][6] * servoDir[5]);
if(basicPosition[positionNo][5] != 99)
angle[6] = servoCenter[6] + (basicPosition[positionNo][5] * servoDir[6]);
if(basicPosition[positionNo][7] != 99)
angle[7] = servoCenter[7] + (basicPosition[positionNo][7] * (-servoDir[7]));
} // servo Move set8Angle( angle[0], angle[1], angle[2], angle[3], angle[4], angle[5], angle[6], angle[7] ); delay(SERVO_DELAY); } // Auther nakarobo 2020.05.10 // Ver 1.1 // End //---------------------