記事一覧

M5Atomで小さい二足歩行ロボット Blynkを使ってiPhoneでコントロール

M5Atomで小さい二足歩行ロボット Blynkを使ってiPhoneでコントロール



こんにちはRoboTakaoです

前回まででM5Atomを使った小さい二足歩行ロボットを作り
モーション作成してきましたが、今回はBlynkを使ってiPhoneでコントロールしてみました

NX13_26.jpg

前進、後進、右転回、左転回、右手上げ、左手上げをBlynkのボタンで制御します。

Blynk

NX13_27.png

NX13_28.png


動作確認

良い感じです。


スケッチ


#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include "M5Atom.h"
#include <BlynkSimpleEsp32_BLE.h>
#include <BLEDevice.h>
#include <BLEServer.h>

char auth[] = "XXXXXXXXXXXXXXXXXXXXXXXX"; //メールで送られるAuth Token

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=160;
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},
{0,0,0,0,0,0}};

int b_s[19][6]={
{0,0,0,0,0,0},
{0,0,-20,-15,0,0},
{0,0,-15,-15,0,0},
{0,-15,-15,-15,-15,20},
{0,-15,0,0,-15,20},
{0,-15,15,20,-15,20},
{0,-15,15,15,-15,20},
{0,0,15,15,0,0},
{0,15,15,15,15,-20},
{0,15,0,0,15,-20},
{0,15,-20,-15,15,-20},
{0,15,-15,-15,15,-20},
{0,0,-15,-15,0,0},
{0,-15,-15,-15,-15,20},
{0,-15,0,0,-15,20},
{0,-15,15,20,-15,20},
{0,-15,15,15,-15,20},
{0,0,15,15,0,0},
{0,0,0,0,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 face_right(){
face_clear();
M5.dis.drawpix(7, 0x00ff00);
M5.dis.drawpix(8, 0x00ff00);
M5.dis.drawpix(9, 0x00ff00);
M5.dis.drawpix(17, 0x0000ff);
M5.dis.drawpix(19, 0x0000ff);
}

void face_left(){
face_clear();
M5.dis.drawpix(5, 0x00ff00);
M5.dis.drawpix(6, 0x00ff00);
M5.dis.drawpix(7, 0x00ff00);
M5.dis.drawpix(15, 0x0000ff);
M5.dis.drawpix(17, 0x0000ff);
}

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

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();
}
delection = 0;
}

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();
}
delection = 0;
}

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

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

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

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

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);
}
}

BLYNK_WRITE(V0) {
int x = param.asInt();
if(x == 1){
delection = 70; //forward step
Serial.println("FWD");
}
}

BLYNK_WRITE(V1) {
int x = param.asInt();
if(x == 1){
delection = 66; //Back step
Serial.println("BACK");
}
}

BLYNK_WRITE(V2) {
int x = param.asInt();
if(x == 1){
delection = 82; //Right step
Serial.println("RIGHT STEP");
}
}

BLYNK_WRITE(V3) {
int x = param.asInt();
if(x == 1){
delection = 76; //Left step
Serial.println("LEFT STEP");
}
}

BLYNK_WRITE(V4) {
int x = param.asInt();
if(x == 1){
delection = 77; //Right Arm
Serial.println("RIGHT ARM");
}
}

BLYNK_WRITE(V5) {
int x = param.asInt();
if(x == 1){
delection = 72; //Left Arm
Serial.println("LEFT ARM");
}
}

void setup() {
Serial.begin(151200);
// void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
M5.begin(true, false, true);

Blynk.setDeviceName("Blynk");
Blynk.begin(auth);

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_center();

Initial_Value();
}

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

switch (delection) {
case 70: // F FWD
forward_step();
break;
case 76: // L LEFT
left_step();
break;
case 82: // R Right
right_step();
break;
case 66: // B Back
back_step();
break;
case 72: // H Left Arm
left_arm();
break;
case 77: // M Right Arm
right_arm();
break;
}
}

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

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

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