BalaC(M5StickC内蔵倒立振子)をPID制御で立たせる

BalaCとはM5StickCを使用したシンプルな倒立振子です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/4f293fcd-baef-965c-3854-82c8f3dcc36e.png)


BALA-C ミニセルフバランスカー - スイッチサイエンス
https://www.switch-science.com/catalog/6346/

いま在庫が復活してるのでチャンス！

もともとこちらの記事を参考にして倒立振子を作っていました。

実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」 - Qiita
https://qiita.com/Google_Homer/items/d0cdb2975491a5ea8129

↓自分が書いた記事
M5StickCはMadgwickフィルタで倒立振子の夢を見るか？ - Qiita
https://qiita.com/coppercele/items/e4d71537a386966338d0

**BalaC製品版を入手したので内容を更新しました**

###車輪を回してみる###

まずサンプルコードがないか探してみたのですがUI Flowのものしかみつからなかったので(今は多分ある)推測しながらゴリゴリ書いていきます

モータードライバがL9110S,I2C: 0x38とのことなので同じ構成のBeetleCのコードから持ってきたら動きました

```cpp


void setup() {
	Wire.begin(0, 26);
}

void wheel(uint8_t val) {
	Wire.beginTransmission(0x38);
	Wire.write(0x00);
	Wire.write(-val);
	Wire.endTransmission();
	Wire.beginTransmission(0x38);
	Wire.write(0x01);
	Wire.write(val);
	Wire.endTransmission();
}

void loop() {
    wheel(100);
    delay(1000);
    wheel(0);
    delay(1000);
    wheel(-100);
    delay(1000);
    wheel(0);
    delay(1000);
}

````

モーターが逆向きに配置されているので反転させて同時に動かしています


##姿勢の推定##

M5StickCライブラリのM5.MPU6886.getAhrsData(&pitch, &roll, &yaw)内で
Madgwickフィルタが実装されており簡単に姿勢の角度を取得できます

```cpp
#include <M5StickC.h>

float roll, pitch, yaw;

void setup() {
  M5.begin();
  M5.MPU6886.Init();
}

void loop() {
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
  Serial.printf("%5.1f,%5.1f,%5.1f\n",pitch,roll,yaw);
  delay(10);
}

```

倒立振子ではrollのみを使用します

###PID制御###
最初は基準の角度に対してのずれに応じてモーターのパワーを上げたらいいんじゃね？(これはP制御)
くらいに思っていたらそんなに甘くはなかったのでPID制御を実装します

Arduinoで簡単なPID制御のプログラムを作ってみる – 自作のいろいろ
https://garchiving.com/pid-control-in-arduino/

こちらを参考にしてソースをアレンジします

```cpp

	M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
	now = target - roll;
	dt = (micros() - preTime) / 1000000; // 処理時間を求める
	preTime = micros(); // 処理時間を記録

	if (-1 < now && now < 1) {
		// +-1以内は制御しない
		power = 0;
		P = 0;
               // Iを0にすると慣性力で倒れるのでIのみ維持する
               //I = 0;
		D = 0;
		wheel(0);
		return;
	}
	if (now < -40 || 40 < now) {
		// +-40度を超えたら倒れたとみなす
		power = 0;
		wheel(power);
		P = 0;
		I = 0;
		D = 0;
		return;
	}

	// 目標角度から現在の角度を引いて偏差を求める
	// pRatioで割ることでKp,Ki,Kdの量を調整できる
	P = (target - roll) / pRatio;
	// 偏差を積分する
	I += P * dt;
	// 偏差を微分する
	D = (P - preP) / dt;
	// 偏差を記録する
	preP = P;
	// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
	if (200 < abs(I * Ki)) {
		I = 0;
		led = !led;
		digitalWrite(GPIO_NUM_10, led ? LOW : HIGH);
		// anti windup
	}
	// 出力を計算する
	power = Kp * P + Ki * I + Kd * D;
	power = constrain(power, -100, 100);
	// モーターの出力を決定する
	wheel(power);


```

Kp：全体的なモータのパワーに効く
　　P制御だけで往復運動(ハンチング)する程度に上げる
　　なるべく周期が長くなるようになるべく少なめの方が良い

Ki：水平移動を止める
　　全体が平行移動しだしたときに止める作用をする
　　結構ガッツリ上げた方が良い

Kd：ハンチングに対するブレーキの役割をする
　　Kdを上げていくとハンチングがおだやかになっていくので少しずつ上げる
　　大きくしすぎると逆にハンチングが激しくなるので優しくブレーキをかけるように上げていく

運動エネルギーが蓄積してハンチングモードに入るのが一番まずいので
運動エネルギーを減衰させるべく値は全て最小限の方がいいです(多分)

###BalaCを立たせてみた###

<details><summary>BalaC.ino</summary>
<div>

```cpp:BalaC.ino

#include <M5StickC.h>

float Kp = 10.0;
float Ki = 0.0;
float Kd = 0.0;
float target = -98.9;
float P, I, D, preP;

float power = 0;
float dt, preTime;

bool started = false;

float roll, pitch, yaw;
float now;
bool led = false;

static float pRatio = 15.0;

void drawScreen() {
	M5.Lcd.setCursor(0, 0);
	M5.Lcd.printf("S:%6.1f\n", target);
	M5.Lcd.printf("roll:%6.1f\n", roll);
	M5.Lcd.printf("now:%4.1f\n", target - roll);
	M5.Lcd.printf("Power:%5.1f\n", power);
	M5.Lcd.printf("Kp:%5.1f\n", Kp);
	M5.Lcd.printf("Ki:%5.1f\n", Ki);
	M5.Lcd.printf("Kd:%6.3f\n", Kd);
	M5.Lcd.printf("Bat:%5.1fV\n", M5.Axp.GetBatVoltage());
	M5.Lcd.printf("Cur:%5.1f\n", M5.Axp.GetBatCurrent());
}

void wheel(uint8_t val) {
	Wire.beginTransmission(0x38);
	Wire.write(0x00);
	Wire.write(-val);
	Wire.endTransmission();
	Wire.beginTransmission(0x38);
	Wire.write(0x01);
	Wire.write(val);
	Wire.endTransmission();
}

void setup() {
	// put your setup code here, to run once:
	M5.begin();
	Wire.begin(0, 26);
	M5.Lcd.setRotation(2);
	M5.Lcd.fillScreen(BLACK);
	M5.Lcd.setTextSize(1);
	M5.Axp.ScreenBreath(8);
	M5.MPU6886.Init();
	wheel(0);
	pinMode(GPIO_NUM_10, OUTPUT);
	digitalWrite(GPIO_NUM_10, HIGH);

	drawScreen();
}

void loop() {

	M5.update();
	if (M5.BtnB.wasReleased()) {
		// ボタンBを押すと再起動
		esp_restart();
	}

	if (M5.BtnA.wasReleased()) {
		// BtnAを押して3秒後にモーター駆動開始
		Serial.println("BtnA.wasReleased() == TRUE");

		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("3");
		delay(1000);
		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("2");
		delay(1000);
		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("1");
		// ボタンを押したら直立させて待ち残り1秒になったら現在の角度を基準に設定する
		float temp = 0.0;
		for (int i = 0; i < 10; i++) {
			M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
			temp += roll;
			delay(10);
		}
		target = temp / 10.0;
		delay(800);
		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("0");
		preTime = micros();
		started = true;
	}
	M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);

	Serial.printf("%8.1f,%8.1f,%8.1f\n", roll, pitch, yaw);
	drawScreen();
	delay(1);
	if (!started) {
		return;
	}
	now = target - roll;
	dt = (micros() - preTime) / 1000000; // 処理時間を求める
	preTime = micros(); // 処理時間を記録

	if (-1 < now && now < 1) {
		// +-1以内は制御しない
		power = 0;
		P = 0;
               // Iを0にすると慣性力で倒れるのでIのみ維持する
               //I = 0;
		D = 0;
		wheel(0);
		return;
	}
	if (now < -40 || 40 < now) {
		// +-40度を超えたら倒れたとみなす
		power = 0;
		wheel(power);
		P = 0;
		I = 0;
		D = 0;
		return;
	}

	// 目標角度から現在の角度を引いて偏差を求める
	// pRatioで割ることでKp,Ki,Kdの量を調整できる
	P = (target - roll) / pRatio;
	// 偏差を積分する
	I += P * dt;
	// 偏差を微分する
	D = (P - preP) / dt;
	// 偏差を記録する
	preP = P;
	// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
	if (200 < abs(I * Ki)) {
		I = 0;
		led = !led;
		digitalWrite(GPIO_NUM_10, led ? LOW : HIGH);
		// anti windup
	}
	// 出力を計算する
	power = Kp * P + Ki * I + Kd * D;
	power = constrain(power, -100, 100);
	// モーターの出力を決定する
	wheel(power);

}

```

</div>
</details>

PIDパラメータを調整するとこんな感じに立ちます＼(＾o＾)／
この動画の時はKp30Ki700Kd1.0くらいだったと思います

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">なんかもう倒れないのは当たり前になったけど前後動は許容しつつパラメーターのバランスでどういう味付けにするかって感じになってきたかも・・・<br><br>PID制御だと有る程度の前後動は仕組み上どうしようもないね・・・(´・ω・`)<a href="https://twitter.com/hashtag/BalaC?src=hash&amp;ref_src=twsrc%5Etfw">#BalaC</a> <a href="https://t.co/IB9YzL3bM2">pic.twitter.com/IB9YzL3bM2</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1270778212696518656?ref_src=twsrc%5Etfw">June 10, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

###立たせるために重要なこと###

BalaCに限らず倒立振子を立たせるためにはPID制御のパラメータを細かく変更して試行錯誤しないといけません。
が、変更するごとにスケッチをコンパイルして書き込んでたら時間がいくらあっても足りません

なのでESP NowでM5StackとM5SticCを接続してパラメーターをリアルタイムに変更する奴を作りました

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">倒立振子のパラメータ調整はシビアだから(特にKd)だから値を変更するたびにコンパイルしてたら禿げるよねと言う動画<a href="https://twitter.com/hashtag/BalaC?src=hash&amp;ref_src=twsrc%5Etfw">#BalaC</a> <a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://t.co/EJEcNiB75y">pic.twitter.com/EJEcNiB75y</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1270022349232824321?ref_src=twsrc%5Etfw">June 8, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

ソースを置いておきますがC++がよくわからないので変なところがあったら直して使ってください

<details><summary>PendulumConfig.ino</summary>
<div>

```cpp:PendulumConfig.ino

#include <M5Stack.h>
#include <esp_now.h>
#include <WiFi.h>
#include <M5Stack.h>
#include <M5StackUpdater.h>

// Global copy of slave
esp_now_peer_info_t slave;
#define CHANNEL 3
#define PRINTSCANRESULTS 0
#define DELETEBEFOREPAIR 0

#define CHAR_X 10
#define CHAR_Y 18

float Kp = 200;
float Ki = 0;
float Kd = 0;
float target = -97.5;

int indexY = 0;

bool isPaired = false;

// Init ESP Now with fallback
void InitESPNow() {
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  }
  else {
    Serial.println("ESPNow Init Failed");
    // Retry InitESPNow, add a counte and then restart?
    // InitESPNow();
    // or Simply Restart
    ESP.restart();
  }
}

// Scan for slaves in AP mode
void ScanForSlave() {
  int8_t scanResults = WiFi.scanNetworks();
  // reset on each scan
  bool slaveFound = 0;
  memset(&slave, 0, sizeof(slave));

  Serial.println("");
  if (scanResults == 0) {
    Serial.println("No WiFi devices in AP Mode found");
  } else {
    Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices ");
    for (int i = 0; i < scanResults; ++i) {
      // Print SSID and RSSI for each device found
      String SSID = WiFi.SSID(i);
      int32_t RSSI = WiFi.RSSI(i);
      String BSSIDstr = WiFi.BSSIDstr(i);

      if (PRINTSCANRESULTS) {
        Serial.print(i + 1);
        Serial.print(": ");
        Serial.print(SSID);
        Serial.print(" (");
        Serial.print(RSSI);
        Serial.print(")");
        Serial.println("");
      }
      delay(10);
      // Check if the current device starts with `Slave`
      if (SSID.indexOf("Slave") == 0) {
        // SSID of interest
        Serial.println("Found a Slave.");
        Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
        // Get BSSID => Mac Address of the Slave
        int mac[6];
        if ( 6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x",  &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5] ) ) {
          for (int ii = 0; ii < 6; ++ii ) {
            slave.peer_addr[ii] = (uint8_t) mac[ii];
          }
        }

        slave.channel = CHANNEL; // pick a channel
        slave.encrypt = 0; // no encryption

        slaveFound = 1;
        // we are planning to have only one slave in this example;
        // Hence, break after we find one, to be a bit efficient
        break;
      }
    }
  }

  if (slaveFound) {
    Serial.println("Slave Found, processing..");
  } else {
    Serial.println("Slave Not Found, trying again.");
  }

  // clean up ram
  WiFi.scanDelete();
}

// Check if the slave is already paired with the master.
// If not, pair the slave with master
bool manageSlave() {
  if (slave.channel == CHANNEL) {
    if (DELETEBEFOREPAIR) {
      deletePeer();
    }

//    Serial.print("Slave Status: ");
    // check if the peer exists
    bool exists = esp_now_is_peer_exist(slave.peer_addr);
    if ( exists) {
      // Slave already paired.
//      Serial.println("Already Paired");
      return true;
    } else {
      // Slave not paired, attempt pair
      esp_err_t addStatus = esp_now_add_peer(&slave);
      if (addStatus == ESP_OK) {
        // Pair success
        Serial.println("Pair success");
        isPaired = true;
        drawScreen();
        return true;
      } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
        // How did we get so far!!
        Serial.println("ESPNOW Not Init");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
        Serial.println("Invalid Argument");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
        Serial.println("Peer list full");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
        Serial.println("Out of memory");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
        Serial.println("Peer Exists");
        return true;
      } else {
        Serial.println("Not sure what happened");
        return false;
      }
    }
  } else {
    // No slave found to process
    Serial.println("No Slave found to process");
    return false;
  }
}

void deletePeer() {
  esp_err_t delStatus = esp_now_del_peer(slave.peer_addr);
  Serial.print("Slave Delete Status: ");
  if (delStatus == ESP_OK) {
    // Delete success
    Serial.println("Success");
  } else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) {
    // How did we get so far!!
    Serial.println("ESPNOW Not Init");
  } else if (delStatus == ESP_ERR_ESPNOW_ARG) {
    Serial.println("Invalid Argument");
  } else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) {
    Serial.println("Peer not found.");
  } else {
    Serial.println("Not sure what happened");
  }
}

uint8_t data;

void sendData() {

  const uint8_t *peer_addr = slave.peer_addr;

  esp_err_t result;
  if(data) {
    data = 0;
    Serial.print("Sending: "); Serial.println("OFF");
    result = esp_now_send(peer_addr, (uint8_t*)"OFF", sizeof((uint8_t*)"OFF"));
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.println("Sent: OFF");
  }
  else {
    data = 1;
    Serial.print("Sending: "); Serial.println("ON");
    result = esp_now_send(peer_addr, (uint8_t*)"ON", sizeof((uint8_t*)"ON"));
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.println("Sent: ON");
  }

//  data = (uint8_t*)"ON";
  
  Serial.print("Send Status: ");
  if (result == ESP_OK) {
    Serial.println("Success");
  } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
    // How did we get so far!!
    Serial.println("ESPNOW not Init.");
  } else if (result == ESP_ERR_ESPNOW_ARG) {
    Serial.println("Invalid Argument");
  } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
    Serial.println("Internal Error");
  } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
    Serial.println("ESP_ERR_ESPNOW_NO_MEM");
  } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
    Serial.println("Peer not found.");
  } else {
    Serial.println("Not sure what happened");
  }
}

// callback when data is sent from Master to Slave
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print("Last Packet Sent to: "); Serial.println(macStr);
  Serial.print("Last Packet Send Status: "); Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

void drawScreen() {
  M5.Lcd.fillScreen(BLACK);

  M5.Lcd.drawString(">",0,indexY * CHAR_Y);
  M5.Lcd.setCursor(3 * CHAR_X,0);
  M5.Lcd.printf("Target:%4.1f",target);
  M5.Lcd.setCursor(3 * CHAR_X,CHAR_Y *1);
  M5.Lcd.printf("Kp:%4.1f",Kp);
  M5.Lcd.setCursor(3 * CHAR_X,CHAR_Y *2);
  M5.Lcd.printf("Ki:%4.1f",Ki);
  M5.Lcd.setCursor(3 * CHAR_X,CHAR_Y *3);
  M5.Lcd.printf("Kd:%4.1f",Kd);
  M5.Lcd.setCursor(3 * CHAR_X,CHAR_Y *4);
  M5.Lcd.printf("Zero reset");
  M5.Lcd.setCursor(0,240 - CHAR_Y * 2);
  M5.Lcd.printf("%s",isPaired ? "CONNECTED" : "NOT CONNECTED");
  
  M5.Lcd.setCursor(0,240 - CHAR_Y);
  M5.Lcd.print("     -     down      +");
}

void setup() {
  Serial.begin(115200);
  M5.begin();
  if(digitalRead(BUTTON_A_PIN) == 0) {
    Serial.println("Will Load menu binary");
    updateFromFS(SD);
    ESP.restart();
  }
  WiFi.mode(WIFI_STA);
  Serial.println("ESPNow/Basic/Master Example");
  // This is the mac address of the Master in Station Mode
  Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
  // Init ESPNow with a fallback logic
  InitESPNow();
  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  esp_now_register_send_cb(OnDataSent);
  ScanForSlave();
  pinMode(GPIO_NUM_10, OUTPUT); // internal LED
  digitalWrite(GPIO_NUM_10, HIGH);
  M5.Lcd.setRotation(1);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setBrightness(64);
  M5.Lcd.fillScreen(BLACK);
  File file2 = SD.open("/param.txt", FILE_READ);
  String str = "";

  while (file2.available()) {
      str += char(file2.read());
  }

  if (str.length() != 0) {
    int start = 0;
    target = str.substring(start,str.indexOf(",")).toFloat();
    start = str.indexOf(",") + 1;
  
    Kp = str.substring(start,str.indexOf(",",start)).toFloat();
    start = str.indexOf(",", start) + 1;
    Ki = str.substring(start,str.indexOf(",",start)).toFloat();
    start = str.indexOf(",", start) + 1;
    Kd = str.substring(start).toFloat();
    Serial.printf("%.1f,%.1f,%.1f,%.1f\n",target,Kp,Ki,Kd);
    file2.close();
  }

  drawScreen();    
}


uint8_t state = 0;


void loop() {
  M5.update();
  
  if (slave.channel == CHANNEL) {
    isPaired = manageSlave();
    if (isPaired) {
      if ( M5.BtnA.isPressed() ) {

        switch (indexY) {
          case 0:
            target -= 0.1;
            drawScreen();
            break;
          case 1:
            Kp -= 1;
            if (Kp < 0) {
              Kp = 0;
            }
            drawScreen();
            break;
          case 2:
            Ki -= 1;
            if (Ki < 0) {
              Ki = 0;
            }
            drawScreen();
            break;
          case 3:
            Kd -= 0.1;
            if (Kd < 0) {
              Kd = 0;
            }
            drawScreen();
            break;
          case 4:
            Kp = 0;
            Ki = 0;
            Kd = 0;
            drawScreen();
            break;
        }
        File file = SD.open("/param.txt",FILE_WRITE);
        file.printf("%.1f,%.1f,%.1f,%.1f",target,Kp,Ki,Kd);
        Serial.printf("Wrote %.1f,%.1f,%.1f,%.1f\n",target,Kp,Ki,Kd);
        file.close();
        Serial.print("Sending: ");

        String str = "";
        str += String(target);
        str += ",";
        str += String(Kp);
        str += ",";
        str += String(Ki);
        str += ",";
        str += String(Kd);
        Serial.printf("Send: %s\n",str.c_str());
        esp_err_t result;
        const uint8_t *peer_addr = slave.peer_addr;
              result = esp_now_send(peer_addr, (uint8_t*)str.c_str(), str.length());
        Serial.print("Send Status: ");
        if (result == ESP_OK) {
          Serial.println("Success");
        } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
          // How did we get so far!!
          Serial.println("ESPNOW not Init.");
        } else if (result == ESP_ERR_ESPNOW_ARG) {
          Serial.println("Invalid Argument");
        } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
          Serial.println("Internal Error");
        } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
          Serial.println("ESP_ERR_ESPNOW_NO_MEM");
        } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
          Serial.println("Peer not found.");
        } else {
          Serial.println("Not sure what happened");
        }

//        Serial.println("BtnA.wasPressed() == TRUE");
//        sendData();
//        if (data) {
//          digitalWrite(GPIO_NUM_10, LOW);
//        }
//        else {
//          digitalWrite(GPIO_NUM_10, HIGH);
//        }
      }
      if ( M5.BtnB.wasPressed() ) {
        indexY++;
        if (indexY == 5) {
          indexY = 0;
        }
        drawScreen();
      }
      if ( M5.BtnC.isPressed() ) {
        switch (indexY) {
          case 0:
            target += 0.1;
            drawScreen();
            break;
          case 1:
            Kp += 1;
            drawScreen();
            break;
          case 2:
            Ki += 1;
            drawScreen();
            break;
          case 3:
            Kd += 0.1;
            drawScreen();
            break;
          case 4:
            Kp = 0;
            Ki = 0;
            Kd = 0;
            drawScreen();
            break;

        }
        File file = SD.open("/param.txt",FILE_WRITE);
        file.printf("%.1f,%.1f,%.1f,%.1f",target,Kp,Ki,Kd);
        Serial.printf("Wrote %.1f,%.1f,%.1f,%.1f\n",target,Kp,Ki,Kd);
        file.close();
        Serial.print("Sending: ");

        String str = "";
        str += String(target);
        str += ",";
        str += String(Kp);
        str += ",";
        str += String(Ki);
        str += ",";
        str += String(Kd);
        Serial.printf("Send: %s\n",str.c_str());
        esp_err_t result;
        const uint8_t *peer_addr = slave.peer_addr;
              result = esp_now_send(peer_addr, (uint8_t*)str.c_str(), str.length());
        Serial.print("Send Status: ");
        if (result == ESP_OK) {
          Serial.println("Success");
        } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
          // How did we get so far!!
          Serial.println("ESPNOW not Init.");
        } else if (result == ESP_ERR_ESPNOW_ARG) {
          Serial.println("Invalid Argument");
        } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
          Serial.println("Internal Error");
        } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
          Serial.println("ESP_ERR_ESPNOW_NO_MEM");
        } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
          Serial.println("Peer not found.");
        } else {
          Serial.println("Not sure what happened");
        }
      }

    } else {
      ScanForSlave();
    }
  }
  else {
    ScanForSlave();
  }
  delay(30);

  // wait for 3seconds to run the logic again
}

```

</div>
</details>

<details><summary>BalaC.ino</summary>
<div>

```cpp:BalaC.ino
#include <M5StickC.h>
#include <esp_now.h>
#include <WiFi.h>

#define CHANNEL 1

float Kp = 10.0;
float Ki = 0.0;
float Kd = 0.0;
float target = -98.9;
float P, I, D, preP;

float power = 0;
float dt, preTime;

bool started = false;

float roll, pitch, yaw;
float now;
bool led = false;

static float pRatio = 15.0;

void InitESPNow() {
	WiFi.disconnect();
	if (esp_now_init() == ESP_OK) {
		Serial.println("ESPNow Init Success");
	} else {
		Serial.println("ESPNow Init Failed");
		// Retry InitESPNow, add a counte and then restart?
		// InitESPNow();
		// or Simply Restart
		ESP.restart();
	}
}

// config AP SSID
void configDeviceAP() {
	const char *SSID = "Slave_1";
	bool result = WiFi.softAP(SSID, "Slave_1_Password", CHANNEL, 0);
	if (!result) {
		Serial.println("AP Config failed.");
	} else {
		Serial.println(
				"AP Config Success. Broadcasting with AP: " + String(SSID));
	}
}

void drawScreen() {
	M5.Lcd.setCursor(0, 0);
	M5.Lcd.printf("S:%6.1f\n", target);
	M5.Lcd.printf("roll:%6.1f\n", roll);
	M5.Lcd.printf("now:%4.1f\n", target - roll);
	M5.Lcd.printf("Power:%5.1f\n", power);
	M5.Lcd.printf("Kp:%5.1f\n", Kp);
	M5.Lcd.printf("Ki:%5.1f\n", Ki);
	M5.Lcd.printf("Kd:%6.3f\n", Kd);
	M5.Lcd.printf("Bat:%5.1fV\n", M5.Axp.GetBatVoltage());
	M5.Lcd.printf("Cur:%5.1f\n", M5.Axp.GetBatCurrent());
}

void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
	char macStr[18];
	snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
			mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4],
			mac_addr[5]);
	Serial.print("Last Packet Recv from: ");
	Serial.println(macStr);
	Serial.print("Last Packet Recv Data: ");
	Serial.println(*data);
	Serial.println("");

//  Serial.printf("Received: %s\n",(const char *)data);

	String str = "";
	for (int i = 0; i < data_len; i++) {
		str += String(((const char*) data)[i]);
	}

	Serial.printf("str: %s\n", str.c_str());

	int start = 0;
	target = str.substring(start, str.indexOf(",")).toFloat();
	start = str.indexOf(",") + 1;

	Kp = str.substring(start, str.indexOf(",", start)).toFloat();
	start = str.indexOf(",", start) + 1;
	Ki = str.substring(start, str.indexOf(",", start)).toFloat();
	start = str.indexOf(",", start) + 1;
	Kd = str.substring(start).toFloat();
	//    Serial.printf("t=%.1f, p=%.1f, i=%.1f, d=%.1f\n",t,p,i,d);
	Serial.printf("%.1f,%.1f,%.1f,%.1f\n", target, Kp, Ki, Kd);
}

void wheel(uint8_t val) {
	Wire.beginTransmission(0x38);
	Wire.write(0x00);
	Wire.write(-val);
	Wire.endTransmission();
	Wire.beginTransmission(0x38);
	Wire.write(0x01);
	Wire.write(val);
	Wire.endTransmission();
}

void setup() {
	// put your setup code here, to run once:
	M5.begin();
	Wire.begin(0, 26);
	M5.Lcd.setRotation(2);
	M5.Lcd.fillScreen(BLACK);
	M5.Lcd.setTextSize(1);
	M5.Axp.ScreenBreath(8);
	M5.MPU6886.Init();
	wheel(0);
	pinMode(GPIO_NUM_10, OUTPUT);
	digitalWrite(GPIO_NUM_10, HIGH);

	drawScreen();
	WiFi.mode(WIFI_AP);
	// configure device AP mode
	configDeviceAP();
	// This is the mac address of the Slave in AP Mode
	Serial.print("AP MAC: ");
	Serial.println(WiFi.softAPmacAddress());
	// Init ESPNow with a fallback logic
	InitESPNow();
	// Once ESPNow is successfully Init, we will register for recv CB to
	// get recv packer info.
	esp_now_register_recv_cb(OnDataRecv);

}

void loop() {

	M5.update();
	if (M5.BtnB.wasReleased()) {
		// ボタンBを押すと再起動
		esp_restart();
	}

	if (M5.BtnA.wasReleased()) {
		// BtnAを押して3秒後にモーター駆動開始
		Serial.println("BtnA.wasReleased() == TRUE");

		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("3");
		delay(1000);
		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("2");
		delay(1000);
		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("1");
		// ボタンを押したら直立させて待ち残り1秒になったら現在の角度を基準に設定する
		float temp = 0.0;
		for (int i = 0; i < 10; i++) {
			M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
			temp += roll;
			delay(10);
		}
		target = temp / 10.0;
		delay(800);
		M5.Lcd.setCursor(0, 0);
		M5.Lcd.print("0");
		preTime = micros();
		started = true;
	}
	M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);

	Serial.printf("%8.1f,%8.1f,%8.1f\n", roll, pitch, yaw);
	drawScreen();
	delay(1);
	if (!started) {
		return;
	}
	now = target - roll;
	dt = (micros() - preTime) / 1000000; // 処理時間を求める
	preTime = micros(); // 処理時間を記録

	if (-1 < now && now < 1) {
		// +-1以内は制御しない
		power = 0;
		P = 0;
               // Iを0にすると慣性力で倒れるのでIのみ維持する
               //I = 0;
		D = 0;
		wheel(0);
		return;
	}
	if (now < -40 || 40 < now) {
		// +-40度を超えたら倒れたとみなす
		power = 0;
		wheel(power);
		P = 0;
		I = 0;
		D = 0;
		return;
	}

	// 目標角度から現在の角度を引いて偏差を求める
	// pRatioで割ることでKp,Ki,Kdの量を調整できる
	P = (target - roll) / pRatio;
	// 偏差を積分する
	I += P * dt;
	// 偏差を微分する
	D = (P - preP) / dt;
	// 偏差を記録する
	preP = P;
	// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
	if (200 < abs(I * Ki)) {
		I = 0;
		led = !led;
		digitalWrite(GPIO_NUM_10, led ? LOW : HIGH);
		// anti windup
	}
	// 出力を計算する
	power = Kp * P + Ki * I + Kd * D;
	power = constrain(power, -100, 100);
	// モーターの出力を決定する
	wheel(power);

	// wheel(50);
	// delay(1000);
	// wheel(0);
	// delay(500);

}

```

</div>
</details>