最近、ロボット開発を始めました。
ロボットの基礎技術は、少なくとも以下が必要で本来取り付く島がないです。
①ロボット制作
②制御技術
③電子回路基板
ところが、以下の本に出合いました。
予算1万円でつくる二足歩行ロボット (I/O BOOKS) Tankobon Hardcover – June 24, 2020
by 中村 俊幸 (著)@amzon
これを購入したのが5/25
そして、決定的になったのは、以下のTweetを見たときでした。
丁度、上記の本を眺めているときだったのでちょっと衝撃です。
これは、3dプリンターでやらなきゃということで、すぐ開始しました。
※3dプリンターは6/8に別のフライホイールの倒立振り子作成のために使い始めていました
ということで、このレベルには到達していませんが、話を始めようと思います。
やったこと
・3dプリント
・回路基板について
・制御プログラム
・結果
・3dプリント
ロボット制作は、本書のリンク先からダウンロードした設計図を3dプリントするだけです。
とはいえ、fusion360のファイルなので、これを3dプリント出力して、tincarcadで処理しました。
処理は、二分割してstlで出力します。
そのファイルをcureで読みんこんで、gcordで出力します。
3dプリンターは、上記のHomemadegarbageさんが使われているものと同じ以下のものを使いました。
※ググるといろいろありますが、同じものを使うのが確実です
ANYCUBIC MEGA-S 3D Printer@amazon
（簡単なんだけど）重量があり、どうにか組み立てられました。
・回路基板について
本書を読むと、回路は回路基板設計用のKicadでやるような説明です。
しかし、これはそれなりに時間がかかりますし、発注するのも初体験になるので面倒です。
ところが、実は制御はこれまでやってきた倒立振り子と大差ないものです。
ということで、ここは全部端折って、m5stackの製品でやれそうです。
ちょっと前にm5atomをやっていたので、これを使うことにしました。
※顔イメージならほかの製品を使うのがいいかもしれません
※まあ、本書の解説はPCA9865を別基盤に載せ替え＋esp32を一体型で回路を設計されています。これはこれで使いやすそうなので、そのうち挑戦したいと思います。
ということで、どうしても回路周りは後回しにして、配線は犠牲にしてもとにかく動けばＯＫというスタンスで進めます。
・制御プログラム
本書には、制御プログラムも書かれていますが、今回は読み飛ばしました。
まず、外部からコントロールするために、以前やったPC3コントローラーで実施します。
今回利用するm5atomのmacアドレスは、以前の方法と同様に取得して利用します。
# include <Ps3Controller.h>
void setup()
{
Serial.begin(115200);
Ps3.begin();
String address = Ps3.getAddress();
Serial.print("The ESP32's Bluetooth MAC address is: ");
Serial.println(address);
}
void loop()
{
String address = Ps3.getAddress();
Serial.print("The ESP32's Bluetooth MAC address is: ");
Serial.println(address);
// The ESP32's Bluetooth MAC address is: 94:b9:7e:92:4a:da
}
また、servoは全部で12個使いますが、この制御はこれもm5atomで倒立振り子をやった時の記事と同様に、PCA9685を利用すれば同じようなコードで制御できます。
※m5atomだと6個までは制御できそうですが、ちょっと不足します。
コード詳細
まだまだ工夫すべきだし、動きは調整が必要ですが、ほぼ以下のコードを改善すればいいと思うレベルになったので、解説していきます。
※予測的な動作をROSでシミュレーションしたいと思いますが、まだできていません
環境構築
Raspi4上のArduinoIDEで開発しました。
ポイントは、以下の通りです。
①ArduinoIDEのダウンロード
Downloadsから、ARM 32 bitsをダウンロードします。
解凍して、ファイルのinstall.shをダブルクリックでraspiのプログラムにインストール（掲載）され、デスクトップにも起動リンクが表れます。
ここでポイントは実は同じようにRaspiにArduinoをインストールできる手順がありますが、それはunoなどは使えますが、設定ファイルが異なりボードが追加できません
そして、一度インストールしてしまうと、再インストールしても元にもどせず、RaspiのSDからやり直しが発生しました。
原因は、よくわかりませんが一度インストール後、新たに出来るArduinoとdirにものを入れるとその後修復が出来なくなってしまいました。
ということで正しい手順で一発でインストールするのが簡単です。
その後は、以前と同様な手順でboard(esp32)インストールやプログラム管理を起動して、m5atom core2, stickcなどのスケッチをインストールします。
また、今回開発中のアプリを別のraspiから移設しましたが、普通にSDに焼いて、そのまま開いて、それをコピーしてやると先ほどのArduinoのdir配下に保存出来ました。
もちろん、SDのままでも継続して開発できます。
利用Libraryと準備
Libraryのうち、Adafruitは、以下のサイトからzipダウンロードして、zipでインストールしました。
adafruit/Adafruit-PWM-Servo-Driver-Library
また、サーボはsg90なので以前と同じ最大値、最小値を利用します。
skは、時間と考えてください
paramはarm() leg() walk()などの動作に紐づけられた変数です。
Ps3Controllerは、プログラム管理からインストールします。
# include <M5Atom.h>
# include <Wire.h>
# include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
# define SERVOMIN 143
// 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最小パルス幅 ：4096/20×0.7＝102.4 （0.5ms：SG90、0°の時のパルス幅）
# define SERVOMAX 471
// 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最大パルス幅 ：4096/20×2.3＝491.5 （2.4ms：SG90、180°の時のパルス幅）
int sk =0;
int param;
# include <Ps3Controller.h>
notify
Ps3コントローラーの命令と紐づけます。
cross/square/triangle/circle buttonは直接動作と紐づけました。
一方、D-pad buttonという左側にある上下で、動作順を繰り上げ、繰り下げするように設定しています。
※紐づけていない動作は、未完成なものです。
int player = 0;
void notify()
{
//--- Digital cross/square/triangle/circle button events ---
if( Ps3.event.button_down.triangle ) {
param = 0; //arm()
Serial.println(param);
Serial.println("Change2Arm");
}
if( Ps3.event.button_down.square ) {
param = 5; //Furue()
Serial.println(param);
Serial.println("Change2Furue");
}
if( Ps3.event.button_down.cross ) {
param = 6; //Swing()
Serial.println(param);
Serial.println("Change2Swing");
}
if( Ps3.event.button_down.circle ) {
param = 3; //side_step()
Serial.println(param);
Serial.println("Change2Step_L");
}
//--------------- Digital D-pad button events --------------
if( Ps3.event.button_down.up ){
param += 1;
Serial.println(param);
Serial.println("Change param up");
}
if( Ps3.event.button_down.down ){
param += -1;
Serial.println(param);
Serial.println("Change param down");
}
}
Bleコネクション
出来たら標準出力にConnectedと表示します。
void onConnect(){
Serial.println("Connected.");
}
初期化
PCA9685のためのI2C初期化が必要です、
void setup() {
Serial.begin(115200); //シリアル通信を115200bpsに設定
Ps3.attach(notify);
Ps3.attachOnConnect(onConnect);
Ps3.begin("94:b9:7e:92:4a:da");
param = 0; //2; //1; //0; //3; //4; //5; //6;
M5.begin(true, false, true);
Wire.begin(26,32); //i2cのpin宣言が重要 SDA, SCL
pwm.begin();
pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
}
12個サーボの書込み関数
それぞれの動作ごとに記載してもいいですが、まとめて書込みする関数を用意しました。
void servo_write_func(float p0,float p15,float p1,float p14,float p2,float p13,float p3,float p12,float p5,float p10,float p7,float p8){
servo_write(0, p0);
servo_write(15, p15);
servo_write(1, p1);
servo_write(14, p14);
servo_write(2, p2);
servo_write(13, p13);
servo_write(3, p3);
servo_write(12, p12);
servo_write(5, p5);
servo_write(10, p10);
servo_write(7, p7);
servo_write(8, p8);
}
動作ごとに関数に数字を送ると動作します。
ちなみに、サーボの基準点が取り付けたときに決まるので、制作段階で合わせておくと設定が楽かもしれません。
void arm(){
servo_write_func(1*sk%180-90,1*sk%180-90,50,-50,60,0,30,-10,-20,20,10,0);
}
void leg(){
servo_write_func(0,0,50,-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void walk(){
servo_write_func(0.5*(1*sk%180-90),0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void back(){
servo_write_func(-0.5*(1*sk%180-90),-0.5*(1*sk%180-90),-0.1*(1*sk%180-90)+50,0.1*(1*sk%180-90)-50,60,0,-0.1*(1*sk%180-90)+30,-0.1*(1*sk%180-90)-10,-0.05*(1*sk%180-90)-20,0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void side_step_R(){
servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,0.2*(1*sk%180-90)+60,0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void side_step_L(){
servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,-0.2*(1*sk%180-90)+60,-0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
loop()関数
あとは、paramにあった動作をさせるだけです。
震えとスイングが面白いです。
上の関数を組み合わせれば、色々な動作をさせられると思います。
void loop() {
param = constrain(param,0,6);
if(param ==0){
arm();
delay(5);
}
if(param ==1){
walk();
delay(5);
}
if(param ==2){
side_step_L();
delay(5);
}
if(param ==3){
side_step_R();
}
if(param ==4){
leg(); //back();
delay(5);
}
if(param ==5){
// furue
if(sk%10>5){ //Furue; 10,5 swing; 360,180
side_step_L();
}
if(sk%10<=5){
side_step_R();
}
delay(0);
}
if(param ==6){
// swing
if(sk%360>180){ //Furue; 10,5 swing; 360,180
side_step_L();
}
if(sk%360<=180){
side_step_R();
}
delay(5);
}
delay(0);
sk += 1;
M5.update();
}
void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
ang = map(ang, -90, 90, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
pwm.setPWM(ch, 0, ang);
}
・結果
結果は、以下のような動作ができるようになりました。
まとめ
・初めての3dプリンターで二足歩行ロボットを作ってみた
・m5atomで歩行させてみた
・動作を整理して、それぞれの動作を調整すればRosに頼らなくても一定の動作はできた
・Rosシミュレーションやりたいな
・ロボットが身近になり、いろいろなロボットを作れるような気がする
コード全体
コード
# include <M5Atom.h>
# include <Wire.h>
# include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
# define SERVOMIN 143
// 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最小パルス幅 ：4096/20×0.7＝102.4 （0.5ms：SG90、0°の時のパルス幅）
# define SERVOMAX 471
// 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最大パルス幅 ：4096/20×2.3＝491.5 （2.4ms：SG90、180°の時のパルス幅）
int sk =0;
int param;
# include <Ps3Controller.h>
int player = 0;
void notify()
{
//--- Digital cross/square/triangle/circle button events ---
if( Ps3.event.button_down.triangle ) {
param = 0; //arm()
Serial.println(param);
Serial.println("Change2Arm");
}
if( Ps3.event.button_down.square ) {
param = 5; //Furue()
Serial.println(param);
Serial.println("Change2Furue");
}
if( Ps3.event.button_down.cross ) {
param = 6; //Swing()
Serial.println(param);
Serial.println("Change2Swing");
}
if( Ps3.event.button_down.circle ) {
param = 3; //side_step()
Serial.println(param);
Serial.println("Change2Step_L");
}
//--------------- Digital D-pad button events --------------
if( Ps3.event.button_down.up ){
param += 1;
Serial.println(param);
Serial.println("Change param up");
}
if( Ps3.event.button_down.down ){
param += -1;
Serial.println(param);
Serial.println("Change param down");
}
}
void onConnect(){
Serial.println("Connected.");
}
void setup() {
Serial.begin(115200); //シリアル通信を115200bpsに設定
Ps3.attach(notify);
Ps3.attachOnConnect(onConnect);
Ps3.begin("94:b9:7e:92:4a:da");
param = 0; //2; //1; //0; //3; //4; //5; //6;
M5.begin(true, false, true);
Wire.begin(26,32); //i2cのpin宣言が重要 SDA, SCL
pwm.begin();
pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
}
void servo_write_func(float p0,float p15,float p1,float p14,float p2,float p13,float p3,float p12,float p5,float p10,float p7,float p8){
servo_write(0, p0);
servo_write(15, p15);
servo_write(1, p1);
servo_write(14, p14);
servo_write(2, p2);
servo_write(13, p13);
servo_write(3, p3);
servo_write(12, p12);
servo_write(5, p5);
servo_write(10, p10);
servo_write(7, p7);
servo_write(8, p8);
}
void arm(){
servo_write_func(1*sk%180-90,1*sk%180-90,50,-50,60,0,30,-10,-20,20,10,0);
}
void leg(){
servo_write_func(0,0,50,-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void walk(){
servo_write_func(0.5*(1*sk%180-90),0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void back(){
servo_write_func(-0.5*(1*sk%180-90),-0.5*(1*sk%180-90),-0.1*(1*sk%180-90)+50,0.1*(1*sk%180-90)-50,60,0,-0.1*(1*sk%180-90)+30,-0.1*(1*sk%180-90)-10,-0.05*(1*sk%180-90)-20,0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void side_step_R(){
servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,0.2*(1*sk%180-90)+60,0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void side_step_L(){
servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,-0.2*(1*sk%180-90)+60,-0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}
void loop() {
param = constrain(param,0,6);
if(param ==0){
arm();
delay(5);
}
if(param ==1){
walk();
delay(5);
}
if(param ==2){
side_step_L();
delay(5);
}
if(param ==3){
side_step_R();
}
if(param ==4){
leg(); //back();
delay(5);
}
if(param ==5){
// furue
if(sk%10>5){ //Furue; 10,5 swing; 360,180
side_step_L();
}
if(sk%10<=5){
side_step_R();
}
delay(0);
}
if(param ==6){
// swing
if(sk%360>180){ //Furue; 10,5 swing; 360,180
side_step_L();
}
if(sk%360<=180){
side_step_R();
}
delay(5);
}
delay(0);
sk += 1;
M5.update();
}
void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
ang = map(ang, -90, 90, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
pwm.setPWM(ch, 0, ang);
}