ひとしれずひっそり

主にソフトに関することをメモしていきます。過程をそのまま書いていたりするので間違いが含まれます。鵜呑みしない様に。

倒立振子に挑戦 (2)

制御はPIDを使う。
普段組み込まれたPIDは作った事はあるが、プログラムした事はない。

こちらを参考にお手軽版でクラスを起こした。

controlabo.com

(追記: ちょっと修正)

class Pid {
  private:
    // aim
    float r;

    // input
    float kp;
    float ki;
    float kd;
    float freq;

    // var
    float t;
    float e_pre;   // error
    float e;    // error
    float de;
    float ie;

    // output
    float u;

  public:

    Pid(float aim, float kp, float ki, float kd, float f) {
      this->r = aim;
      this->kp = kp;
      this->ki = ki;
      this->kd = kd;
      this->freq = f;
      this->t = 1 / f;
    }

    float getAim() { return r; }
    float getKp() { return kp; }
    float getKi() { return ki; }
    float getKd() { return kd; }
    void setAim(float aim) { r = aim; }
    void setKp(float k) { kp = k; }
    void setKi(float k) { ki = k; }
    void setKd(float k) { kd = k; }

    float getError() { return e; }
    float getPreviousError() { return e_pre; }
    float getDiffError() { return de; }
    float getIntegralError() { return ie; }

    float cal(float y) {
      e_pre = e;

      e = r - y;
      ie += (e + e_pre) * t / 2.0;
      de = (e - e_pre) / t;
      u = kp * e + ki * ie + kd * de;
      return u;
    }

};

あとはToioを走らせて係数調整するだけ。

倒立振子に挑戦 (1)

前回書いた様に倒立振子に挑戦してみる。

まず振子の姿勢を知るためにIMUを使用するので、M5AtomS3を制御に使用する。
姿勢を知るにはroll、pitch、yawを計算すれば良い。

M5AtomライブラリーではM5. MPU6886.getAhrsData()で得られるが、M5AtomS3ライブラリーにはない様だ。ただMahonyAHRS関連の関数はあるようなのでそれを使えば良さそう。

M5AtomS3/MahonyAHRS.h at main · m5stack/M5AtomS3 · GitHub

M5UnifiedにはMahonyAHRS関連は用意されてない様なのでMadgwickライブラリーを読み込んで使用する。

lib_deps = 
    M5Unified
    arduino-libraries/Madgwick@^1.2.0

周期的に計算される事を前提としている様なので、最初タイマーを使用したがスタックを結構使用する様でスタックオーバーフローになってしまった。
タイマーに割り当てるスタックサイズを変更できることも書いてあったが、これからToioの制御もあるのでタスクを使用する事にした。(厳密には計算時間分遅れちゃうんだけど)

static void imu_task(void*)
{
  int freq = 100;
  madgwick.begin(freq);
  while(true) {
    float val[6];
    M5.Imu.getGyro(&val[0], &val[1], &val[2]);
    M5.Imu.getAccel(&val[3], &val[4], &val[5]);
    madgwick.updateIMU(val[0], val[1], val[2], val[3], val[4], val[5]);
    roll = madgwick.getRoll();
    pitch = madgwick.getPitch();
    yaw = madgwick.getYaw();
    delay(1000 / freq);
    Serial.printf("roll: %f, pitch: %f, yaw: %f\r\n", roll, pitch, yaw);
  }
}

表示はM5Unifiedのサンプルを流用した。

M5Unified/HowToUse.ino at master · m5stack/M5Unified · GitHub

表示の判定にあるこの部分が最初理解できなかった。

    if ((xpos[i] < 0) != (px < 0))

結果として左の条件と右の条件のXORを取っている事になっていた。
左は現在の値が負の値で、右は前回の値が負の値を判定している。
負の値負の値 の場合は成立しない。
判定に直接出てこないが 正の値正の値 の場合も成立しない。(0の場合も含まれる)
正と負 、または 負と正符号が違う場合 に条件が成立する。

つまり前回と符号か変わったかを判定している。

この書き方は馴染みがなかったが、XOR判定するときに今度使ってみよう。
というか != はXOR演算子なのか。長い事ソフトをやっているが驚きの発見だ。

上からroll(赤)、pitch(緑)、yaw(青)を表していて、今回はrollを使えば良さそうだ。

ここまで M5AtomS3とToioで倒立振子に挑戦 · GitHub

M5StackToio

Atom Mate for Toio で倒立振子を作ろうとしている。
倒立振子Lego Mindstormsを使った講座を受けた事はあるが、教材の指示に従っていっただけで、仕組みなど全く理解していない。
のでちゃんと動作するのはずっと先の事だろう。

Atom Mate for ToioはUIFlowから使った事はあるが、Arduino Frameworkからは使った事がない。
Toioを動かせないことには始まらないが、M5StackToioでできそうなので試してみる。

github.com

ドキュメントがしっかり書かれているし、例もあるのでわかりやすい。
onMotion()コールバックが呼び出されなかったのが気になる。

前後に動かすだけなのでdriveメソッドだけでできそうだ。

https://github.com/futomi/M5StackToio#-drive-メソッド-運転

M5でmruby/c (2)

前回できたlibmrubyc.aをlibディレクトリーに置いてみるるが全く無視されている様だ。

それではとダウンロードしたmrubycディレクトリーを丸ごとlibディレクトリーに置いてみる。
エラーは出ているがmrubycのMakefileを読み込んでうまい具合にやろうとしている様だ。

mruby/cを動かすためにmain.cppファイルを書き換える。 main()処理のサンプルが mrubyc/sample_c/sample_include.c にある。
これをこのままmain.cppにコピーする。
このサンプルでは mrubyc のバイナリーを sample_include_bytecode.c から読み込んで実行させている。
ファイルのパスが違うので、パスの変更と、Espressif IoT Development Framework では app_main() がメイン関数になるのでそこから呼び出す様に追加する。

#include <stdio.h>
#include <stdlib.h>
#include "mrubyc.h"

#include "../lib/mrubyc/sample_c/sample_include_bytecode.c"

#define MEMORY_SIZE (1024*30)
static uint8_t memory_pool[MEMORY_SIZE];

int main(void)
{
  mrbc_init(memory_pool, MEMORY_SIZE);
  
  if( mrbc_create_task(ary, 0) != NULL ){
    mrbc_run();
  }

  return 0;
}

void app_main() {
  main();
}

ヘッダーの読み込みができない様なので、インクルードディレクトリをplatformio.iniでビルドオプションとして追加する。

[env:m5stack-core-esp32]
platform = espressif32
board = m5stack-core-esp32
framework = espidf
build_flags=
  -I lib/mrubyc/src

ビルドが進んでhalのコンパイルで躓いた。
どうやら使用するターゲットをフラグで指定している様だ。
これもビルドオプションで指定する。
ターゲットはM5StackなのでESP32であろうMRBC_USE_HAL_ESP32を定義する。

[env:m5stack-core-esp32]
platform = espressif32
board = m5stack-core-esp32
framework = espidf
build_flags=
  -I lib/mrubyc/src
  -DMRBC_USE_HAL_ESP32

コンパイルすると使用しないhalもlibディレクトリ内にあるのでコンパイルしようとしてエラーになる。
特定のディレクトリを除外する仕方がわからないので、exampleフォルダを作って使用しないhalのディレクトリをそこに移動させた。

これでコンパイルは通る様になった。

書き込んでみると、RTCWDT_RTC_RESETが繰り返し発生している。

rst:0x10 (RTCWDT_RTC_RESET),boot:0x17 (SPI_FAST_FLASH_BOOT)
configsip: 0, SPIWP:0xee
clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00
mode:QIO, clock div:1
load:0x3fff0030,len:6656
load:0xffffffff,len:-1
ets Jun  8 2016 00:22:57

検索するとうまく書き込みできていないためらしい。
dioというmodeで書かないといけないらしい。
検索してもdioの指定についてうまくヒットしなかったが M5AtomS3 の指定が正にdioの指定だったのでそこからコピーしてきた。

[env:m5stack-core-esp32]
platform = espressif32
board = m5stack-core-esp32
framework = espidf
build_flags=
  -I lib/mrubyc/src
  -DMRBC_USE_HAL_ESP32
board_build.f_cpu = 240000000L
board_build.f_flash = 80000000L
board_build.flash_mode = dio
monitor_speed = 115200

これで書き込んでsampleと繰り返し表示される様になった。

 *  Executing task: platformio device monitor 

--- Terminal on /dev/cu.usbserial-013F02F0 | 115200 8-N-1
--- Available filters and text transformations: colorize, debug, default, direct, esp32_exception_decoder, hexlify, log2file, nocontrol, printable, send_on_enter, time
--- More details at https://bit.ly/pio-monitor-filters
--- Quit: Ctrl+C | Menu: Ctrl+T | Help: Ctrl+T followed by Ctrl+H
sample
sample
sample
sample

sample_include_bytecode.cの元になったsample_include_bytecode.rbを見てみる。
1秒毎にsampleを表示する様になっていてmrubycのコードが動いていることが確認できた。

while true
  puts "sample"
  sleep 1
end

platformioでmruby/cの開発環境が思っていたより簡単にできる事がわかった。

参考までにtreeを載せておく。

% tree
.
├── CMakeLists.txt
├── include
│   └── README
├── lib
│   ├── README
│   └── mrubyc
│       ├── Dockerfile
│       ├── Doxyfile
│       ├── Gemfile
│       ├── Gemfile.lock
│       ├── LICENSE
│       ├── Makefile
│       ├── README.md
│       ├── doc
│       │   ├── How_to_make_your_own_HAL.md
│       │   ├── compile.md
│       │   └── test.md
│       ├── mrblib
│       │   ├── Makefile
│       │   ├── array.rb
│       │   ├── global.rb
│       │   ├── hash.rb
│       │   ├── numeric.rb
│       │   ├── object.rb
│       │   ├── range.rb
│       │   └── string.rb
│       ├── sample_c
│       │   ├── Makefile
│       │   ├── myclass.rb
│       │   ├── sample_concurrent
│       │   ├── sample_concurrent.c
│       │   ├── sample_concurrent.dSYM
│       │   │   └── Contents
│       │   │       ├── Info.plist
│       │   │       └── Resources
│       │   │           └── DWARF
│       │   │               └── sample_concurrent
│       │   ├── sample_include
│       │   ├── sample_include.c
│       │   ├── sample_include.dSYM
│       │   │   └── Contents
│       │   │       ├── Info.plist
│       │   │       └── Resources
│       │   │           └── DWARF
│       │   │               └── sample_include
│       │   ├── sample_include_bytecode.c
│       │   ├── sample_include_bytecode.rb
│       │   ├── sample_myclass
│       │   ├── sample_myclass.c
│       │   ├── sample_myclass.dSYM
│       │   │   └── Contents
│       │   │       ├── Info.plist
│       │   │       └── Resources
│       │   │           └── DWARF
│       │   │               └── sample_myclass
│       │   ├── sample_no_scheduler
│       │   ├── sample_no_scheduler.c
│       │   ├── sample_no_scheduler.dSYM
│       │   │   └── Contents
│       │   │       ├── Info.plist
│       │   │       └── Resources
│       │   │           └── DWARF
│       │   │               └── sample_no_scheduler
│       │   ├── sample_scheduler
│       │   ├── sample_scheduler.c
│       │   └── sample_scheduler.dSYM
│       │       └── Contents
│       │           ├── Info.plist
│       │           └── Resources
│       │               └── DWARF
│       │                   └── sample_scheduler
│       ├── src
│       │   ├── Makefile
│       │   ├── _autogen_builtin_symbol.h
│       │   ├── _autogen_class_array.h
│       │   ├── _autogen_class_exception.h
│       │   ├── _autogen_class_float.h
│       │   ├── _autogen_class_hash.h
│       │   ├── _autogen_class_integer.h
│       │   ├── _autogen_class_math.h
│       │   ├── _autogen_class_object.h
│       │   ├── _autogen_class_range.h
│       │   ├── _autogen_class_string.h
│       │   ├── _autogen_class_symbol.h
│       │   ├── alloc.c
│       │   ├── alloc.h
│       │   ├── alloc.o
│       │   ├── c_array.c
│       │   ├── c_array.h
│       │   ├── c_array.o
│       │   ├── c_hash.c
│       │   ├── c_hash.h
│       │   ├── c_hash.o
│       │   ├── c_math.c
│       │   ├── c_math.h
│       │   ├── c_math.o
│       │   ├── c_numeric.c
│       │   ├── c_numeric.h
│       │   ├── c_numeric.o
│       │   ├── c_object.c
│       │   ├── c_object.h
│       │   ├── c_object.o
│       │   ├── c_range.c
│       │   ├── c_range.h
│       │   ├── c_range.o
│       │   ├── c_string.c
│       │   ├── c_string.h
│       │   ├── c_string.o
│       │   ├── class.c
│       │   ├── class.h
│       │   ├── class.o
│       │   ├── console.c
│       │   ├── console.h
│       │   ├── console.o
│       │   ├── error.c
│       │   ├── error.h
│       │   ├── error.o
│       │   ├── example
│       │   │   ├── hal_pic24
│       │   │   │   └── hal.h
│       │   │   ├── hal_posix
│       │   │   │   ├── hal.c
│       │   │   │   ├── hal.h
│       │   │   │   └── hal.o
│       │   │   ├── hal_psoc5lp
│       │   │   │   ├── hal.c
│       │   │   │   └── hal.h
│       │   │   └── hal_rp2040
│       │   │       ├── hal.c
│       │   │       └── hal.h
│       │   ├── global.c
│       │   ├── global.h
│       │   ├── global.o
│       │   ├── hal_esp32
│       │   │   ├── hal.c
│       │   │   └── hal.h
│       │   ├── hal_selector.h
│       │   ├── hal_selector.mk
│       │   ├── keyvalue.c
│       │   ├── keyvalue.h
│       │   ├── keyvalue.o
│       │   ├── libmrubyc.a
│       │   ├── load.c
│       │   ├── load.h
│       │   ├── load.o
│       │   ├── mrblib.c
│       │   ├── mrblib.o
│       │   ├── mrubyc.h
│       │   ├── opcode.h
│       │   ├── rrt0.c
│       │   ├── rrt0.h
│       │   ├── rrt0.o
│       │   ├── symbol.c
│       │   ├── symbol.h
│       │   ├── symbol.o
│       │   ├── value.c
│       │   ├── value.h
│       │   ├── value.o
│       │   ├── vm.c
│       │   ├── vm.h
│       │   ├── vm.o
│       │   └── vm_config.h
│       ├── support
│       │   ├── common_sub.rb
│       │   ├── make_method_table.rb
│       │   └── make_symbol_table.rb
│       └── test
│           ├── array_test.rb
│           ├── bool_test.rb
│           ├── def_test.rb
│           ├── exception_test.rb
│           ├── global_test.rb
│           ├── hash_test.rb
│           ├── integer_test.rb
│           ├── math_test.rb
│           ├── models
│           │   ├── my_alias.rb
│           │   ├── my_block.rb
│           │   ├── my_equal3.rb
│           │   ├── my_instance_variable.rb
│           │   ├── my_super.rb
│           │   └── object.rb
│           ├── my_alias_test.rb
│           ├── my_block_test.rb
│           ├── my_equal3_test.rb
│           ├── my_instance_variable_test.rb
│           ├── my_nested_class_test.rb
│           ├── my_super_test.rb
│           ├── nil_class_test.rb
│           ├── object_test.rb
│           ├── op_basic_test.rb
│           ├── op_method_test.rb
│           ├── range_test.rb
│           ├── sprintf_test.rb
│           ├── string_ljust_rjust_test.rb
│           ├── string_split_test.rb
│           ├── string_strip_test.rb
│           ├── string_test.rb
│           ├── symbol_test.rb
│           └── tmp
│               └── hal -> ../../src/hal_posix
├── platformio.ini
├── sdkconfig.m5stack-core-esp32
├── src
│   ├── CMakeLists.txt
│   ├── main.c
│   └── scripts
└── test
    └── README

onChange に気をつけろ(onChangeだけじゃないけどな)

SwiftUIで onChange を使ったらハングアップしてしばらくしてからクラッシュする現象が出た。
なんかViewが再帰的に呼び出されている様でクラッシュに至る様だ。
ストップボタンもしばらく受け付けてくれない様な状況で、クラッシュするのを待つだけしか手がない。

こんな感じで書いてたんだけど、

SomeView()

.onChange(of: hoo) { newValue in
}

実はonChangeの前のドットを入れ忘れてこうなってた。

SomeView()

onChange(of: hoo) { newValue in
}

コンパイル時点でエラーで弾かれそうだが、なんか通ってて原因が分からずってことになってた。
分かるまでほんと大変だった。

試しに onAppear もドット外してみたらコンパイルが通る。
これは気をつけないといけない。

M5でmruby/c (1)

M5やmicrobitでmrubyやmruby/cが動かせるが platformio で構築できないかトライしてみる。

通常Arduinoフレームワークを選択するが、Espressif IoT Development Framework を使うとなんかできないか試してみる。

まずはmruby/cから

  • Githubからダウンロード github.com
  • makeを実行 ライブラリーlibmrubyc.aが作られとmrubyコードのサンプルがコンパイルされる。
    一瞬で終わって軽量なのがよくわかる。
    libmrubyc.aはクロスコンパイルされてない気がするが、さて…

M5Unifiedで機種(ボード)判定

M5Unifiedを使用すると同じバイナリーで様々なM5の機種に対応ができるとTwitterとかで見たことがある。

内部で判定している場所があるはずなので探ってみる。

M5Unified.hpp内で _board が定義されていてここにデバイスに合わせて設定されるっぽい。

M5Unified/M5Unified.hpp at master · m5stack/M5Unified · GitHub

m5gfx::board_t _board = m5gfx::board_t::board_unknown;

M5.begin()内で判定される様だ。

M5Unified/M5Unified.hpp at master · m5stack/M5Unified · GitHub

      auto board = _check_boardtype(_primaryDisplay.getBoard());
      if (board == board_t::board_unknown) { board = cfg.fallback_board; }
      _board = board;

判定された機種はM5. getBoard()で得られる様だ。

M5Unified/M5Unified.hpp at master · m5stack/M5Unified · GitHub

    /// get the board type of the runtime environment.
    /// @return board type
    board_t getBoard(void) const { return _board; }

そしてどの様な機種が定義されているかというと、M5Unifiedが依存しているM5GFXのboareds.hppに書かれている。

M5GFX/boards.hpp at master · m5stack/M5GFX · GitHub

抜粋だが、Displayの機種も下の方にある。

    enum board_t
    { board_unknown
    , board_Non_Panel
    , board_M5Stack
    , board_M5StackCore2
    , board_M5StickC
    , board_M5StickCPlus
    , board_M5StackCoreInk
    , board_M5Paper
    , board_M5Tough
    , board_M5Station
    , board_M5StackCoreS3
    , board_M5AtomS3

前回のIR送信でM5StickCにも対応させようとすると、こうできる。
ポインターになってしまうのはしょうがないかな。

#include <Arduino.h>
#include <IRremoteESP8266.h>
#include <IRsend.h>
#include <M5Unified.h>

#ifndef USE_USEBSERIAL
#define USBSerial Serial
#endif

static uint16_t kIrLed = 4;

IRsend *irsend;

void setup() {
  M5.begin();

  switch(M5.getBoard()) {
    case m5gfx::board_t::board_M5StickC:
      kIrLed = 9;
      break;
    case m5gfx::board_t::board_M5AtomS3:
      kIrLed = 4;
      break;
  }
  irsend = new IRsend(kIrLed);

  USBSerial.begin(115200);
  irsend->begin();
}

void loop() {
  M5.update();
  if (M5.BtnA.wasClicked()) {
    USBSerial.println("Power off");
    irsend->sendSony(0xa90, 12, 3);  // 12 bits & 3 repeats
    //irsend.sendNEC(0x00FFE01FUL);
    delay(2000);
  }
}

このまま転送しようとするとplatformioだと機種判定されて転送できないので、今回は同じバイナリーで動作できるかは追及せず環境切り替えで行うことにした。
またSerialとUSBSerialの違いもあるのでS3系はシリアル出力ではもうひと工夫が要りそうだ。

M5StickCだと2m位までの距離で反応した。

M5AtomS3でソニーテレビの赤外線リモコン信号を送信 · GitHub