M5stickC+ジョイスティックユニットで、BALA-C ミニセルフバランスカーを操作




https://twitter.com/shirox22/status/1429378676433375233?s=20&t=ho7PKHjOWAmCMlvOrpQcvw

M5stickC+ジョイスティックユニットでPCマウスを作ったのですが，それを使って試しにM5stackのバランスカーを操作してみようと思いました．

ＰＣマウス作成はこちら↓
https://qiita.com/shirox22/items/9ce30d7ef57bd78839bd

BALA-Cの本体側コードは，Kiraku-Labo さん，@copperceleさんのコードを参考にしました．

BLEでのマウス操作のloopと，MPU6886での倒立制御のloopを，同時に処理する必要があるため，マルチタスク機能を使う必要があります． 

void task0(void* pvParameters) 
の方に，倒立制御のloopを入れ込み，

void loop()
の方に，bluetoothのloopを入れこみました．

マルチタスクの必要性に気づかず，ここにかなりハマってしまったので，参考にしてみてください．


<details><summary>BALA-C本体側のコードを見る</summary><div>

```
// BalaC balancing robot (IMU:MPU6886)

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <M5StickC.h>

#define LED 10
#define N_CAL1 100
#define N_CAL2 100
#define LCDV_MID 60

boolean serialMonitor = true;
boolean standing = false;
int16_t counter = 0;
uint32_t time0 = 0, time1 = 0;
int16_t counterOverPwr = 0, maxOvp = 20;
float power, powerR, powerL, yawPower;
float varAng, varOmg, varSpd, varDst, varIang;
float gyroXoffset, gyroYoffset, gyroZoffset, accXoffset;
float gyroXdata, gyroYdata, gyroZdata, accXdata, accZdata;
float aveAccX = 0.0, aveAccZ = 0.0, aveAbsOmg = 0.0;
float cutoff = 0.1; //~=2 * pi * f (Hz)
const float clk = 0.01; // in sec,
const uint32_t interval = (uint32_t) (clk * 1000); // in msec
float  Kang, Komg, KIang, Kyaw, Kdst, Kspd;
int16_t maxPwr;
float yawAngle = 0.0;
float moveDestination, moveTarget;
float moveRate = 0.0;
const float moveStep = 0.2 * clk;
int16_t fbBalance = 0;
int16_t motorDeadband = 0;
float mechFactR, mechFactL;
int8_t motorRDir = 0, motorLDir = 0;
bool spinContinuous = false;
float spinDest, spinTarget, spinFact = 1.0;
float spinStep = 0.0; //deg per 10msec
int16_t ipowerL = 0, ipowerR = 0;
int16_t motorLdir = 0, motorRdir = 0; //0:stop 1:+ -1:-
float vBatt, voltAve = 3.7;
int16_t punchPwr, punchPwr2, punchDur, punchCountL = 0, punchCountR = 0;
byte demoMode = 0;

// UUID HID
static BLEUUID serviceUUID("1812");
// UUID Report Charcteristic
static BLEUUID charUUID("2a4d");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;

static BLEAdvertisedDevice *myDevice;

std::vector<BLEAdvertisedDevice*> myDevices;

static void notifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify) {
  // ボタンを押すなどしてNotifyが実行されると呼び出される

  const int INDEX_LEVER_UPDOWN = 2;
  const int INDEX_LEVER_LEFTRIGHT = 1;

  const uint8_t LEVER_UP = 0xFC;
  const uint8_t LEVER_DOWN = 0x04;
  const uint8_t LEVER_RIGHT = 0x04;
  const uint8_t LEVER_LEFT = 0xFC;

if (pData[INDEX_LEVER_UPDOWN] == LEVER_UP) {
    printf("UP\n");
    M5.Lcd.drawCentreString("    UP    ", 80, 30, 4);

    moveRate = -2.0;
    spinContinuous = true;
    spinStep = 0.0 * clk;

  }
  else if (pData[INDEX_LEVER_UPDOWN] == LEVER_DOWN) {
    printf("DOWN\n");
    M5.Lcd.drawCentreString("  DOWN  ", 80, 30, 4);

    moveRate = 2.0;
    spinContinuous = true;
    spinStep = 0.0 * clk;

  }

if (pData[INDEX_LEVER_LEFTRIGHT] == LEVER_RIGHT) {
    printf("RIGHT\n");
    M5.Lcd.drawCentreString("RIGHT", 80, 30, 4);
    moveRate = -1.0;
    spinContinuous = true;
    spinStep = -40.0 * clk;

  }
else if (pData[INDEX_LEVER_LEFTRIGHT] == LEVER_LEFT) {
    printf("LEFT\n");
    M5.Lcd.drawCentreString("  LEFT  ", 80, 30, 4);
    moveRate = -1.0;
    spinContinuous = true;
    spinStep = 40.0 * clk;

  }

if (pData[1]==0 && pData[2]==0) {
printf("STOP\n");
M5.Lcd.drawCentreString("  STOP  ", 80, 30, 4);
  moveRate = 0.0;
    spinContinuous = true;
    spinStep = 0.0 * clk;
}

}

class MyClientCallback: public BLEClientCallbacks {
    void onConnect(BLEClient *pclient) {
    }

    void onDisconnect(BLEClient *pclient) {
      connected = false;
      Serial.println("onDisconnect");
    }
};

bool connectToServer() {

  Serial.printf("myDevices=%d\n", myDevices.size());
  for (int i = 0; i < myDevices.size(); i++) {
    Serial.print("Forming a connection to ");
    Serial.println(myDevices.at(i)->getAddress().toString().c_str());

    BLEClient *pClient = BLEDevice::createClient();
    Serial.println(" - Created client");

    pClient->setClientCallbacks(new MyClientCallback());

    // Connect to the remove BLE Server.
    pClient->connect(myDevices.at(i));  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
    Serial.println(" - Connected to server");


    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService *pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID: ");
      Serial.println(serviceUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our service");

    // 複数のCharacteristicを持つHIDの事は考慮されていないので
    // 複数のCharacteristicに対応した処理をする
    std::map<uint16_t, BLERemoteCharacteristic*> *pCharacteristicMap;
    pCharacteristicMap = pRemoteService->getCharacteristicsByHandle();
    // 引数なしのgetCharacteristics()は複数のCharacteristicを返してこないのでこちらを利用する
    if (pCharacteristicMap == nullptr) {
      Serial.println("pCharacteristicMap=null");
      return false;
    }

    for (auto itr = pCharacteristicMap->begin(); itr != pCharacteristicMap->end(); ++itr) {
      Serial.print("UUID: ");
      Serial.println(itr->second->getUUID().toString().c_str());
      if (itr->second->getUUID().equals(charUUID)) {
        Serial.print("CharUUID matched: ");
        Serial.println(itr->second->getUUID().toString().c_str());
        if (itr->second->canNotify()) {
          // ReportCharacteristicsはNotifyCallbackでデータを通信するため
          // Notify属性を持っているCharacteristicをすべて登録する
          Serial.print("Notify registerd: ");
          Serial.println(itr->second->getUUID().toString().c_str());
          itr->second->registerForNotify(notifyCallback);
        }
      }
    }


  }
  connected = true;
  return true;
}
/**
   Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    /**
       Called for each advertising BLE server.
    */
    void onResult(BLEAdvertisedDevice advertisedDevice) {
      Serial.print("BLE Advertised Device found: ");
      Serial.println(advertisedDevice.toString().c_str());

      // We have found a device, let us now see if it contains the service we are looking for.
      if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

        //      BLEDevice::getScan()->stop();
        myDevice = new BLEAdvertisedDevice(advertisedDevice);
        myDevices.push_back(myDevice);
        doConnect = true;
        doScan = true;

      } // Found our server
    } // onResult
};

void task0(void* pvParameters) {

  while (true) {
    checkButtonP();
#ifdef DEBUG
    if (debugLoop1()) return;
#endif
    getGyro();
    if (!standing) {
      dispBatVolt();
      aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
      aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
      M5.Lcd.setCursor(10, 130);
      M5.Lcd.printf("%5.2f   ", -aveAccZ);
      if (abs(aveAccZ) > 0.9 && aveAbsOmg < 1.5) {
        calib2();
        if (demoMode == 1) startDemo();
        standing = true;
      }
    }
    else {
      if (abs(varAng) > 30.0 || counterOverPwr > maxOvp) {
        resetMotor();
        resetVar();
        standing = false;
        setMode(false);
      }
      else {
        drive();
      }
    }
    counter += 1;
    if (counter >= 100) {
      counter = 0;
      dispBatVolt();
      if (serialMonitor) sendStatus();
    }
    do time1 = millis();
    while (time1 - time0 < interval);
    time0 = time1;

  }
  delay(1);
}

void setup() {
  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);
  Serial.begin(115200);
  M5.begin();
  Wire.begin(0, 26); //SDA,SCL
  imuInit();
  M5.Axp.ScreenBreath(11);
  M5.Lcd.setRotation(2);
  M5.Lcd.setTextFont(4);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  resetMotor();
  resetPara();
  resetVar();
  calib1();
#ifdef DEBUG
  debugSetup();
#else
  setMode(false);
#endif

  BLEDevice::init("");
  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan *pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);

  xTaskCreatePinnedToCore(task0, "bleTask", 8192, NULL, 10, NULL, 1);
}

void loop() {
  // If the flag "doConnect" is true then we have scanned for and found the desired
  // BLE Server with which we wish to connect.  Now we connect to it.  Once we are
  // connected we set the connected flag to be true.
  if (doConnect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the BLE Server.");
    }
    else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    doConnect = false;
  }

  // If we are connected to a peer BLE Server, update the characteristic each time we are reached
  // with the current time since boot.
  if (connected) {
    // String newValue = "Time since boot: " + String(millis()/1000);
    // Serial.println("Setting new characteristic value to \"" + newValue + "\"");

    // // Set the characteristic's value to be the array of bytes that is actually a string.
    // pRemoteCharacteristic->writeValue(newValue.c_str(), newValue.length());
  }
  else if (doScan) {
    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
  }

  delay(1); // Delay a second between loops.
}

void calib1() {
  calDelay(30);
  digitalWrite(LED, LOW);
  calDelay(80);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, LCDV_MID);
  M5.Lcd.print(" Cal-1  ");
  gyroYoffset = 0.0;
  for (int i = 0; i < N_CAL1; i++) {
    readGyro();
    gyroYoffset += gyroYdata;
    delay(9);
  }
  gyroYoffset /= (float)N_CAL1;
  M5.Lcd.fillScreen(BLACK);
  digitalWrite(LED, HIGH);
}

void calib2() {
  resetVar();
  resetMotor();
  digitalWrite(LED, LOW);
  calDelay(80);
  M5.Lcd.setCursor(0, LCDV_MID);
  M5.Lcd.println(" Cal-2  ");
  accXoffset = 0.0;
  gyroZoffset = 0.0;
  for (int i = 0; i < N_CAL2; i++) {
    readGyro();
    accXoffset += accXdata;
    gyroZoffset += gyroZdata;
    delay(9);
  }
  accXoffset /= (float)N_CAL2;
  gyroZoffset /= (float)N_CAL2;
  M5.Lcd.fillScreen(BLACK);
  digitalWrite(LED, HIGH);
}

void checkButtonP() {
  byte pbtn = M5.Axp.GetBtnPress();
  if (pbtn == 2) calib1(); //short push
  else if (pbtn == 1) setMode(true); //long push
}

void calDelay(int n) {
  for (int i = 0; i < n; i++) {
    getGyro();
    delay(9);
  }
}

void setMode(bool inc) {
  if (inc) demoMode = ++demoMode % 2;
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(5, 5);
  if (demoMode == 0) M5.Lcd.print("Stand ");
  else if (demoMode == 1) M5.Lcd.print("Demo ");
}

void startDemo() {
  moveRate = 1.0;
  spinContinuous = true;
  spinStep = -40.0 * clk;
}

void resetPara() {
  Kang = 37.0;
  Komg = 0.84;
  KIang = 800.0;
  Kyaw = 4.0;
  Kdst = 85.0;
  Kspd = 2.7;
  mechFactL = 0.45;
  mechFactR = 0.45;
  punchPwr = 20;
  punchDur = 1;
  fbBalance = -3;
  motorDeadband = 10;
  maxPwr = 120;
  punchPwr2 = max(punchPwr, motorDeadband);
}

void getGyro() {
  readGyro();
  varOmg = (gyroYdata - gyroYoffset); //unit:deg/sec
  yawAngle += (gyroZdata - gyroZoffset) * clk; //unit:g
  varAng += (varOmg + ((accXdata - accXoffset) * 57.3 - varAng) * cutoff ) * clk; //complementary filter
}

void readGyro() {
  float gX, gY, gZ, aX, aY, aZ;
  M5.Imu.getGyroData(&gX, &gY, &gZ);
  M5.Imu.getAccelData(&aX, &aY, &aZ);
  gyroYdata = gX;
  gyroZdata = -gY;
  gyroXdata = -gZ;
  accXdata = aZ;
  accZdata = aY;
}

void drive() {
#ifdef DEBUG
  debugDrive();
#endif
  if (abs(moveRate) > 0.1) spinFact = constrain(-(powerR + powerL) / 10.0, -1.0, 1.0); //moving
  else spinFact = 1.0; //standing
  if (spinContinuous) spinTarget += spinStep * spinFact;
  else {
    if (spinTarget < spinDest) spinTarget += spinStep;
    if (spinTarget > spinDest) spinTarget -= spinStep;
  }
  moveTarget += moveStep * (moveRate + (float)fbBalance / 100.0);
  varSpd += power * clk;
  varDst += Kdst * (varSpd * clk - moveTarget);
  varIang += KIang * varAng * clk;
  power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
  if (abs(power) > 1000.0) counterOverPwr += 1;
  else counterOverPwr = 0;
  if (counterOverPwr > maxOvp) return;
  power = constrain(power, -maxPwr, maxPwr);
  yawPower = (yawAngle - spinTarget) * Kyaw;
  powerR = power - yawPower;
  powerL = power + yawPower;

  ipowerL = (int16_t) constrain(powerL * mechFactL, -maxPwr, maxPwr);
  int16_t mdbn = -motorDeadband;
  int16_t pp2n = -punchPwr2;
  if (ipowerL > 0) {
    if (motorLdir == 1) punchCountL = constrain(++punchCountL, 0, 100);
    else punchCountL = 0;
    motorLdir = 1;
    if (punchCountL < punchDur) drvMotorL(max(ipowerL, punchPwr2));
    else drvMotorL(max(ipowerL, motorDeadband));
  }
  else if (ipowerL < 0) {
    if (motorLdir == -1) punchCountL = constrain(++punchCountL, 0, 100);
    else punchCountL = 0;
    motorLdir = -1;
    if (punchCountL < punchDur) drvMotorL(min(ipowerL, pp2n));
    else drvMotorL(min(ipowerL, mdbn));
  }
  else {
    drvMotorL(0);
    motorLdir = 0;
  }

  ipowerR = (int16_t) constrain(powerR * mechFactR, -maxPwr, maxPwr);
  if (ipowerR > 0) {
    if (motorRdir == 1) punchCountR = constrain(++punchCountR, 0, 100);
    else punchCountR = 0;
    motorRdir = 1;
    if (punchCountR < punchDur) drvMotorR(max(ipowerR, punchPwr2));
    else drvMotorR(max(ipowerR, motorDeadband));
  }
  else if (ipowerR < 0) {
    if (motorRdir == -1) punchCountR = constrain(++punchCountR, 0, 100);
    else punchCountR = 0;
    motorRdir = -1;
    if (punchCountR < punchDur) drvMotorR(min(ipowerR, pp2n));
    else drvMotorR(min(ipowerR, mdbn));
  }
  else {
    drvMotorR(0);
    motorRdir = 0;
  }
}

void drvMotorL(int16_t pwm) {
  drvMotor(0, (int8_t)constrain(pwm, -127, 127));
}

void drvMotorR(int16_t pwm) {
  drvMotor(1, (int8_t)constrain(-pwm, -127, 127));
}

void drvMotor(byte ch, int8_t sp) {
  Wire.beginTransmission(0x38);
  Wire.write(ch);
  Wire.write(sp);
  Wire.endTransmission();
}

void resetMotor() {
  drvMotorR(0);
  drvMotorL(0);
  counterOverPwr = 0;
}

void resetVar() {
  power = 0.0;
  moveTarget = 0.0;
  moveRate = 0.0;
  spinContinuous = false;
  spinDest = 0.0;
  spinTarget = 0.0;
  spinStep = 0.0;
  yawAngle = 0.0;
  varAng = 0.0;
  varOmg = 0.0;
  varDst = 0.0;
  varSpd = 0.0;
  varIang = 0.0;
}

void sendStatus () {
  Serial.print(millis() - time0);
  Serial.print(" stand="); Serial.print(standing);
  Serial.print(" accX="); Serial.print(accXdata);
  Serial.print(" power="); Serial.print(power);
  Serial.print(" ang="); Serial.print(varAng);
  Serial.print(", ");
  Serial.print(millis() - time0);
  Serial.println();
}

void imuInit() {
  M5.Imu.Init();
  if (M5.Imu.imuType = M5.Imu.IMU_MPU6886) {
    M5.Mpu6886.SetGyroFsr(M5.Mpu6886.GFS_250DPS); //250DPS 500DPS 1000DPS 2000DPS
    M5.Mpu6886.SetAccelFsr(M5.Mpu6886.AFS_4G); //2G 4G 8G 16G
    if (serialMonitor) Serial.println("MPU6886 found");
  }
  else if (serialMonitor) Serial.println("MPU6886 not found");
}

void dispBatVolt() {
  M5.Lcd.setCursor(5, LCDV_MID);
  vBatt = M5.Axp.GetBatVoltage();
  M5.Lcd.printf("%4.2fv ", vBatt);
}
```
</div></details>