BugCとJoyCで遊んでみた

BugCとJoyCを組み合わせてラジコンを作ってみます。

BugCとは

BugCはM5StickCを接続して使用する、ロボットベースです。ご利用には別途M5StickCが必要です。
本製品にはM5StickCは含まれませんので、ご注意ください。
BugCは四つのDCモーター、モータードライバー、二つのRGB LED、バッテリー、バッテリーホルダー、電源スイッチで構成されています。
スイッチサイエンスで買えます。
https://www.switch-science.com/catalog/6208/

JoyCとは

JoyCはM5StickC用に設計されたコントローラーモジュールです。ご利用には別途M5StickCが必要です。
本製品にはM5StickCは含まれませんので、ご注意ください。
M5StickCとはI2Cで接続し、M5StickCからデータを送信することができます。
スイッチサイエンスで買えます。
https://www.switch-science.com/catalog/6207/

やってみたこと

• JoyCを操作してBugCを動かす
• デバイス間はBluetoothで接続
• Bluetoothは簡単そうなSPP(Serial Port Profile)を使用
  

操作方法

• 左スティックで左前、左後のモーターを制御
• 右スティックで右前、右後のモーターを制御
• スティックはそれぞれ前に倒すと前進方向、後ろに倒すと後進方向に進む
• スティックの倒し加減によりスピードを調整

走らせてみた

まっすぐ走らなかった、、、。
軸ごとに係数とか与えてみたけどうまくいかず。接地面が小さすぎるせい？モーターの個体差？
ギヤとかかませて車輪とか付けてみてもいいかも。

ソースコード

Arduino IDEで開発
BugC側

BugC.ino

#include "M5StickC.h"
#include "bugC.h"
#include "BluetoothSerial.h"

#include "esp_bt_main.h"
#include "esp_bt_device.h"
#include "esp_gap_bt_api.h"
#include "esp_err.h"

int rotate = 0;
BluetoothSerial SerialBT;

void setup() 
{
    M5.begin();
    Wire.begin(0, 26, 400000);
    M5.Lcd.setTextColor(TFT_GREEN);
    M5.Lcd.setRotation(1);
    M5.Lcd.drawCentreString("BUGC SPP", 80, 30, 2);
    // if add battery, need increase charge current
    M5.Axp.SetChargeCurrent(CURRENT_360MA);
    // シリアル接続
    Serial.begin(115200);
    SerialBT.begin("ESP32BugC"); //Bluetooth device name
    Serial.println("The device started, now you can pair it with bluetooth!");
}

void loop() 
{
    M5.update();

    if (SerialBT.available()) {
      String rcvData = SerialBT.readStringUntil(';');
      Serial.print(rcvData);
      M5.Lcd.fillScreen(BLACK);
      M5.Lcd.setCursor(1, 0, 1);
      M5.Lcd.print("string: " + rcvData);

      int len = rcvData.length();
      M5.Lcd.setCursor(1, 10, 1);
      M5.Lcd.printf("length: %d", len);

      int idx = rcvData.indexOf(',');
      M5.Lcd.setCursor(1, 20, 1);
      M5.Lcd.printf("idx: %d", idx);
      if (idx != -1)
      {
        String firstStr = rcvData.substring(0, idx);
        M5.Lcd.setCursor(1, 30, 1);
        M5.Lcd.print(firstStr);

        String secondStr = rcvData.substring(idx+1, rcvData.length());
        M5.Lcd.setCursor(1, 40, 1);
        M5.Lcd.print(secondStr);

        int firstSpeed = firstStr.toInt();
        int secondSpeed = secondStr.toInt();

        BugCSetColor(0x100000, 0x001000);
        BugCSetAllSpeed(firstSpeed, -secondSpeed, firstSpeed, -secondSpeed);
      }
    }

    if(M5.BtnB.wasPressed())
    {
        BugCSetColor(0x000000, 0x000000);
        BugCSetAllSpeed(0, 0, 0, 0);
    }

    if(M5.BtnA.wasPressed())
    {
      initBluetooth();
    }

    delay(10);
}

bool initBluetooth()
{
  if(!btStart()) {
    Serial.println("Failed to initialize controller");
    return false;
  }
 
  if(esp_bluedroid_init() != ESP_OK) {
    Serial.println("Failed to initialize bluedroid");
    return false;
  }
 
  if(esp_bluedroid_enable() != ESP_OK) {
    Serial.println("Failed to enable bluedroid");
    return false;
  }

  Serial.println("Success to initialize controller");
  return true;
}

JoyC側

JoyC.ino

#ifndef _M5_STICK_C_
#define _M5_STICK_C_
#include <M5StickC.h>
#endif

#include <BluetoothSerial.h>
#include "JoyC.h"

JoyC joyc;
TFT_eSprite img = TFT_eSprite(&M5.Lcd);

BluetoothSerial SerialBT;

String MACadd = "XX:XX:XX:XX:XX:XX";
uint8_t address[6]  = {0xXX, 0xXX, 0xXX, 0xXX, 0xXX, 0xXX};
String name = "OBDII";
char *pin = "1234"; //<- standard pin would be provided by default
bool connected;

void setup() {
  M5.begin();
  Wire.begin(0, 26, 400000);
  img.createSprite(80, 160);

  Serial.begin(115200);
  SerialBT.begin("ESP32test", true); 
  Serial.println("The device started in master mode, make sure remote BT device is on!");
  M5.Lcd.printf("The device started in master mode, make sure remote BT device is on!");
  
  // connect(address) is fast (upto 10 secs max), connect(name) is slow (upto 30 secs max) as it needs
  // to resolve name to address first, but it allows to connect to different devices with the same name.
  // Set CoreDebugLevel to Info to view devices bluetooth address and device names
  //connected = SerialBT.connect(name);
  connected = SerialBT.connect(address);
  
  if(connected) {
    Serial.println("Connected Succesfully!");
    M5.Lcd.printf("Connected Succesfully!");
  } else {
    while(!SerialBT.connected(10000)) {
      Serial.println("Failed to connect. Make sure remote device is available and in range, then restart app."); 
      M5.Lcd.printf("Failed to connect. Make sure remote device is available and in range, then restart app.");
    }
  }
  // disconnect() may take upto 10 secs max
  if (SerialBT.disconnect()) {
    Serial.println("Disconnected Succesfully!");
  }
  // this would reconnect to the name(will use address, if resolved) or address used with connect(name/address).
  connected = SerialBT.connect();
  if(connected) {
    Serial.println("Connected Succesfully!");
    M5.Lcd.printf("Connected Succesfully!");
  }
  else
  {
      Serial.println("Failed to connect."); 
      M5.Lcd.printf("Failed to connect.");
  }
}

void loop() {
  char text_buff[100];
  
  M5.update();

  if (!connected)
  {
    delay(100);
    return;
  }
  
  img.fillSprite(TFT_BLACK);
  img.drawCentreString("angle left", 40, 6, 1);
  sprintf(text_buff, "%d", joyc.GetAngle(0));
  img.drawCentreString(text_buff, 40, 20, 1);

  img.drawCentreString("dis left", 40, 34, 1);
  sprintf(text_buff, "%d", joyc.GetDistance(0));
  img.drawCentreString(text_buff, 40, 48, 1);
  
  img.drawCentreString("angle right", 40, 62, 1);
  sprintf(text_buff, "%d", joyc.GetAngle(1));
  img.drawCentreString(text_buff, 40, 76, 1);
  
  img.drawCentreString("dis right", 40, 90, 1);
  sprintf(text_buff, "%d", joyc.GetDistance(1));
  img.drawCentreString(text_buff, 40, 104, 1);

  int leftAngle = joyc.GetAngle(0);
  int leftSpeed = joyc.GetDistance(0);
  int rightAngle = joyc.GetAngle(1);
  int rightSpeed = joyc.GetDistance(1);

  if (leftAngle >= 90 && leftAngle < 270)
  {
    leftSpeed = leftSpeed * -1;
  }
  if (rightAngle >= 90 && rightAngle < 270)
  {
    rightSpeed = rightSpeed * -1;
  }

  String sendStr = "";
  sendStr = String(leftSpeed) + "," + String(rightSpeed) + ";";

  img.drawCentreString("String", 40, 118, 1);
  img.drawCentreString(sendStr, 40, 132, 1);

  int len = sendStr.length();

  img.drawCentreString(String(len), 40, 146, 1);
  img.pushSprite(0, 0);

  byte byteBuff[len + 1];
  sendStr.getBytes(byteBuff, len + 1);
  SerialBT.write(byteBuff, len);

  if (Serial.available()) {
    SerialBT.write(Serial.read());
  }
  if (SerialBT.available()) {
    Serial.write(SerialBT.read());
  }
  
  delay(100);
}

「MACadd」にはBugCに繋げたM5StickCのMACアドレスを指定。