Example of controlling a toio Core Cube from a M5STickC

toio.ino

#include <M5StickC.h>
#include <BLEDevice.h>

// Define the position and orientation of the two points where your Core Cube will move
const unsigned int left = 98;
const unsigned int top = 142;
const unsigned int right = 402;
const unsigned int bottom = 358;
const unsigned int blockWH = 43;
const unsigned int startX = right - blockWH;
const unsigned int startY = top + blockWH;
const unsigned int startT = 180;
const unsigned int endX = left + blockWH;
const unsigned int endY = top + blockWH;
const unsigned int endT = 0;

// Sets the time for the timer here (1 minute in this example)
const unsigned long intervalTime = 1 * 60 * 1000;

// toio Core Cube Service UUID
static BLEUUID coreCubeServiceUUID("10B20100-5B3B-4571-9508-CF3EFCD7BBAE");

// toio Core Cube Motor Characteristic UUID
static BLEUUID coreCubeMotorCharacteristicUUID("10B20102-5B3B-4571-9508-CF3EFCD7BBAE");

static BLERemoteCharacteristic *pCoreCubeMotorChar;
static BLEAdvertisedDevice *myDevice;

bool atStartPoint = false;

unsigned long lastNotificationTime = 0;

enum
{
  SCANNING = 0,
  READY_TO_CONNECT,
  CONNECTED,
  RUNNING,
  DISCONNECTED,
  NO_CORE_CUBE,
};

int state = SCANNING;
int lastState = state;

class MyClientCallback : public BLEClientCallbacks
{
  void onConnect(BLEClient *client)
  {
    state = CONNECTED;
  }

  void onDisconnect(BLEClient *client)
  {
    state = DISCONNECTED;
  }
};

bool connectToCoreCube()
{
  BLEClient *pClient = BLEDevice::createClient();
  pClient->setClientCallbacks(new MyClientCallback());
  pClient->connect(myDevice);

  BLERemoteService *pRemoteService = pClient->getService(coreCubeServiceUUID);
  if (pRemoteService == nullptr)
  {
    pClient->disconnect();
    return false;
  }

  pCoreCubeMotorChar = pRemoteService->getCharacteristic(coreCubeMotorCharacteristicUUID);
  if (pCoreCubeMotorChar == nullptr)
  {
    pClient->disconnect();
    return false;
  }

  return true;
}

class MyAdvertisedDeviceCallback : public BLEAdvertisedDeviceCallbacks
{
  void onResult(BLEAdvertisedDevice advertisedDevice)
  {
    Serial.print("BLEAdvertisedDevice: ");
    Serial.println(advertisedDevice.toString().c_str());

    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(coreCubeServiceUUID))
    {
      BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      state = READY_TO_CONNECT;
    }
    else
    {
      state = NO_CORE_CUBE;
    }
  }
};

void setup()
{
  M5.begin();
  M5.Lcd.setRotation(1);
  M5.Lcd.setTextFont(2);

  Serial.begin(115200);

  BLEDevice::init("");

  BLEScan *pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallback());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.drawString("Turn on a Core Cube", 10, 10);

  pBLEScan->start(30);
}

void loop()
{
  M5.update();

  unsigned long now = millis();
  unsigned long elapsedTime = now - lastNotificationTime;

  switch (state)
  {
  case READY_TO_CONNECT:
    connectToCoreCube();
    break;

  case CONNECTED:
    if (M5.BtnA.wasPressed())
    {
      lastNotificationTime = now;
      moveTo(startX, startY, startT);
      atStartPoint = true;
      state = RUNNING;
    }
    break;

  case RUNNING:
    if (elapsedTime >= intervalTime)
    {
      if (atStartPoint)
      {
        moveTo(endX, endY, endT);
        atStartPoint = false;
      }
      else
      {
        moveTo(startX, startY, startT);
        atStartPoint = true;
      }
      lastNotificationTime = now;
    }
    break;

  default:
    break;
  }

  if (lastState != state)
  {
    M5.Lcd.fillScreen(BLACK);

    switch (state)
    {
    case READY_TO_CONNECT:
      M5.Lcd.drawString("Ready to connect", 10, 10);
      break;

    case CONNECTED:
      M5.Lcd.drawString("Connected", 10, 10);
      M5.Lcd.drawString("Press the A button", 10, 30);
      M5.Lcd.drawString("to start", 10, 50);
      break;

    case RUNNING:
      M5.Lcd.drawString("Running", 10, 10);
      break;

    case DISCONNECTED:
      M5.Lcd.drawString("Disconnected", 10, 10);
      break;

    case NO_CORE_CUBE:
      M5.Lcd.drawString("Couldn't find a Cube", 10, 10);
      break;

    default:
      break;
    }
    lastState = state;
  }

  delay(100);
}

void moveTo(unsigned int x, unsigned int y, unsigned int theta)
{
  uint8_t data[13] = {0};
  data[0] = 0x03; // Control type (motor control with target)
  data[1] = 0x00; // Control ID
  data[2] = 0x05; // Timeout
  data[3] = 0x00; // Move type
  data[4] = 0x50; // Max speed
  data[5] = 0x00; // Speed type
  data[6] = 0x00; // Reserved
  data[7] = lowByte(x);
  data[8] = highByte(x);
  data[9] = lowByte(y);
  data[10] = highByte(y);
  data[11] = lowByte(theta);
  data[12] = highByte(theta);

  pCoreCubeMotorChar->writeValue(data, sizeof(data));
}