記事一覧

M5Atomで小さい二足歩行ロボット歩行モーション作成

M5Atomで小さい二足歩行ロボット歩行モーション作成



今日は、RoboTakaoです。

先日まででM5Atomで二足歩行ロボットを組み立てて来ました。
今回は、歩行モーションを作成しました。

NX13_19.jpeg

まず、モーション作成前に、足を作り直しました。
スタイル重視で足を小さくしてしまっていたので、これでは蹴り上げることが出来ないと気が付き
足を大きくする変更をしました。

NX13_20.jpeg

あと、どうしても滑ってしまうので、足の裏にゴムを貼りました。

NX13_21.jpeg


さて、モーションは自作のMacOSアプリで作成。

NX13_22.png

NX13_23.png

前進、後進、右方向転回、左方向転回、右手上げ、左手上げを作りました。

動作確認



スケッチ


#include "M5Atom.h"
#include "BluetoothSerial.h" //BluetoothSerial

BluetoothSerial SerialBT;

const uint8_t Srv0 = 22; //GPIO Right Arm
const uint8_t Srv1 = 19; //GPIO Right Leg
const uint8_t Srv2 = 23; //GPIO Right Foot
const uint8_t Srv3 = 33; //GPIO Left Foot
const uint8_t Srv4 = 25; //GPIO Left Leg
const uint8_t Srv5 = 21; //GPIO Left Arm

const uint8_t srv_CH0 = 0, srv_CH1 = 1, srv_CH2 = 2, srv_CH3 = 3, srv_CH4 = 4, srv_CH5 = 5; //チャンネル
const double PWM_Hz = 50; //PWM周波数
const uint8_t PWM_level = 16; //PWM 16bit(0~65535)

int pulseMIN = 1640; //0deg 500μsec 50Hz 16bit
int pulseMAX = 8190; //180deg 2500μsec 50Hz 16bit

int cont_min = 0;
int cont_max = 180;

int angZero[] = {90, 78, 85, 88, 92, 90};
int ang0[6];
int ang1[6];
int ang_b[6];
char ang_c[6];
int ts=300;
int td=20;

bool IMU6886Flag = false; //今回はなくても良い

// Forward Step
int f_s[19][6]={
{0,0,0,0,0,0},
{0,0,-15,-10,0,0},
{0,0,-15,-15,0,0},
{-20,15,-15,-15,15,-20},
{-20,15,0,0,15,-20},
{-20,15,10,15,15,-20},
{-20,15,15,15,15,-20},
{0,0,15,15,0,0},
{20,-15,15,15,-15,20},
{20,-15,0,0,-15,20},
{20,-15,-15,-10,-15,20},
{20,-15,-15,-15,-15,20},
{0,0,-15,-15,0,0},
{-20,15,-15,-15,15,-20},
{-20,15,0,0,15,-20},
{-20,15,10,15,15,-20},
{-20,15,15,15,15,-20},
{0,0,15,15,0,0}};

int b_s[19][6]={
{0,0,0,0,0,0},
{0,0,-15,-10,0,0},
{0,0,-15,-15,0,0},
{20,-15,-15,-15,-15,20},
{20,-15,0,0,-15,20},
{20,-15,10,15,-15,20},
{20,-15,15,15,-15,20},
{0,0,15,15,0,0},
{-20,15,15,15,15,-20},
{-20,15,0,0,15,-20},
{-20,15,-15,-10,15,-20},
{-20,15,-15,-15,15,-20},
{0,0,-15,-15,0,0},
{20,-15,-15,-15,-15,20},
{20,-15,0,0,-15,20},
{20,-15,10,15,-15,20},
{20,-15,15,15,-15,20},
{0,0,15,15,0,0}};

int l_s[9][6]={
{0,0,0,0,0,0},
{0,0,10,15,0,0},
{0,0,15,15,0,0},
{-20,15,15,15,-15,-20},
{-20,15,0,0,-15,-20},
{-20,15,-15,-10,-15,-20},
{-20,15,-15,-15,-15,-20},
{0,0,-15,-15,0,0},
{0,0,0,0,0,0}};

int r_s[9][6]={
{0,0,0,0,0,0},
{0,0,-15,-10,0,0},
{0,0,-15,-15,0,0},
{-20,15,-15,-15,-15,-20},
{-20,15,0,0,-15,-20},
{-20,15,10,15,-15,-20},
{-20,15,15,15,-15,-20},
{0,0,15,15,0,0},
{0,0,0,0,0,0}};

int r_a[7][6]={
{0,0,0,0,0,0},
{80,0,0,0,0,0},
{0,0,0,0,0,0},
{80,0,0,0,0,0},
{0,0,0,0,0,0},
{80,0,0,0,0,0},
{0,0,0,0,0,0}};

int l_a[7][6]={
{0,0,0,0,0,0},
{0,0,0,0,0,-80},
{0,0,0,0,0,0},
{0,0,0,0,0,-80},
{0,0,0,0,0,0},
{0,0,0,0,0,-80},
{0,0,0,0,0,0}};

int delection = 0;

void Initial_Value(){ //initial servo angle
for (int j=0; j <=5 ; j++){
ang0[j] = angZero[j];
}
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j];
}
servo_set();
}

void face_clear(){
for(int i=0; i<25; i++){
M5.dis.drawpix(i, 0x000000); //black
//M5.dis.drawpix(i, 0xa5ff00); //orange
}
}

void face_center(){
M5.dis.drawpix(6, 0x00ff00); //red
M5.dis.drawpix(7, 0x00ff00);
M5.dis.drawpix(8, 0x00ff00);
M5.dis.drawpix(16, 0x0000ff); //blue 0x0000ff
M5.dis.drawpix(18, 0x0000ff);
}

void Srv_drive(int srv_CH,int SrvAng){
SrvAng = map(SrvAng, cont_min, cont_max, pulseMIN, pulseMAX);
ledcWrite(srv_CH, SrvAng);
}

void rs_gets_3c(char gtc[4])
{
char gc;
int i;

sprintf(gtc," ");
for( i = 0 ; i < 4 ; i++ )
{
gc = SerialBT.read();
if (gc == ',')
break;
else
gtc[i] = gc;
}
}

void forward_step()
{
for (int i=0; i <=18 ; i++){
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j] + f_s[i][j];
}
servo_set();
}
}

void back_step()
{
for (int i=0; i <=18 ; i++){
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j] + b_s[i][j];
}
servo_set();
}
}

void right_step()
{
for (int i=0; i <=8 ; i++){
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j] + r_s[i][j];
}
servo_set();
}
}

void left_step()
{
for (int i=0; i <=8 ; i++){
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j] + l_s[i][j];
}
servo_set();
}
}

void right_arm()
{
for (int i=0; i <=6 ; i++){
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j] + r_a[i][j];
}
servo_set();
}
}

void left_arm()
{
for (int i=0; i <=6 ; i++){
for (int j=0; j <=5 ; j++){
ang1[j] = angZero[j] + l_a[i][j];
}
servo_set();
}
}

void servo_set(){
int a[6],b[6];

for (int j=0; j <=5 ; j++){
a[j] = ang1[j] - ang0[j];
b[j] = ang0[j];
ang0[j] = ang1[j];
}

for (int k=0; k <=td ; k++){

Srv_drive(srv_CH0, a[0]*k/td+b[0]);
Srv_drive(srv_CH1, a[1]*k/td+b[1]);
Srv_drive(srv_CH2, a[2]*k/td+b[2]);
Srv_drive(srv_CH3, a[3]*k/td+b[3]);
Srv_drive(srv_CH4, a[4]*k/td+b[4]);
Srv_drive(srv_CH5, a[5]*k/td+b[5]);

delay(ts/td);
}
}

void setup() {
// void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
M5.begin(true, false, true);
Serial.begin(151200);
SerialBT.begin("M5Atom"); //PC側で確認するときの名前

pinMode(Srv0, OUTPUT);
pinMode(Srv1, OUTPUT);
pinMode(Srv2, OUTPUT);
pinMode(Srv3, OUTPUT);
pinMode(Srv4, OUTPUT);
pinMode(Srv5, OUTPUT);

//モータのPWMのチャンネル、周波数の設定
ledcSetup(srv_CH0, PWM_Hz, PWM_level);
ledcSetup(srv_CH1, PWM_Hz, PWM_level);
ledcSetup(srv_CH2, PWM_Hz, PWM_level);
ledcSetup(srv_CH3, PWM_Hz, PWM_level);
ledcSetup(srv_CH4, PWM_Hz, PWM_level);
ledcSetup(srv_CH5, PWM_Hz, PWM_level);

//モータのピンとチャンネルの設定
ledcAttachPin(Srv0, srv_CH0);
ledcAttachPin(Srv1, srv_CH1);
ledcAttachPin(Srv2, srv_CH2);
ledcAttachPin(Srv3, srv_CH3);
ledcAttachPin(Srv4, srv_CH4);
ledcAttachPin(Srv5, srv_CH5);

face_clear();
face_center();

Initial_Value();
}

void loop() {
M5.update();
if ( M5.Btn.wasReleased() ) {
Initial_Value();
}

if(SerialBT.available()){
delection = SerialBT.read();
switch (delection) {
case 70: // F FWD
SerialBT.println("FWD") ;
forward_step();
break;
case 76: // L LEFT
SerialBT.println("LEFT") ;
left_step();
break;
case 82: // R Right
SerialBT.println("RIGHT") ;
right_step();
break;
case 66: // B Back
SerialBT.println("BACK") ;
back_step();
break;
case 72: // H Left Arm
SerialBT.println("LEFT_Arm") ;
left_arm();
break;
case 77: // M Right Arm
SerialBT.println("RIGHT_Arm") ;
right_arm();
break;
case 68: //Data
SerialBT.println("DATA") ;
for (int i=0; i <=5; i++){
rs_gets_3c(ang_c);
ang_b[i] = atoi(ang_c);
if ((ang_b[i] >= 0) && (ang_b[i] <=180))
{
ang0[i] = ang1[i];
ang1[i] = ang_b[i];
}
}
servo_set();
break;
}
}
}




スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

スポンサーリンク