PID制御のパラメータと自立のための目標角度（TARGET)をWebsocket UIでスマホから設定します。今のところ、パラメータの値が適切でないようで、硬い床面上では短時間で倒れます。やわらかい素材の上では倒れないので、この状態で適切なパラメータを試行錯誤で探索中、、、WebベースのUIのためか、細かなパラメータ設定の操作性がいまいちなので、改善が必要かもしれない。
倒立振子のコードは、https://qiita.com/Google_Homer/items/3897e7ffef9d247e2f56 を参考にしています。
Websocketのコードは、https://github.com/mgo-tec/ESP32_SPIFFS_EasyWebSocket を参考にしています。
#include <WiFi.h>
#include <WiFiMulti.h>
#include "ESP32_SPIFFS_EasyWebSocket.h" //beta ver 1.60
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
#include "I2Cdev.h"
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU_ADDRESS 0x68
#include "Wire.h"
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//PIDパラメータ調整
//PID係数
//#define TARGET 0.4
//#define KP 100.0
//#define KI 2.0
//#define KD 2.0
//Motor
#define LED_PIN 27 // 内蔵LED
#define MOTOR_PIN_F A4 // to DC Motor Driver FIN
#define MOTOR_PIN_R A18 // to DC Motor Driver RIN
#define MOTOR_PWM_F 0 // PWM CHANNEL
#define MOTOR_PWM_R 1 // PWM CHANNEL
#define MOTOR_POWER_MIN 50
int MOTOR_POWER_MAX = 200;
#define DPS 1000 // Gscale [deg/s]
const char* ssid = "＊＊＊＊＊＊＊"; //ご自分のルーターのSSIDに書き換えてください
const char* password = "＊＊＊＊＊＊"; //ご自分のルーターのパスワードに書き換えてください
const char* HTM_head_file1 = "/EWS/LIPhead1.txt"; //HTMLヘッダファイル1
const char* HTM_head_file2 = "/EWS/LIPhead2.txt"; //HTMLヘッダファイル2
const char* HTML_body_file = "/EWS/dummy.txt"; //HTML body要素ファイル（ここではダミーファイルとしておく）
const char* dummy_file = "/EWS/dummy.txt"; //HTMLファイル連結のためのダミーファイル
ESP32_SPIFFS_EasyWebSocket ews;
WiFiMulti wifiMulti;
IPAddress LIP; //ローカルIPアドレス自動取得用
String ret_str; //ブラウザから送られてくる文字列格納用
String txt = "text send?"; //ブラウザから受信した文字列を ESP32から再送信する文字列
int PingSendTime = 10000; //ESP32からブラウザへPing送信する間隔(ms)
long ESP32_send_LastTime;
int ESP32_send_Rate = 300;
byte cnt = 0;
//PID
float power = 0, I = 0, preP = 0, preTime;
float now = 0, Duty = 0, pitch, roll, yaw;
float KP = 100.0, KI = 2.0, KD = 2.0, TARGET = 0.0;
boolean f_disp = false;
void setup() {
Wire.begin();
Serial.begin(38400);
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(MPU6050_PWR_MGMT_1); //MPU6050_PWR_MGMT_1レジスタの設定
Wire.write(0x00);
Wire.endTransmission();
Serial.print(F("Connecting to "));
Serial.println(ssid);
wifiMulti.addAP(ssid, password);
Serial.println(F("Connecting Wifi..."));
if (wifiMulti.run() == WL_CONNECTED) {
Serial.println("");
Serial.println(F("WiFi connected"));
Serial.println(F("IP address: "));
LIP = WiFi.localIP(); //ESP32のローカルIPアドレスを自動取得
Serial.println(WiFi.localIP());
}
ews.EWS_server_begin();
Serial.println(); Serial.println("Initializing SPIFFS ...");
if (!SPIFFS.begin()) {
Serial.println("SPIFFS failed, or not present");
return;
}
Serial.println("SPIFFS initialized. OK!");
MadgwickFilter.begin(100); //100Hz
// SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
Serial.println(F("SSD1306 allocation failed"));
for (;;); // Don't proceed, loop forever
}
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
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);
// 初期化
delay(10);
// Clear the buffer
display.clearDisplay();
// テキストサイズを設定
display.setTextSize(1);
// テキスト色を設定
display.setTextColor(WHITE);
display.setCursor(20, 5);
display.println("Start...");
display.setCursor(20, 25);
display.println(WiFi.localIP());
display.display();
delay(10); // Pause for 2 seconds
preTime = micros();
// TaskHandle_t th; //ESP32 マルチタスク ハンドル定義
// xTaskCreatePinnedToCore(Task1, "Task1", 4096, NULL, 5, &th, 0); //マルチタスク core 0 実行
ESP32_send_LastTime = millis();
}
void loop() {
websocket_handshake();
if (ret_str != "_close") {
if (millis() - ESP32_send_LastTime > ESP32_send_Rate) {
if (cnt > 3) {
cnt = 0;
}
websocket_send(cnt, txt);
cnt++;
ESP32_send_LastTime = millis();
}
ret_str = ews.EWS_ESP32CharReceive(PingSendTime);
if (ret_str != "\0") {
Serial.println(ret_str);
if (ret_str != "Ping") {
if (ret_str[0] != 't') {
int ws_data = (ret_str[0] - 0x30) * 100 + (ret_str[1] - 0x30) * 10 + (ret_str[2] - 0x30);
switch (ret_str[4]) {
case '!':
ESP32_send_Rate = ws_data;
break;
case 'B':
TARGET = float(5.0 - ws_data / 20.0);
break;
case 'G':
KP = float(ws_data / 1.8) ;
break;
case 'R':
KI = float(ws_data / 50.0);
break;
case '_':
KD = float(ws_data / 50.0);
break;
case 'A':
f_disp = ! f_disp;
break;
case 'O':
KP = 100, KI = 2.0, KD = 2.0, TARGET = 0.0;
break;
}
} else if (ret_str[0] == 't') {
txt = ret_str.substring(ret_str.indexOf('|') + 1, ret_str.length() - 1);
Serial.println(txt);
}
}
}
} else if (ret_str == "_close") {
ESP32_send_LastTime = millis();
ret_str = "";
}
PID();
}
//************* 倒立振り子 ****************************************
void PID() {
float P, D, dt, Time;
static float powerI = 0;
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14, true);
while (Wire.available() < 14);
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
Temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
float acc_x = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
float acc_y = ayRaw / 16384.0;
float acc_z = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gyro_x = gxRaw / 131.0; // (度/s)
float gyro_y = gyRaw / 131.0;
float gyro_z = gzRaw / 131.0;
/*
//c.f. Madgwickフィルターを使わずに、PRY（pitch, roll, yaw）を計算
double roll = atan2(acc_y, acc_z) * RAD_TO_DEG;
double pitch = atan(-acc_x / sqrt(acc_y * acc_y + acc_z * acc_z)) * RAD_TO_DEG;
*/
//Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
//PRYの計算結果を取得する
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw();
now = TARGET - roll ; // 目標角度から現在の角度を引いて偏差を求める
if (f_disp) {
display.clearDisplay();
display.setCursor(20, 5);
display.println( roll);
display.setCursor(20, 25);
display.println( now);
display.display();
}
if (-20 < now && now < 20) {
Time = micros() ;
dt = (Time - preTime) / 1000000 ; // 処理時間を求める
preTime = Time ; // 処理時間を記録
P = now / 90 ; // -90~90→-1.0~1.0
I += P * dt ; // 偏差を積分する
D = (P - preP) / dt ; // 偏差を微分する
preP = P ; // 偏差を記録する
power += KP * P + KI * I + KD * D ; // 出力を計算する
if (power < -1) power = -1 ; // →-1.0~1.0
if (1 < power) power = 1 ;
//Motor駆動
Duty = (int)((MOTOR_POWER_MAX - MOTOR_POWER_MIN) * abs(power) + MOTOR_POWER_MIN);
ledcWrite( MOTOR_PWM_F, (power < 0 ? 0 : Duty) );
ledcWrite( MOTOR_PWM_R, (power < 0 ? Duty : 0) );
digitalWrite(LED_PIN, HIGH);
if (f_disp) {
display.clearDisplay();
display.setCursor(20, 5);
display.println( roll);
display.setCursor(20, 25);
display.println( Duty);
display.setCursor(20, 45);
display.println( power);
display.display();
}
} else { // 転倒したら停止
ledcWrite(MOTOR_PWM_F, 0);
ledcWrite(MOTOR_PWM_R, 0);
power = 0;
I = 0;
digitalWrite(LED_PIN, LOW);
}
}
//**************************************************************
void LED_PWM(byte Led_gr, byte channel, int data_i) {
Serial.println(data_i);
}
//*********************************************
void websocket_send(uint8_t count, String str_txt) {
String str, tmp;
//※WebSocketへのテキスト送信は110 byte 程度なので、全角３５文字程度に抑えること
tmp = "AGL=" + String(roll) + ",TGT=" + String(TARGET) + ",P=" + String(KP) + ",I=" + String(KI) + ",D=" + String(KD);
switch (cnt) {
case 0:
//str = str_txt;
str = tmp;
break;
case 1:
str = tmp;
break;
case 2:
str = tmp;
break;
case 3:
str = tmp;
break;
}
ews.EWS_ESP32_Str_SEND(str, "wroomTXT"); //ブラウザに文字列を送信
}
//************************* Websocket handshake **************************************
void websocket_handshake() {
if (ews.Get_Http_Req_Status()) { //ブラウザからGETリクエストがあったかどうかの判定
String html_str1 = "", html_str2 = "", html_str3 = "", html_str4 = "", html_str5 = "", html_str6 = "", html_str7 = "";
//※String変数一つにEWS_Canvas_Slider_T関数は２つまでしか入らない
html_str1 += "<body style='background:#000; color:#fff;'>\r\n";
html_str1 += "<font size=3>\r\n";
html_str1 += "ESP-WROOM-32(ESP32)\r\n";
html_str1 += "<br>\r\n";
html_str1 += "ESP32_SPIFFS_EasyWebSocket Beta1.60 Sample\r\n";
html_str1 += "</font><br>\r\n";
html_str1 += ews.EWS_BrowserSendRate();
html_str1 += "<br>\r\n";
html_str1 += ews.EWS_ESP32_SendRate("!esp32t-Rate");
html_str1 += "<br>\r\n";
html_str1 += ews.EWS_BrowserReceiveTextTag2("wroomTXT", "from ESP32 DATA", "#555", 20, "#00FF00");
html_str1 += "<br>\r\n";
html_str1 += ews.EWS_Status_Text2("WebSocket Status", "#555", 20, "#FF00FF");
html_str1 += "<br><br>\r\n";
html_str2 += ews.EWS_TextBox_Send("txt1", "Hello Easy WebSocket Beta1.60", "送信");
html_str2 += "<br><br>\r\n";
html_str2 += "SETTING \r\n";
html_str2 += ews.EWS_On_Momentary_Button("ALL", "ALL-ON", 80, 25, 15, "#000000", "#AAAAAA");
html_str2 += ews.EWS_On_Momentary_Button("OUT", "DEFALT", 80, 25, 15, "#FFFFFF", "#555555");
html_str2 += "<br>\r\n";
html_str3 += "<br>TGT_ :\r\n";
html_str3 += ews.EWS_Canvas_Slider_T("BLUE", 240, 40, "#777777", "#0000ff"); //CanvasスライダーはString文字列に２つまでしか入らない
html_str3 += "<br>PID P:\r\n";
html_str3 += ews.EWS_Canvas_Slider_T("GREEN", 250, 40, "#777777", "#00ff00"); //CanvasスライダーはString文字列に２つまでしか入らない
html_str4 += "<br>PID I:\r\n";
html_str4 += ews.EWS_Canvas_Slider_T("RED", 250, 40, "#777777", "#ff0000"); //CanvasスライダーはString文字列に２つまでしか入らない
html_str4 += "<br>PID D:\r\n";
html_str4 += ews.EWS_Canvas_Slider_T("_RGB", 250, 40, "#777777", "#ffff00");
html_str7 += "<br><br>\r\n";
html_str7 += ews.EWS_WebSocket_Reconnection_Button2("WS-Reconnect", "grey", 200, 40, "black" , 17);
html_str7 += "<br><br>\r\n";
html_str7 += ews.EWS_Close_Button2("WS CLOSE", "#bbb", 150, 40, "red", 17);
html_str7 += ews.EWS_Window_ReLoad_Button2("ReLoad", "#bbb", 150, 40, "blue", 17);
html_str7 += "</body></html>";
//WebSocket ハンドシェイク関数
ews.EWS_HandShake_main(3, HTM_head_file1, HTM_head_file2, HTML_body_file, dummy_file, LIP, html_str1, html_str2, html_str3, html_str4, html_str5, html_str6, html_str7);
}
}