先日 ATOMS3 という製品がM5Stack社から発売されました。
[bc url=”https://docs.m5stack.com/en/core/AtomS3″]
M5ATOM MatrixのLEDマトリクスがLCD (128×128ピクセル)に変更され、コントローラとしてESP32-S3が採用されています。
この度 ATOMS3 を入手し姿勢制御モジュールを製作しましたので報告させていただきます。
ATOMS3
待ちわびた製品が到着。白くてカワイイ
コントローラがESP32-S3に変わったのでピン番号が変わっていますが、電源・GNDやIMUのI2Cピンなど構成はMatrixと同様です。
姿勢制御モジュールで味見
試しにM5ATOM Matrix を使って製作したSHISEIGYO-1 Jr. をATOMS3で試してみました。
ATOMS3のArduinoライブラリが用意されていましたが、この時点 (2022/12/28)ではコンパイルエラーで使用できませんでした。
ATOMS3ライブラリは使用せずにSHISEIGYO-1 Jr. のコードをそのまま使用し、対応するGPIOの番号を変えるのみで動作しました。
Arduino IDEでのボードは”ESP32S3 Dev Module”を選択。
ディスプレイはまだ駆動できていません。
もう１点 Matrixと比較してIMUセンサMPU6886の軸が異なっていました。
ATOM MatrixはLED基板の裏に実装されていましたが、ATOMS3は恐らく本体基板に普通に実装されてるようです (中みていないので確定ではないですが)。
傾きを算出する際に注意が必要となります。
ディスプレイ表示
ATOMS3には128×128のLCDが搭載されております。ピンアサインはパッケージに記載があるのですがドライバが不明で良くわからず。。
しかも専用ライブラリは現状使えないので途方に暮れていたのですが、Twitter上でM5GFXライブラリで表示可能との情報を得ました。
[bc url=”https://github.com/m5stack/M5GFX”]
早速 表示実験
M5GFXライブラリのおかげで何の設定もなしに簡単に表示できました。ありがたや。
小さいディスプレイですがコントラストも良く視野角も広くて素晴らしいです。
SHISEIGYO-1 Jr. S3
ディスプレイ表示ができるようになったので、姿勢制御モジュールを仕上げました。
SHISEIGYO-1 Jr. S3 爆誕
ということで ATOMS3 の白いボディに合わせて真っ白に仕上げてみました。
ディスプレイ表示もいい感じです♪
Arduino IDEコード
SHISEIGYO-1 Jr. のサンプルコードをベースにコーディングしました。
#include "MPU6886.h"
#include <Kalman.h>
#include "FastLED.h"
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include <M5GFX.h>
M5GFX lcd;
WebServer server(80);
const char ssid[] = "SHISEIGYO-1 Jr."; // SSID
const char pass[] = "password"; // password
const IPAddress ip(192, 168, 22, 10); // IPアドレス
const IPAddress subnet(255, 255, 255, 0); // サブネットマスク
#define ENC_A 5
#define ENC_B 6
#define brake 7
#define rote_pin 1
#define PWM_pin 2
#define button 41
MPU6886 IMU;
unsigned long oldTime = 0, loopTime, nowTime;
float dt;
volatile byte pos;
volatile int enc_count = 0;
float Kp = 1.8;
float Kd = 2;
float Kw = 0.4;
float IDRS = 0.6;
float getupRange = 0.1;
float injectRatio = 14;
int rotMaxL = 720;
int rotMaxR = 790;
int delayTime = 2;
int pwmDuty;
int GetUP = 0;
int GetUpCnt = 0;
float M;
float Aj = 0.0;
float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;
float theta_acc = 0.0;
float theta_dot = 0.0;
Kalman kalmanY;
float kalAngleY, kalAngleDotY;
Preferences preferences;
//加速度センサから傾きデータ取得 [deg]
float get_theta_acc() {
IMU.getAccelData(&accX,&accY,&accZ);
//傾斜角導出 単位はdeg
theta_acc = atan(-1.0 * accX / accZ) * 57.29578f;
return theta_acc;
}
//Y軸 角速度取得
float get_gyro_data() {
IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
theta_dot = gyroY;
return theta_dot;
}
//起き上がり
void getup(){
digitalWrite(brake, HIGH);
int rotMax;
//回転方向
if(kalAngleY < 0.0){
rotMax = rotMaxL;
digitalWrite(rote_pin, LOW);
GetUP = 1;
}else{
rotMax = rotMaxR;
digitalWrite(rote_pin, HIGH);
GetUP = 2;
}
for(int i = 1023; i >= rotMax; i--){
ledcWrite(0, i);
delay(5);
}
ledcWrite(0, rotMax);
delay(300);
if(kalAngleY > 0.0){
digitalWrite(rote_pin, LOW);
}else{
digitalWrite(rote_pin, HIGH);
}
}
//Core0
void display(void *pvParameters) {
for (;;){
lcd.setTextFont(4);
lcd.setCursor(10, 2);
lcd.printf("%+05.1f", kalAngleY);
int yy = map(kalAngleY, 20, -20, 0, 127);
yy = constrain(yy, 0, 127);
uint32_t colorY;
if(fabs(kalAngleY) <= 1.0){
colorY = 0x00FF00U;
}else if(fabs(kalAngleY) <= 15){
colorY = 0x0000FFU;
}else{
colorY = 0xFF0000U;
}
lcd.fillRect(yy - 2, 25, 5, 128, colorY);
delay(33);
lcd.clear();
disableCore0WDT();
}
}
//ブラウザ表示
void handleRoot() {
String temp ="<!DOCTYPE html> \n<html lang=\"ja\">";
temp +="<head>";
temp +="<meta charset=\"utf-8\">";
temp +="<title>SHISEIGYO-1 Jr.</title>";
temp +="<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">";
temp +="<style>";
temp +=".container{";
temp +=" max-width: 500px;";
temp +=" margin: auto;";
temp +=" text-align: center;";
temp +=" font-size: 1.2rem;";
temp +="}";
temp +="span,.pm{";
temp +=" display: inline-block;";
temp +=" border: 1px solid #ccc;";
temp +=" width: 50px;";
temp +=" height: 30px;";
temp +=" vertical-align: middle;";
temp +=" margin-bottom: 20px;";
temp +="}";
temp +="span{";
temp +=" width: 120px;";
temp +="}";
temp +="button{";
temp +=" width: 100px;";
temp +=" height: 40px;";
temp +=" font-weight: bold;";
temp +=" margin-bottom: 20px;";
temp +="}";
temp +="</style>";
temp +="</head>";
temp +="<body>";
temp +="<div class=\"container\">";
temp +="<h3>SHISEIGYO-1 Jr.</h3>";
//起き上がりボタン
temp +="<button type=\"button\" ><a href=\"/GetUp\">GetUp</a></button><br>";
//Kp
temp +="Kp<br>";
temp +="<a class=\"pm\" href=\"/KpM\">-</a>";
temp +="<span>" + String(Kp) + "</span>";
temp +="<a class=\"pm\" href=\"/KpP\">+</a><br>";
//Kd
temp +="Kd<br>";
temp +="<a class=\"pm\" href=\"/KdM\">-</a>";
temp +="<span>" + String(Kd) + "</span>";
temp +="<a class=\"pm\" href=\"/KdP\">+</a><br>";
//Kw
temp +="Kw<br>";
temp +="<a class=\"pm\" href=\"/KwM\">-</a>";
temp +="<span>" + String(Kw) + "</span>";
temp +="<a class=\"pm\" href=\"/KwP\">+</a><br>";
//Rot Max L
temp +="Rot Max L<br>";
temp +="<a class=\"pm\" href=\"/rotMaxLm\">-</a>";
temp +="<span>" + String(rotMaxL) + "</span>";
temp +="<a class=\"pm\" href=\"/rotMaxLp\">+</a><br>";
//Rot Max R
temp +="Rot Max R<br>";
temp +="<a class=\"pm\" href=\"/rotMaxRm\">-</a>";
temp +="<span>" + String(rotMaxR) + "</span>";
temp +="<a class=\"pm\" href=\"/rotMaxRp\">+</a><br>";
//IDRS
temp +="IDRS<br>";
temp +="<a class=\"pm\" href=\"/IDRSm\">-</a>";
temp +="<span>" + String(IDRS) + "</span>";
temp +="<a class=\"pm\" href=\"/IDRSp\">+</a><br>";
temp +="</div>";
temp +="</body>";
server.send(200, "text/HTML", temp);
}
void handleGetUp() {
handleRoot();
if(GetUP == 0){
getup();
}
}
void KpM() {
if(Kp >= 0.1){
Kp -= 0.1;
preferences.putFloat("Kp", Kp);
}
handleRoot();
}
void KpP() {
if(Kp <= 30){
Kp += 0.1;
preferences.putFloat("Kp", Kp);
}
handleRoot();
}
void KdM() {
if(Kd >= 0.1){
Kd -= 0.1;
preferences.putFloat("Kd", Kd);
}
handleRoot();
}
void KdP() {
if(Kd <= 30){
Kd += 0.1;
preferences.putFloat("Kd", Kd);
}
handleRoot();
}
void KwM() {
if(Kw >= 0.1){
Kw -= 0.1;
preferences.putFloat("Kw", Kw);
}
handleRoot();
}
void KwP() {
if(Kw <= 30){
Kw += 0.1;
preferences.putFloat("Kw", Kw);
}
handleRoot();
}
void handleRotMaxLm() {
if(rotMaxL >= 10){
rotMaxL -= 10;
preferences.putInt("rotMaxL", rotMaxL);
}
handleRoot();
}
void handleRotMaxLp() {
if(rotMaxL <= 1010){
rotMaxL += 10;
preferences.putInt("rotMaxL", rotMaxL);
}
handleRoot();
}
void handleRotMaxRm() {
if(rotMaxR >= 10){
rotMaxR -= 10;
preferences.putInt("rotMaxR", rotMaxR);
}
handleRoot();
}
void handleRotMaxRp() {
if(rotMaxR <= 1010){
rotMaxR += 10;
preferences.putInt("rotMaxR", rotMaxR);
}
handleRoot();
}
void IDRSm() {
if(IDRS >= 0.1){
IDRS -= 0.1;
preferences.putFloat("IDRS", IDRS);
}
handleRoot();
}
void IDRSp() {
if(IDRS <= 10){
IDRS += 0.1;
preferences.putFloat("IDRS", IDRS);
}
handleRoot();
}
void setup() {
Serial.begin(115200);
lcd.begin();
lcd.setTextColor(0xFFFFFFU, 0x000000U); //１つ目の引数が文字色、２つ目の引数が背景色
pinMode(ENC_A, INPUT);
pinMode(ENC_B, INPUT);
pinMode(brake, OUTPUT);
pinMode(button, INPUT);
attachInterrupt(ENC_A, ENC_READ, CHANGE);
attachInterrupt(ENC_B, ENC_READ, CHANGE);
IMU.Init();
//フルスケールレンジ
IMU.SetAccelFsr(IMU.AFS_2G);
IMU.SetGyroFsr(IMU.GFS_250DPS);
kalmanY.setAngle(get_theta_acc());
ledcSetup(0, 20000, 10);
ledcAttachPin(PWM_pin, 0);
pinMode(rote_pin, OUTPUT);
digitalWrite(brake, LOW);
preferences.begin("parameter", false);
//パラメータ初期値取得
rotMaxL = preferences.getInt("rotMaxL", rotMaxL);
rotMaxR = preferences.getInt("rotMaxR", rotMaxR);
Kp = preferences.getFloat("Kp", Kp);
Kd = preferences.getFloat("Kd", Kd);
Kw = preferences.getFloat("Kw", Kw);
IDRS = preferences.getFloat("IDRS", IDRS);
WiFi.softAP(ssid, pass); // SSIDとパスの設定
delay(100); // 追記：このdelayを入れないと失敗する場合がある
WiFi.softAPConfig(ip, ip, subnet); // IPアドレス、ゲートウェイ、サブネットマスクの設定
IPAddress myIP = WiFi.softAPIP(); // WiFi.softAPIP()でWiFi起動
server.on("/", handleRoot);
server.on("/GetUp", handleGetUp);
server.on("/KpP", KpP);
server.on("/KpM", KpM);
server.on("/KdP", KdP);
server.on("/KdM", KdM);
server.on("/KwP", KwP);
server.on("/KwM", KwM);
server.on("/rotMaxLm", handleRotMaxLm);
server.on("/rotMaxLp", handleRotMaxLp);
server.on("/rotMaxRm", handleRotMaxRm);
server.on("/rotMaxRp", handleRotMaxRp);
server.on("/IDRSp", IDRSp);
server.on("/IDRSm", IDRSm);
server.begin();
//回転位置検出 タスク
xTaskCreatePinnedToCore(
display
, "display" // A name just for humans
, 4096 // This stack size can be checked & adjusted by reading the Stack Highwater
, NULL
, 1 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
, NULL
, 0);
}
void loop() {
server.handleClient();
//オフセット再計算&起き上がり
if (digitalRead(button) == 0){
getup();
}
nowTime = micros();
loopTime = nowTime - oldTime;
oldTime = nowTime;
dt = (float)loopTime / 1000000.0; //sec
//モータの角速度算出
float theta_dotWheel = -1.0 * float(enc_count) * 3.6 / dt;
enc_count = 0;
//カルマンフィルタ 姿勢 傾き
kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt);
//カルマンフィルタ 姿勢 角速度
kalAngleDotY = kalmanY.getRate();
if(GetUP == 1 || GetUP == 2){
if(GetUP == 1 && kalAngleY >= getupRange){
digitalWrite(brake, LOW);
GetUP = 99;
}else if(GetUP == 2 && kalAngleY <= -getupRange){
digitalWrite(brake, LOW);
GetUP = 99;
}else{
if(GetUP == 1 && kalAngleY < 0 || GetUP == 2 && kalAngleY > 0){
ledcWrite(0, max(1023 - int(injectRatio * fabs(kalAngleY)), 0));
}else {
ledcWrite(0,511);
}
}
}else {
if (fabs(kalAngleY) < 1 && GetUP == 0){
GetUP = 80;
}
/*
Serial.print("Kp: ");
Serial.print(Kp);
Serial.print(", Kd: ");
Serial.print(Kd,3);
Serial.print(", Kw: ");
Serial.print(Kw, 3);
Serial.print(", kalAngleY: ");
Serial.print(kalAngleY);
*/
if(GetUP == 99 || GetUP == 80){
//ブレーキ
if(fabs(kalAngleY) > 15.0){
digitalWrite(brake, LOW);
Aj = 0.0;
GetUP = 0;
}else{
digitalWrite(brake, HIGH);
}
//モータ回転
if(GetUP == 80){
M = Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 20000.0;
GetUpCnt++;
if(GetUpCnt > 100){
GetUpCnt = 0;
GetUP = 99;
}
}else{
Aj += IDRS * theta_dotWheel / 1000000.0;
M = Kp * (kalAngleY + Aj) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0;
}
M = max(-1.0f, min(1.0f, M));
pwmDuty = 1023 * (1.0 - fabs(M));
//回転方向
if(M > 0.0){
digitalWrite(rote_pin, LOW);
ledcWrite(0, pwmDuty);
}else{
digitalWrite(rote_pin, HIGH);
ledcWrite(0, pwmDuty);
}
}
Serial.print(", loopTime: ");
Serial.print((float)loopTime / 1000.0);
delay(delayTime);
Serial.println("");
}
}
//ブラシレスモータエンコーダ出力 割り込み処理
//参考：https://jumbleat.com/2016/12/17/encoder_1/
void ENC_READ() {
byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A);
byte old = pos & B00000011;
byte dir = (pos & B00110000) >> 4;
if (cur == 3) cur = 2;
else if (cur == 2) cur = 3;
if (cur != old)
{
if (dir == 0)
{
if (cur == 1 || cur == 3) dir = cur;
} else {
if (cur == 0)
{
if (dir == 1 && old == 3) enc_count--;
else if (dir == 3 && old == 1) enc_count++;
dir = 0;
}
}
bool rote = 0;
if (cur == 3 && old == 0) rote = 0;
else if (cur == 0 && old == 3) rote = 1;
else if (cur > old) rote = 1;
pos = (dir << 4) + (old << 2) + cur;
}
}
ESP32-S3対応のためにGPIO番号を修正しています。
MPU6886.cppのI2Cピン番号も変更が必要です。 [ Wire1.begin(38,39,100000); ]
カルマンフィルタで誤差算出をするので、事前のIMUのオフセットの導出は必要ないことに気づいたので廃止しました。
ディスプレイはデュアルコアにしてcore0で表示させています。
モジュールの各変数はAPモードでESP32-S3とつなげて、ブラウザから調整できるようにしています。
調整した値は電源OFF後もフラッシュで記憶されます。
起き上がり動作ボタンも実装しています。
コードでは “SHISEIGYO-1 Jr.” にWiFi接続して (パスワード：”password”)、192.168.22.10にブラウザアクセスで設定画面が表示されます。
おわりに
ATOMS3を用いて姿勢制御モジュールの製作を楽しみました。
小型ですがきれいなディスプレイもついて しかも低価格なのでATOMS3は非常におすすめの製品です。
まだライブラリが整ていないようで、ツギハギのコードになってしまいましたが動けばこっちのもんです。
しかしこれまでのSHISEIGYO-1 もATOM Matrixのライブラリが成熟する前に製作したので、今度どこかのタイミングで最新ライブラリでコードをきれいにまとめたいなぁなんて思っています。
次の記事
SHISEIGYO-1 DC 完成！ ーリアクションホイールへの道47ー
