つづきに
年末に公開したよつあしロボットの「ナノ四駆」ですが、急ぎモデルを修正しています。
右側が作り直したフレームです。変更はミリ単位なので、パット見は変わりません。
アップロード完了
修正後のモデルを印刷して動作確認し、3Dモデル共有サイトにアップロードしておきました。
ダウンロードはこちらから
単3も使える
機体を少し大きくしたので(と言っても1mmです。)単3のNi-MHを使えるようになりました。これで一日中遊べます。ただちょっと重くなって約200gになりました。
スケッチは今週末の予定
そろそろArduino Nano の互換品も在庫が減ってきたので、久しぶりにAruduino Nano Every を使って動作確認をしています。
動作確認用のスケッチは今週末に公開する予定ですが、現時点のスケッチをここに載せておきます。(これで間違って消してしまっても大丈夫!)
// Arduiroid [Nano4Runner] Robot 0.0 nakarobo 2021.1.27 // File Arduroid3Test.ino // Servo #include <Servo.h> #define MAX_SERVO_NUM 4 Servo servo[MAX_SERVO_NUM+1]; bool servoOn; // true:Servo Power On false:Off //Arduroid3 (Top View) //FRONT // ----- ----- // | 1 | | 2 | // | P04 | | P05 | // ----- ----- // // ----- ----- // | 3 | | 4 | // | P06 | | P07 | // ----- ----- //BACK // Setup First Power On at Once void setup() { // put your setup code here, to run once: Serial.begin(9600); while (!Serial); pinMode(LED_BUILTIN, OUTPUT); // Servo Test servoOn = false; attachServo(); detachServo(); } // MainLoop void loop() { // if there's any serial available, read it: while (Serial.available() > 0) { char command = Serial.read(); switch (command){ case '0': // DEMO set4Pos( 80, 80,100,100); delay(500); set4Pos( 100,100,80,80); delay(500); set4Pos( 60, 60,120,120); delay(500); set4Pos( 120,120,60,60); delay(500); set4Pos( 40, 40,140,140); delay(500); set4Pos( 140,140,40,40); delay(500); set4Pos( 90, 90, 90, 90); delay(100); break; case '1': // Right set4Pos(125,135,125,135); delay(200); set4Pos( 90, 90,125,135); delay(200); set4Pos( 90, 90, 90, 90); delay(100); break; case '2': // Foward set4Pos( 90, 90, 90, 90); delay(100); set4Pos( 45, 80,120, 70); delay(200); set4Pos( 80, 90,120,120); delay(200); set4Pos( 90, 90, 90, 90); delay(200); set4Pos(100,135,110, 60); delay(200); set4Pos(100,100, 60, 60); delay(200); set4Pos( 90, 90, 90, 90); delay(100); break; case '3': // Left set4Pos( 45, 55, 45, 55); delay(200); set4Pos( 90, 90, 45, 55); delay(200); set4Pos( 90, 90, 90, 90); delay(100); break; case '4': // Lay down set4Pos(160, 20,160, 20); delay(300); break; case '5': // Stand up set4Pos( 70,110,110, 70); delay(100); set4Pos( 90, 90, 90, 90); delay(300); break; case '6': // Ready set4Pos( 20, 20,160, 20); delay(300); break; case '7': // Hi-Hi set4Pos( 45,135,150, 30); delay(100); set4Pos(135,135,135, 20); delay(300); set4Pos(135, 70,145, 30); delay(300); set4Pos( 45, 70,145, 45); delay(300); set4Pos( 45,45,150, 30); delay(100); break; case '8': // Back set4Pos( 90, 90, 90, 90); delay(100); set4Pos( 90, 90, 60, 30); delay(200); set4Pos( 90,135, 60, 90); delay(200); set4Pos( 90, 90, 90, 90); delay(200); set4Pos( 90, 90,150,120); delay(200); set4Pos( 45, 90, 90,120); delay(200); set4Pos( 90, 90, 90, 90); delay(100); break; case '9': // Run set4Pos( 60,120, 45,135); delay(200); set4Pos( 60, 120, 45,135); delay(100); set4Pos( 90, 90, 90, 90); delay(200); set4Pos( 100, 80, 170, 10); delay(200); set4Pos( 100, 80, 120, 60); delay(100); set4Pos( 60, 120, 45,135); delay(300); set4Pos( 90, 90, 90, 90); delay(200); set4Pos( 100, 80, 170, 10); delay(200); set4Pos( 100, 80, 120, 60); delay(100); set4Pos( 60, 120, 45,135); delay(300); set4Pos( 90, 90, 90, 90); delay(200); set4Pos( 100, 80, 170, 10); delay(200); set4Pos( 100, 80, 120, 60); delay(100); break; case 'T': // Test case 't': set4Pos( 90, 90, 90, 90); delay(100); set4Pos( 45, 80, 120, 70); delay(200); set4Pos( 80, 90,120,120); delay(200); set4Pos( 90, 90, 90, 90); delay(200); set4Pos( 100,135, 110, 60); delay(200); set4Pos( 100,100, 60, 60); delay(200); set4Pos( 90, 90, 90, 90); delay(100); break; case 'O': // Serbo On/OFF case 'o': if( servoOn ) { detachServo(); servoOn = false; } else { attachServo(); servoOn = true; } break; case 'D': // Wait case 'd': delay(300); break; } // switch // SERVO Set command if ( command == 'S' || command == 's' ) // 'S' is Servo Command "S,Servo_no,Angle" { 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+1 ) { // Single Servo Move servo[servoNo].write(servoAngle); // Wait Servo Move delay(300); // 180/60(°/100msec)=300(msec) } } // SERVO Set END // SERVO All command if ( command == 'A' || command == 'a' ) // 'A' is Servo All Command "A,Angle0, Angle1...Angle3" { Serial.print(command); // read Angle from Serial int angle[MAX_SERVO_NUM]; for (int i = 0; i < MAX_SERVO_NUM; i++) { angle[i] = Serial.parseInt(); Serial.print(','); Serial.print(angle[i]); } Serial.println(); // servo Move set4Pos( angle[0], angle[1], angle[2], angle[3]); // Wait Servo Move delay(300); } // SERVO All End } // End while } // End loop // attach ServoMotors void attachServo() { // attach Pin to ServoMotor // servo[1].attach(4); // servo[2].attach(5); // servo[3].attach(6); // servo[4].attach(7); // MS18 600-1500-2400 // servo[1].attach(4, 600, 2400); // servo[2].attach(5, 600, 2400); // servo[3].attach(6, 600, 2400); // servo[4].attach(7, 600, 2400); // SG90 500-1450-2400 servo[1].attach(4, 500, 2400); servo[2].attach(5, 500, 2400); servo[3].attach(6, 500, 2400); servo[4].attach(7, 500, 2400); servoOn = true; // LED ON digitalWrite(LED_BUILTIN, HIGH); } // dettach ServoMotors void detachServo() { servo[1].detach(); servo[2].detach(); servo[3].detach(); servo[4].detach(); servoOn = false; // LED OFF digitalWrite(LED_BUILTIN, LOW); } // Set 4 ServoMotor angles void set4Pos(int a, int b, int c, int d){ servo[1].write(a); servo[2].write(b); servo[3].write(c); servo[4].write(d); } // End //---------------------
おわりに
やはりTower Pro 純正のSG90と単3 Ni-Mh電池の組み合わせはパワーが違います。機体は重くなりましたが、ものともせずにキビキビ動きます。ただ、SG90は動きが早すぎるせいか、足が滑ってしまっている感じです。また、機体が重くなると”しなり”を使っている分、モーションの修正も必要となっています。そこのところを修正して公開する予定です。
おしまい
購入品情報
Arduino Nano Every を久しぶりに乗せてみましたが、Aruduino IDE(開発環境)も落ち着いて来たので、普通に使えるようになってきました。
Amazonでも買えるようになりましたが、まだ高いですね。Arduino Nano とは値段とコネクタくらいで、特に変わったところはありませんが、内部のレジスタを使うようなArduino Nano のスケッチは動きません。Arduino Nanoのように使いこなすには、もう少し勉強が必要です。