こちらの情報は古いです
ソフトウェアについてはこちらの記事の方が新しいのでこちらを参照してください
BalaC(M5StickC内蔵倒立振子)をPID制御で立たせてみた件について(製品版に更新) - Qiita
https://qiita.com/coppercele/items/527228e3f08c53597bd1
倒立振子を作ろうと思ったきっかけはこちらの記事を読んだことでした
実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」 - Qiita
https://qiita.com/Google_Homer/items/d0cdb2975491a5ea8129
構成もシンプルで自分もチャレンジしてみようと思いました
姿勢の推定
参考記事ではMPU6886の出力をいろいろと計算して現在の角度を推定しています
が自分は全然わからないのでMadgwickフィルタを使います
実は倒立振子といえばカルマンフィルタっていうのは噂で聞いていたのですが、
ライブラリを使うにしても事前計算があったりして結構めんどくさいんですよね
Twitterでカルマンフィルタより新しいMadgwickフィルタってのがあると言う噂を聞いていたので使ってみました
Madgwickフィルタライブラリを使う
Arduino IDEのライブラリマネージャで検索するとすぐに出てきます
追記：M5StickCライブラリにMadgwickフィルタを使用してる関数があり
M5.MPU6886.getAhrsData()を使うことで外部ライブラリが不要になりました
使い方は
#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);
}
なんとこれだけです
マッジで？！という声が出ました(これが言いたかっただけ)
M5StickCとmadgwickフィルタで倒立振子もどき作ってみたけどモーター繋いで制御となるとメチャメチャ大変なんだろうな(´・ω・`) pic.twitter.com/Hne90JoJ7f
— もけ@ムギ㌠ (@coppercele) February 9, 2020
値が収束するのに10秒ほどかかりますが重力加速度で補正するらしくて何もせずに簡易の角度計が作れちゃいます
ただし6軸センサの値を利用しているので重力に鉛直方向を軸(下図のZ軸)にしての回転方向(ヨー)はドリフトしっぱなしであてになりません
今回はロール軸(X軸)だけを使うので問題はありません
※こちらから引用
カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー - 電子工作 - HomeMadeGarbage
https://homemadegarbage.com/bala01
PID制御
最初は基準の角度に対してのずれに応じてモーターのパワーを上げたらいいんじゃね？(これはP制御)
くらいに思っていたらそんなに甘くはなかったのでPID制御を実装します
参考にした記事の方ではMPU6886の値から角速度などを得てPID制御してました
今回はMadgwickフィルタを使っていて現在の角度しかわからないのでロール角のみを使った処理に書き直します
Arduinoで簡単なPID制御のプログラムを作ってみる – 自作のいろいろ
https://garchiving.com/pid-control-in-arduino/
こちらを参考にしてソースをアレンジします
dt = (micros() - preTime) / 1000000; // 処理時間を求める
preTime = micros(); // 処理時間を記録
// 目標角度から現在の角度を引いて偏差を求める
// -90~90を取るので90で割って-1.0~1.0にする
P = (target - roll) / 90;
// 偏差を積分する
I += P * dt;
// 偏差を微分する
D = (P - preP) / dt;
// 偏差を記録する
preP = P;
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
if (100 < 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);
// モーターの出力を決定する(PWM8bitで0~255)
// 今使ってるギアボックスは100でやっと回り始めるのでオフセットを100にしている
Duty = (int)(130 * abs(power / 100) + 100);
// +-30度を越えたら倒れたとみなす
if (-30 < now && now < 30) {
ledcWrite( MOTOR_PWM_F,(power < 0 ? 0 : Duty) );
ledcWrite( MOTOR_PWM_R,(power < 0 ? Duty : 0) );
// Serial.printf("power= %3f\n", power);
}
else {
ledcWrite(MOTOR_PWM_F, 0);
ledcWrite(MOTOR_PWM_R, 0);
power = 0;
I = 0;
}
powerが-100~100で決まるのでそれを%として使用してDutyを決定します
倒立振子を構築する
基本的にはこちらの記事と同じですが手元に安いギアボックスがあったのでそれを使っています
ただ遊びが大きいのでキッチリ立たせたいなら元記事の遊びが少ないギアードモータを使うべきです
実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」 - Qiita
https://qiita.com/Google_Homer/items/d0cdb2975491a5ea8129
Amazon | Yeeco DC 電動モーター 3-6V デュアルシャフトギアード TT 磁気ギアボックスエンジン
https://www.amazon.co.jp/dp/B07H4KXNP4/
ちなみに自分のはこういう感じになっています
3Dプリントした白い台座にL字ピンヘッダを入れてG0とG26からPWMを出力してモータードライバを動かしています
逆側のギアボックス側面にM5StickCを貼り付けてGroveポートからモータードライバに接続するのもアリだと思います(重心も下がるし)
最初はこの配置でやってました
個体差があるので同じKp,Ki,Kdでは立たないと思いますが試行錯誤してください
自分の理解に基づいたPIDパラメータの設定法をメモしておきます
Kp：全体的なモータのパワーに効く
モーターの出力が足りなくて倒れるときはKpを上げる
Kpを上げると激しく往復するようになる(ハンチング)のでなるべく往復の周期を長くするようにする
Kd：傾斜角度が大きくなった時の反発力に効く
大きくすると激しく往復運動をするようになるので程々の周期に落ち着くようにする
Kd：激しく動いたときのブレーキの役割をする
Kpを上げて往復してるところに優しく効かせてやると往復が緩やかになってくる
Kdも上げ過ぎると反発力が強くなりすぎて往復するのでバランスが大事
Ki：バランスが崩れて全体が平行移動しだしたときに効く
全体が平行移動しだしたときに止める作用をする
#include <M5StickC.h>
#define MOTOR_PIN_F 26 // to DC Motor Driver FIN
#define MOTOR_PIN_R 0 // to DC Motor Driver RIN
#define MOTOR_PWM_F 0 // PWM CHANNEL
#define MOTOR_PWM_R 1 // PWM CHANNEL
float Kp = 142.0;
float Ki = 745.0;
float Kd = 2.0;
float target = -80.2;
float P, I, D, preP;
float power = 0;
float dt, preTime;
bool started = false;
Madgwick madgwick;
float roll, pitch, yaw;
float standard;
float now;
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 setup() {
Serial.begin(151200);
M5.begin();
M5.Lcd.setRotation(2);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Axp.ScreenBreath(8);
M5.MPU6886.Init();
pinMode(MOTOR_PIN_F, OUTPUT);
pinMode(MOTOR_PIN_R, OUTPUT);
ledcSetup(MOTOR_PWM_F, 312500, 8); //CHANNEL, FREQ, BIT
ledcSetup(MOTOR_PWM_R, 312500, 8);
ledcAttachPin(MOTOR_PIN_F, MOTOR_PWM_F);
ledcAttachPin(MOTOR_PIN_R, MOTOR_PWM_R);
pinMode(GPIO_NUM_10, OUTPUT);
digitalWrite(GPIO_NUM_10, HIGH);
drawScreen();
}
static int Duty;
bool led = false;
void loop() {
M5.update();
if ( M5.BtnB.wasReleased() ) {
// ボタンBを押すと再起動
esp_restart();
}
if ( M5.BtnA.wasReleased() ) {
// ボタンAを押して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(2);
if (!started) {
return;
}
now = target - roll;
dt = (micros() - preTime) / 1000000; // 処理時間を求める
preTime = micros(); // 処理時間を記録
// 目標角度から現在の角度を引いて偏差を求める
// -90~90を取るので90で割って-1.0~1.0にする
P = (target - roll) / 90;
// 偏差を積分する
I += P * dt;
// 偏差を微分する
D = (P - preP) / dt;
// 偏差を記録する
preP = P;
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
if (100 < abs(I * Ki)) {
I = 0;
led = !led;
// アンチワインドアップされたらLEDを点灯する
digitalWrite(GPIO_NUM_10, led ? LOW : HIGH);
// anti windup
}
// 出力を計算する
power = Kp * P + Ki * I + Kd * D;
power = constrain(power, -100, 100);
// モーターの出力を決定する(PWM8bitで0~255)
// 今使ってるギアボックスは100でやっと回り始めるのでオフセットを100にしている
Duty = (int)(130 * abs(power / 100) + 100);
// +-30度を越えたら倒れたとみなす
if (-30 < now && now < 30) {
ledcWrite( MOTOR_PWM_F,(power < 0 ? 0 : Duty) );
ledcWrite( MOTOR_PWM_R,(power < 0 ? Duty : 0) );
// Serial.printf("power= %3f\n", power);
}
else {
ledcWrite(MOTOR_PWM_F, 0);
ledcWrite(MOTOR_PWM_R, 0);
power = 0;
I = 0;
}
}
パラメータ調整前
立った！ #M5StickC が立った！#倒立振子 pic.twitter.com/XZmmdWiI5p
— もけ@ムギ㌠ (@coppercele) February 24, 2020
パラメータ調整後
#M5StickC #倒立振子 プログラムの見直しとパラメータ追い込みで1分越え達成＼(^o^)／
— もけ@ムギ㌠ (@coppercele) February 26, 2020
M5StickCはMadgwickフィルタで倒立振子の夢を見るか？https://t.co/QAAutbKWsn pic.twitter.com/c03O7vvCNT
課題
・PIDパラメータの単位がおかしいのを直したい
・たまにMPU6886の値がぶっ壊れることがある(直立してるはずなのに70度を表示してるとか)
・さらなるPIDパラメータの追い込み
おまけ、というか今回のオチ
倒立振子を作った人は全員思ったと思います
PIDパラメータを変更するたびにスケッチ書き込むのめんどくさい！
と・・
私も思ったのでESP NOWを使ってリアルタイムでパラメーターを変更する奴を作りました
M5Stackの真ん中を押すと項目を移動し、左右ボタンで値をM5StickCに送信してパラメーターを変更します
モーター動作中でも変更できるので動きの変化がはっきりわかって面白いですよ
基本的にはESP32のMaster,SlaveのサンプルをコピペしてM5シリーズ固有の処理を追加したものです
書いた人はC++が不自由なので変なところがあったら直して使ってください
動いてるから良しとします
M5Stack側ソース
#include <M5Stack.h>
#include <esp_now.h>
#include <WiFi.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 = 3;
float Ki = 1;
float Kd = 200;
float target = -87.0;
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");
}
}
// 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 setup() {
Serial.begin(115200);
//Set device in STA mode to begin with
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.begin();
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();
}
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(0,240 - CHAR_Y * 2);
M5.Lcd.printf("%s",isPaired ? "CONNECTED" : "NOT CONNECTED");
M5.Lcd.setCursor(0,240 - CHAR_Y);
M5.Lcd.print(" - down +");
}
uint8_t state = 0;
void loop() {
M5.update();
// In the loop we scan for slave
// If Slave is found, it would be populate in `slave` variable
// We will check if `slave` is defined and then we proceed further
if (slave.channel == CHANNEL) {
isPaired = manageSlave();
if (isPaired) {
if ( M5.BtnA.wasPressed() ) {
switch (indexY) {
case 0:
target -= 0.1;
drawScreen();
break;
case 1:
Kp -= 0.1;
if (Kp < 0) {
Kp = 0;
}
drawScreen();
break;
case 2:
Ki -= 0.1;
if (Ki < 0) {
Ki = 0;
}
drawScreen();
break;
case 3:
Kd -= 0.1;
if (Kd < 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");
}
}
if ( M5.BtnB.wasPressed() ) {
indexY++;
if (indexY == 4) {
indexY = 0;
}
drawScreen();
}
if ( M5.BtnC.wasPressed() ) {
switch (indexY) {
case 0:
target += 0.1;
drawScreen();
break;
case 1:
Kp += 0.1;
drawScreen();
break;
case 2:
Ki += 0.1;
drawScreen();
break;
case 3:
Kd += 0.1;
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(10);
// wait for 3seconds to run the logic again
}
M5StickC側ソース
#include <M5StickC.h>
#include <MadgwickAHRS.h>
#include <esp_now.h>
#include <WiFi.h>
#define CHANNEL 1
#define MOTOR_PIN_F 26 // to DC Motor Driver FIN
#define MOTOR_PIN_R 0 // to DC Motor Driver RIN
#define MOTOR_PWM_F 0 // PWM CHANNEL
#define MOTOR_PWM_R 1 // PWM CHANNEL
float Kp = 30;
float Ki = 0.0;
float Kd = 0.0;
float target = -91.7;
float P, I, D, preP;
float power = 0;
float dt, preTime;
bool started = false;
Madgwick madgwick;
float roll, pitch, yaw;
float standard;
float now;
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 setup() {
Serial.begin(151200);
M5.begin();
M5.Lcd.setRotation(2);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Axp.ScreenBreath(8);
M5.MPU6886.Init();
// madgwick.begin(100);
pinMode(MOTOR_PIN_F, OUTPUT);
pinMode(MOTOR_PIN_R, OUTPUT);
ledcSetup(MOTOR_PWM_F, 312500, 8); //CHANNEL, FREQ, BIT
ledcSetup(MOTOR_PWM_R, 312500, 8);
ledcAttachPin(MOTOR_PIN_F, MOTOR_PWM_F);
ledcAttachPin(MOTOR_PIN_R, MOTOR_PWM_R);
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);
}
static int Duty;
bool led = false;
void loop() {
M5.update();
if ( M5.BtnB.wasReleased() ) {
// ボタンBを押すと再起動
esp_restart();
}
if ( M5.BtnA.wasReleased() ) {
// ボタンAを押して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(2);
if (!started) {
return;
}
now = target - roll;
dt = (micros() - preTime) / 1000000; // 処理時間を求める
preTime = micros(); // 処理時間を記録
// 目標角度から現在の角度を引いて偏差を求める
// -90~90を取るので90で割って-1.0~1.0にする
P = (target - roll) / 90;
// 偏差を積分する
I += P * dt;
// 偏差を微分する
D = (P - preP) / dt;
// 偏差を記録する
preP = P;
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
if (100 < 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);
// モーターの出力を決定する(PWM8bitで0~255)
// 今使ってるギアボックスは100でやっと回り始めるのでオフセットを100にしている
Duty = (int)(130 * abs(power / 100) + 100);
// +-30度を越えたら倒れたとみなす
if (-30 < now && now < 30) {
ledcWrite( MOTOR_PWM_F,(power < 0 ? 0 : Duty) );
ledcWrite( MOTOR_PWM_R,(power < 0 ? Duty : 0) );
// Serial.printf("power= %3f\n", power);
}
else {
ledcWrite(MOTOR_PWM_F, 0);
ledcWrite(MOTOR_PWM_R, 0);
power = 0;
I = 0;
}
}