move toio precisely with M5Stack. ToioMover class and example (Please use this branch https://github.com/mhama/M5StackToio/tree/feature/id_reader_sensor of M5StackToio)

M5StackToioTargetMover.ino

#include <M5Stack.h>
#include <Toio.h>
#include "ToioMover.h"

// Toio オブジェクト生成
Toio toio;
ToioMover *mover;

void setup() {
  // M5Stack の初期化
  M5.begin();
  M5.Power.begin();
  M5.Lcd.clear();

  Serial.println("- toio コア キューブをスキャンします。");
  std::vector<ToioCore*> toiocore_list = toio.scan(3);
  size_t n = toiocore_list.size();
  if (n == 0) {
    Serial.println("- toio コア キューブが見つかりませんでした。");
    return;
  }
  Serial.printf("- %d 個の toio コア キューブが見つかりました。\n", n);

  Serial.println("- toio コア キューブに BLE 接続します。");
  ToioCore *toiocore = toiocore_list.at(0);
  bool connected = toiocore->connect();
  if (!connected) {
    Serial.println("- BLE 接続に失敗しました。");
    return;
  }
  Serial.println("- BLE 接続に成功しました。");
  mover = new ToioMover(toiocore);

  //toiocore->disconnect();
  //Serial.println("- BLE コネクションを切断しました。");
}

void loop() {
  toio.loop();
  if (mover != NULL) {
    mover->loop();
    if (mover->getState() == ToioMover::StateIdle) {
      int targetX = random(120,360);
      int targetY = random(170,340);
      mover->setTarget(targetX, targetY);
    }
  }
  delay(1);
}

ToioMover.cpp

#include <M5Stack.h>
#include "ToioMover.h"

static float radToDegree(float rad) {
  return rad * 180.0f / 3.14159f;
}

static float degreeToRad(float degree) {
  return degree * 3.14159f / 180.0f;
}

ToioMover::ToioMover(ToioCore *toiocore)
: toiocore(toiocore)
{
    toiocore->onIDReaderData([this](ToioCoreIDData data) {
        this->onReceive(data);
    });
}

void ToioMover::drawToioPosition(int x, int y, int angleDegree, uint16_t color) {
  float scale = 1.0f;
  int centerX = (x-posXBase) * scale + 160;
  int centerY = (y-posYBase) * scale + 120;
  M5.Lcd.drawCircle(centerX, centerY, 20, color);
  float angleRad = angleDegree * 3.14159f / 180.0f;
  float lineLen = 60.0f;
  int lineX = cos(angleRad) * lineLen;
  int lineY = sin(angleRad) * lineLen;
  M5.Lcd.drawLine(centerX, centerY, centerX+lineX, centerY+lineY, color);
}

void ToioMover::drawTargetPosition(int x, int y, uint16_t color) {
  float scale = 1.0f;
  int centerX = (x-posXBase) * scale + 160;
  int centerY = (y-posYBase) * scale + 120;
  M5.Lcd.fillCircle(centerX, centerY, 10, color);
}

void ToioMover::stopMotor() {
  toiocore->controlMotor(true, 0, true, 0, 0);      
}

void ToioMover::runState() {
  float diffX = targetX-posX;
  float diffY = targetY-posY;
  float distance2 = diffX * diffX + diffY * diffY;
  float angleNormalized = (angle < 180) ? angle : angle - 360;
  float angleRad = angleNormalized * 3.14159f / 180.0f;
  float diffAngle = atan2(diffY, diffX) - angleRad;
  if (diffAngle > 3.14159f) {
    diffAngle -= 2.0f * 3.14159f;
  }
  if (diffAngle < -3.14159f) {
    diffAngle += 2.0f * 3.14159f;
  }
  float diffAngleAbs = abs(diffAngle);
  
  Serial.println("state:" + String(state));

  switch(state) {
  case StateIdle:
    if (distance2 > stopDistance2) {
      state = StateRotate;
      break;
    }
    break;
  case StateRotate:
    {
      if (distance2 < stopDistance2) {
        state = StateIdle;
        break;
      }
      float motorRotSpeed = min(8.0f + (diffAngleAbs * 5.0f), 15.f);
      Serial.println("angleRad:" + String(angleRad) + " diffAngle:" + String(diffAngle));
      if (diffAngleAbs <= degreeToRad(3.0f)) {
        stopMotor();
        state = StateMove;
        break;
      } else {
        toiocore->controlMotor(diffAngle > 0, motorRotSpeed, diffAngle < 0, motorRotSpeed, 0);
      }
      break;
    }
  case StateMove:
    {
      if (distance2 < stopDistance2) {
        stopMotor();
        state = StateIdle;
        break;
      }
      if (diffAngleAbs >= degreeToRad(7.0f)) {
        stopMotor();
        state = StateRotate;
        break;
      }
      float motorMoveSpeed = min(15.0f + (sqrt(distance2) * 1.0f), 40.f);
      float motorRotSpeed = diffAngle * 1.5f;
      toiocore->controlMotor(true, motorMoveSpeed + motorRotSpeed, true, motorMoveSpeed - motorRotSpeed, 0);
      break;
    }
  }
}

void ToioMover::onReceive(const ToioCoreIDData & data) {
  if (data.type == ToioCoreIDTypePosition) {
    Serial.println("posX: " + String(data.position.cubePosX) + " posY: " + String(data.position.cubePosY) + " angle: " + String(data.position.cubeAngleDegree));
    posX = data.position.cubePosX;
    posY = data.position.cubePosY;
    angle = data.position.cubeAngleDegree;
  }
  else if (data.type == ToioCoreIDTypeStandard) {
    Serial.println("Standard ID: " + String(data.standard.standardID));
  }
  else {
    Serial.println("no id found.");
  }
}

void ToioMover::loop()
{
  M5.Lcd.fillRect(0, 0, 320, 80, BLACK);
  M5.Lcd.setCursor(10, 10);
  M5.Lcd.print("target:("+String(targetX)+" ,"+String(targetY)+")");
  M5.Lcd.setCursor(10, 30);
  M5.Lcd.print("current:("+String(posX)+" ,"+String(posY)+")");

  // delete previous
  drawToioPosition(posXPrev, posYPrev, anglePrev, BLACK);
  drawTargetPosition(targetXPrev, targetYPrev, BLACK);

  // draw current
  drawToioPosition(posX, posY, angle, GREEN);
  drawTargetPosition(targetX, targetY, RED);
  
  posXPrev = posX;
  posYPrev = posY;
  anglePrev = angle;
  targetXPrev = targetX;
  targetYPrev = targetY;
  runState();
}

ToioMover::State ToioMover::getState() {
    return state;
}

void ToioMover::setTarget(int x, int y)
{
    targetX = x;
    targetY = y;
}

ToioMover.h

#include <M5Stack.h>
#include <Toio.h>

class ToioMover {
public:
    enum State {
        StateIdle,
        StateRotate,
        StateMove,
    };

private:
    State state = StateIdle;
    ToioCore *toiocore;

    int posX = 350;
    int posXPrev = 0;
    int posXBase = 250;
    int posY = 180;
    int posYBase = 250;
    int posYPrev = 0;
    int angle = 0;
    int anglePrev = 0;

    int targetX = 350;
    int targetY = 180;
    int targetXPrev = targetX;
    int targetYPrev = targetY;

    float stopDistance = 5.0f;
    float stopDistance2 = stopDistance * stopDistance;

public:
    ToioMover(ToioCore *toiocore);
    void loop();
    void setTarget(int x, int y);
    State getState();

private:
    void onReceive(const ToioCoreIDData & data);
    void runState();
    void drawToioPosition(int x, int y, int angleDegree, uint16_t color);
    void drawTargetPosition(int x, int y, uint16_t color);
    void stopMotor();
};