自作メカナムホイール・ローバーM5Atomに換装

2020/06/13
10:21

自作メカナムホイール・ローバーM5Atomに換装

こんにちはRoboTakaoです。
以前、自作のメカナムホイールローバーでM5StickCを使って作ったことを紹介して、
私のロボットサイトでもまとめましたが、M5 UnitVを接続するにあたり
M5Atomに換装しました。
M5StickCだとGrove端子をサーボを駆動するために使ってしまっていたので
M5Atomに変更してGrove端子はM5 UnitVを接続する計画です。
とりあえず今回はM5StickCからM5Atomに感想です。

結線図

今回は秋付きで買った５V昇圧基板で５V生成したものを電源としています。
M5UnitVと接続するので付けてみました。後で不要なら削除したいと思います。

動作確認

いい感じで動いてくれます。

スケッチ

M5Atom固有のところ、例えばポート番号やLCDをLEDに変更などを行なっています。


#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 Srv_RF = 22, Srv_RR = 19, Srv_LF = 23, Srv_LR = 33; //GPIO No.
const uint8_t srvCH_RF = 0, srvCH_RR = 1, srvCH_LF = 2, srvCH_LR = 3; //チャンネル
const double PWM_Hz = 50;   //PWM周波数
const uint8_t PWM_level = 16; //PWM 16bit(0～65535)

int mRF = 0, mRR = 0, mLF = 0, mLR = 0;

int pulseMIN = 2295;  //700μsec 50Hz 16bit
int pulseMAX = 7533;  //2300μsec 50Hz 16bit

int cont_max = 100;
int cont_thres = 40;
int cont_thres2 = 10;

//右ジョイスティックのデータ受信
BLYNK_WRITE(V0) {
  int x = param[0].asInt();
  int y = param[1].asInt();

  if(abs(y) > cont_thres){
    if(x > cont_thres){  //前輪旋回
      mRF = y;
      mRR = 0;
      mLF = -y;
      mLR = 0;
    }else if(x < -cont_thres){  //後輪旋回
      mRF = 0;
      mRR = y;
      mLF = 0;
      mLR = -y;
    }else{  //超信地旋回
      mRF = y;
      mRR = y;
      mLF = -y;
      mLR = -y;
    }
  }else if(cont_thres2 < y && y < cont_thres){
    if(x > cont_thres){  //右前信地旋回
      mRF = x;
      mRR = x;
      mLF = 0;
      mLR = 0;
    }else if(x < -cont_thres){  //右信地旋回
      mRF = 0;
      mRR = 0;
      mLF = x;
      mLR = x;
    }
  }else if(-cont_thres < y && y < -cont_thres2){
    if(x > cont_thres){    //左前信地旋回
      mRF = 0;
      mRR = 0;
      mLF = x;
      mLR = x;
    }else if(x < -cont_thres){  //左後信地旋回
      mRF = x;
      mRR = x;
      mLF = 0;
      mLR = 0;
    }
  }else if(abs(y) <= cont_thres2 && abs(x)>cont_thres){  //前後進
    mRF = x;
    mRR = x;
    mLF = x;
    mLR = x;
  }else{
    mRF = 0;
    mRR = 0;
    mLF = 0;
    mLR = 0;
  }
}

//左ジョイスティックのデータ受信
BLYNK_WRITE(V1) {
  int x = param[0].asInt();
  int y = param[1].asInt();

  if(y < -cont_thres){
    if(x > cont_thres){  //右斜め前
      mRF = 0;
      mRR = -y;
      mLF = -y;
      mLR = 0;
    }else if(x < -cont_thres){  //右斜め後
      mRF = y;
      mRR = 0;
      mLF = 0;
      mLR = y;
    }else{  //右スライド
      mRF = y;
      mRR = -y;
      mLF = -y;
      mLR = y;
    }
  }else if(y > cont_thres){
    if(x > cont_thres){  //左斜め前
      mRF = y;
      mRR = 0;
      mLF = 0;
      mLR = y;
    }else if(x < -cont_thres){  //左斜め後
      mRF = 0;
      mRR = -y;
      mLF = -y;
      mLR = 0;
    }else{  //左スライド
      mRF = y;
      mRR = -y;
      mLF = -y;
      mLR = y;
    }
  }else if(abs(x) > cont_thres){   //前後進
    mRF = x;
    mRR = x;
    mLF = x;
    mLR = x;
  }else{
    mRF = 0;
    mRR = 0;
    mLF = 0;
    mLR = 0;
  }
}

void motor_drive(int motor_RF, int motor_RR, int motor_LF, int motor_LR){
    motor_RF = map(motor_RF, -cont_max, cont_max, pulseMIN, pulseMAX);
    motor_RR = map(motor_RR, -cont_max, cont_max, pulseMIN, pulseMAX);
    motor_LF = map(motor_LF, -cont_max, cont_max, pulseMIN, pulseMAX);
    motor_LR = map(motor_LR, -cont_max, cont_max, pulseMIN, pulseMAX);
  
    ledcWrite(srvCH_RF, motor_RF);
    ledcWrite(srvCH_RR, motor_RR);
    ledcWrite(srvCH_LF, motor_LF);
    ledcWrite(srvCH_LR, motor_LR);
}

void led_red(){
  M5.dis.drawpix(0, 0x000000); //なぜか最初のLEDが付かないためダミー
  for(int i=0; i<25; i++){
    M5.dis.drawpix(i, 0x00ff00); //red
  }
}

void led_green(){
  M5.dis.drawpix(0, 0x000000); //なぜか最初のLEDが付かないためダミー
  for(int i=0; i<25; i++){
    M5.dis.drawpix(i, 0xff0000); //green
  }
}

void setup() {
  Serial.begin(151200);
  M5.begin(true, false, true); // void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
  pinMode(Srv_RF, OUTPUT);
  pinMode(Srv_RR, OUTPUT);
  pinMode(Srv_LF, OUTPUT);
  pinMode(Srv_LR, OUTPUT);

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

  led_red();
  
  //モータのPWMのチャンネル、周波数の設定
  ledcSetup(srvCH_RF, PWM_Hz, PWM_level);
  ledcSetup(srvCH_RR, PWM_Hz, PWM_level);
  ledcSetup(srvCH_LF, PWM_Hz, PWM_level);
  ledcSetup(srvCH_LR, PWM_Hz, PWM_level);

  //モータのピンとチャンネルの設定
  ledcAttachPin(Srv_RF, srvCH_RF);
  ledcAttachPin(Srv_RR, srvCH_RR);
  ledcAttachPin(Srv_LF, srvCH_LF);
  ledcAttachPin(Srv_LR, srvCH_LR);
}

void loop() {
  M5.update();
  Blynk.run();
  
  motor_drive(-mRF, -mRR, mLF, mLR);
  delay(10);
}