LoginSignup
33
27

More than 3 years have passed since last update.

@coppercele(Moke Nakamura)

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

Last updated at Posted at 2020-03-30

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

image.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のコードから持ってきたら動きました



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フィルタが実装されており簡単に姿勢の角度を取得できます

#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/

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


    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を立たせてみた

BalaC.ino
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);

}


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

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

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

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

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

PendulumConfig.ino
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
}


BalaC.ino
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);

}


33
27
2

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
33
27