ROBOTIS Dynamixelシリアルサーボの制御

Dynamixelについて

ROBOTIS Dynamixelはロボット用シリアルサーボのシリーズ。

Dynamixel XL-320の仕様

f:id:licheng:20181119163436j:plain
XL-320は、小型二足歩行ロボット DARWIN-MINI に採用されいている。

  • 電圧: 6 ~ 8.4V (推奨: 7.4V)
  • トルク: 3.82kg-cm (0.39Nm)
  • 重量:16.7g / サイズ:24mm x 36mm x 27mm
  • 通信プロトコル: Protocol 2.0 (V2)
  • ボーレート: 7343 bps ~ 1 Mbps (デフォルト: 1Mbps)
  • 市販のシリアルサーボとしてはかなり小さい。実勢価格3千円前後。
  • XL-320 (ROBOTIS 英文資料)

GR-ROSEでの制御

ルネサスRX-65N搭載のGR-ROSEボード(ベータ版)でXL-320を制御したようす。
GR-ROSEは半二重シリアルサーボをSerial1, Serial2に直接挿せる。
プログラムはArduinoベースの環境(IDE for GR)で作成。

GR-ROSE(ベータ版)でDynamixelシリアルサーボ

コード

#include<stdint.h>

// CRC-16-IBM
uint16_t CRC_calc(uint8_t *data, int length)
{
  uint16_t crc16;
  int i,j;
  
  crc16 = 0x0000; // 初期値=0
  for(i=0;i<length;i++){
    crc16 ^= ( ((uint16_t)data[i]) << 8);
    for(j=0;j<8;j++){
      if(crc16 & 0x8000){
        crc16 = (crc16 << 1) ^ 0x8005; // 生成多項式
      }else{
        crc16 <<= 1;
      }
    }
  }
  return crc16;
}

bool sendpos(uint8_t id, uint16_t pos) {
  // インストラクションパケット
  uint8_t tx_buf[] = { // 多バイトの数値はリトルエンディアン
    0xFF, 0xFF, 0xFD, 0x00, // Header(固定値)
    id,         // ID
    0x09, 0x00, // Length
    0x03,       // Instruction (0x03 = WRITE)
    30, 0,      // Address (30 = GoalPosition)
    (uint8_t)(pos & 0xFF), (uint8_t)(pos >> 8), 0x00, 0x00, // Data
    0, 0        // CRC (下で計算)
  };
  // CRC計算
  uint16_t crc = CRC_calc(tx_buf, 14);
  tx_buf[14] = (uint8_t)(crc & 0xFF);
  tx_buf[15] = (uint8_t)(crc >> 8);
  // 送信
  while (Serial1.available ()) Serial1.read (); // バッファクリア
  Serial1.write (tx_buf, 16);
  
  // ステータスパケット受信
  uint8_t rx_buf[16+11];
  if (Serial1.readBytes (rx_buf, 16+11) == 16+11){
    if(rx_buf[16+4] != id){ // IDの一致をチェック
     Serial.println("ID not matched!");
     return false;
    }
    if(rx_buf[16+8] != 0){ // エラーをチェック
     Serial.print("Status Error = "); Serial.println(rx_buf[16+8]);
     return false;
    }
    Serial.println("OK!");
    return true;
  }else{
    Serial.println("Bad response!");
    return false;
  }
}

void setup() {
  Serial1.begin (1000000); // 1Mbps
  Serial1.direction(HALFDUPLEX); // GR-ROSEではこれが必要
  Serial1.setTimeout (50);
  
  Serial.begin(115200);
}
 
void loop() {
  for(int id = 1; id <= 3; id++){
    sendpos (id, 0);
    delay (1000);
    sendpos (id, 1023);
    delay (1000);
  }
}

Processingで魚眼

魚眼のことがちょっとだけ分かった。Processingたのしい。

魚眼でレナさん

f:id:licheng:20181116083006p:plain

メモ

f:id:licheng:20181116200111j:plain

スケッチ

PImage srcImg; // 元画像
PImage dstImg; // 処理後の画像
int W,H; // 画像のサイズ
int R; // レンズの半径
int D; // レンズの中心から投影面までの距離

void settings() {
  // Processing 3系ではsizeに変数を使う場合は
  // setup()でははくsettings()で
  srcImg = loadImage("lena.jpg");
  W = srcImg.width;
  H = srcImg.height;
  R = int(W * 0.6);
  D = int(R * 0.3); // 小さいほど大きく歪む
  dstImg = createImage(W, H, RGB);
  size(W, H);
}

void setup() {
}

void draw() {
  // マウス座標をレンズの中心とする
  int x0 = mouseX;
  int y0 = mouseY;

  float x,y; //写像前の座標
  // 写像後の座標
  for(int Y = 0; Y < H; Y++){
    for(int X = 0; X < W; X++){
      color c;
      
      // レンズの中心からの相対座標
      int dX = X - x0;
      int dY = Y - y0;
      float d = sqrt(dX*dX + dY*dY);
      if(d <= R){
        // 写像:元画像→魚眼画像
        // X = R*x/√(D^2+x^2+y^2)
        // Y = R*y/√(D^2+x^2+y^2)
        // 逆写像:魚眼画像→元画像
        // x = D*X/√(R^2-X^2-Y^2)
        // y = D*Y/√(R^2-X^2-Y^2)
        float Z = sqrt(R*R - dX*dX - dY*dY);
        x = x0 + (D*dX) / Z;
        y = y0 + (D*dY) / Z;
        
        if(x>=0 && x<W && y>=0 && y<H){
          c = srcImg.get(int(x), int(y));
        }else{
          c = color(0); // 元画像の外側なら黒塗り
        }
      }else{
        c = color(0); // レンズの外側なら黒塗り
      }
      dstImg.set(X, Y, c);
    }
  }
  image(dstImg, 0, 0);
}

専門外分野のメモ

JavaScriptに関するメモ

高速化について
  • 重いスクリプトの<script>要素は、<head>要素内でなく、<body>要素内末尾に書いた方がページの表示が速い。
  • JS MinifierとかYUI Compressorとかでコードの圧縮(コメントやら空白やら改行を除去)すると読み込み時間短縮。
  • evalは処理コスト高い。たいていの場合evalは使わなくて済むはず。
  • 例外は処理コスト高い。ビジネスロジックでの入力値チェック例外は利用せず、if文で処理したほうが速い。
即時関数で変数名の衝突を避ける
(function(){
    ほげほげ
}).call(this);

みたいな記述。単に ほげほげ と書けばいいだけのように見えるが、こうすることで「ほげほげ」内で定義される変数はローカル変数となり、グローバル変数の名前の衝突を避けることができる。

関数型言語っぽい性格
  • 関数を変数に代入できたり、関数を関数に渡せたりする。
  • 関数リテラルが書ける。(名前のない関数をその場で書ける。)
  • クロージャの仕組みがある。(関数が書かれたスコープの変数をキャプチャできる。)
クラスが無い(無かった?)

C#で親Formからデータを受け取る

Form1からForm2を開いて、Form2でForm1のデータを受け取る方法。
(ownerなんて引数があるのを知らずに、いつも別途渡していた orz)


Form1からForm2を開くとき、自分自身を引数(owner)として渡す。

var form2 = new Form2();
form2.ShowDialog(this);

Form2で自分のOwnerをForm1にキャストし、データを受け取る。

var form1 = (Form1)this.Owner;
MessageBox.Show(form1.hoge);

STM32のRTCの落とし穴

STM32のRTCを使ってみてハマった落とし穴などについて書く。

  • 環境は、STM32F767ZI (Nucleo-F767ZI) + HALドライバ + CubeMX + IAR EWARM
  • 以下では単にSTM32と書くが、STM32F767ZIでしか試していない。
  • やや不可解な内容も含むので誤りがあればご指摘ください。

異常な時刻になるバグ

STM32のRTCは、12時間制モードで23時などのありえない時刻を設定すると、バグって異常な動作をはじめるようだ。具体的には、23:59:59のあと日付が変わらずに24:00:00になってしまい、その後も日付が変わらないままにありえない時刻が進んでいく。
下図は時刻が24:00:02になった状態のRTCのレジスタ値である。CR=0x00000040は12時間制モードの設定である。
f:id:licheng:20181112170301p:plain

HAL_RTC_SetTime関数の注意点

HALドライバのHAL_RTC_SetTime関数はRTCの時刻を設定するAPIだが、気を付けないと前述の異常な時刻になるバグを発生させてしまう。

    RTC_TimeTypeDef time;
    time.Hours   = hour;
    time.Minutes = min;
    time.Seconds = sec;
    HAL_RTC_SetTime(&hrtc, &time, FORMAT_BIN);

これで一見問題なさそうだが、じつはRTC_TimeTypeDef構造体は、時・分・秒以外にもメンバ変数がある。これらが不定値であるとRTCのレジスタに意図しない値が設定され、場合によっては前述のバグを引き起こすのだ。あまり使わない設定項目だがちゃんと設定してやる。

    RTC_TimeTypeDef time;
    
    // ちゃんとすべて設定する
    time.TimeFormat     = RTC_HOURFORMAT_24; // 24時間制モード (じつはほとんど意味なし)
    time.DayLightSaving = RTC_DAYLIGHTSAVING_NONE ; // サマータイムではない
    time.StoreOperation = RTC_STOREOPERATION_RESET; // サマータイム設定に変更なし(?)
    
    // あるいはゼロクリアする(上記設定と同じ)
    memset(&time, 0x00, sizeof(time));
    
    time.Hours   = hour;
    time.Minutes = min;
    time.Seconds = sec;
    HAL_RTC_SetTime(&hrtc, &time, FORMAT_BIN);

バックアップドメイン

RTC関係のレジスタはバックアップドメインと呼ばれ、主電源とは別のバッテリー(ないしコンデンサ)によってバックアップされ、主電源OFF時も内容が保持される。また、バックアップドメインには32バイトの自由に使えるRTCバックアップレジスタ(RTC_BKPxR)があり、不揮発なデータやフラグの保存場所としてアプリケーションから利用できる。

HAL_RTC_Init関数の注意点

HALドライバのHAL_RTC_Init関数はRTCを初期化するAPIだが、初期化の際に秒以下の時刻がリセットされる。つまり平均0.5秒時刻が遅れることになり、起動のたびにHAL_RTC_Init関数を実行すれば100回で約50秒時刻が遅れる。これを回避するには、起動時に時計が設定済みであれば自分で最低限の設定だけしてHAL_RTC_Init関数は呼ばないようにする。HALドライバのコードを修正することもできるが、保守性を著しく損なうので避けることにした。最低限の設定の内容については後述する。
※ ほんとうにそんなイケてないライブラリなのだろうか? ぼくの使い方が悪いのだろうか?

CubeMXが生成する初期化コードの問題点

CubeMXが生成する初期化コードではRTCの初期化はMX_RTC_Init関数で行われる。

static void MX_RTC_Init(void)
{
  RTC_TimeTypeDef sTime;
  RTC_DateTypeDef sDate;

  // ここで必ずRTCが初期化される
  hrtc.Instance = RTC;
  (中略)
  if (HAL_RTC_Init(&hrtc) != HAL_OK) 
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  // 日時設定ずみでなければ設定する
  // (RTCバックアップレジスタにマジックナンバーがあるかで判断)
  if(HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR0) != 0x32F2){
    (中略)
    if (HAL_RTC_SetTime(&hrtc, &sTime, RTC_FORMAT_BIN) != HAL_OK)
    {
      _Error_Handler(__FILE__, __LINE__);
    }
    (中略)
    if (HAL_RTC_SetDate(&hrtc, &sDate, RTC_FORMAT_BIN) != HAL_OK)
    {
      _Error_Handler(__FILE__, __LINE__);
    }
    // RTCバックアップレジスタにマジックナンバーを書き込み
    HAL_RTCEx_BKUPWrite(&hrtc,RTC_BKP_DR0,0x32F2);
  }
}

つまり、時計が設定済みであろうがなかろうが、起動時に必ずHAL_RTC_Init関数が呼ばれてしまい、そのたびに時刻が平均0.5秒遅れる。このコードは本来は手で編集すべきものではないが、不都合なので修正する。ただし、CubeMXでコードを生成し直すたびに修正内容が消されてしまうので、必ずバージョン管理ツールを使用して慎重に再編集を行う。修正内容については後述する。
※ ほんとうにそんなイケてないライブラリなのだろうか? ぼくの使い方が悪いのだろうか?

最低限の設定

起動時に時刻未設定の場合のみHAL_RTC_Init関数を呼び、時刻設定ずみであればhrtc構造体に設定と状態を与え、HAL_RTC_MspInit関数を呼べばよい。具体的には前述のMX_RTC_Init関数を以下のように修正する。

static void MX_RTC_Init(void)
{
  RTC_TimeTypeDef sTime;
  RTC_DateTypeDef sDate;
  
  // 最低限のRTCの設定
  hrtc.Instance = RTC;
  (中略)
  hrtc.Lock = HAL_UNLOCKED;
  hrtc.State = HAL_RTC_STATE_READY;
  HAL_RTC_MspInit(&hrtc); // これを実行しないとRTCが動作しないようである

  // 日時設定ずみでなければ設定する
  if(HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR0) != 0x32F2){
    
    // ここでRTCの完全初期化 (日時未設定のときのみ完全初期化する)
    if (HAL_RTC_Init(&hrtc) != HAL_OK)
    {
      _Error_Handler(__FILE__, __LINE__);
    }
    (中略)
    if (HAL_RTC_SetTime(&hrtc, &sTime, RTC_FORMAT_BIN) != HAL_OK)
    {
      _Error_Handler(__FILE__, __LINE__);
    }
    (中略)
    if (HAL_RTC_SetDate(&hrtc, &sDate, RTC_FORMAT_BIN) != HAL_OK)
    {
      _Error_Handler(__FILE__, __LINE__);
    }
    HAL_RTCEx_BKUPWrite(&hrtc,RTC_BKP_DR0,0x32F2);
  }
}

※ ほんとうにそんなイケてないライブラリなのだろうか? ぼくの使い方が悪いのだろうか?

バックアップドメインのリセット

前述の異常な時刻になるバグのようにRTCが異常な状態に陥った場合、その状態は主電源をOFFしてもバッテリーによって保持される。バックアップドメインをリセットするには下記のようなコードを実行する。なお、このコードではHALドライバだけでなく一部LLドライバのAPIを使用している。適宜LLドライバのソースをプロジェクトに取り入れ、ヘッダをインクルードすること。

#include "stm32f7xx_ll_rcc.h" // STM32F7xxの場合
    HAL_PWR_EnableBkUpAccess();
    LL_RCC_ForceBackupDomainReset();
    HAL_Delay(10);
    LL_RCC_ReleaseBackupDomainReset();
    HAL_PWR_EnableBkUpAccess();

バックアップドメインリセット後の再設定

バックアップドメインリセット後は、当然ながらRTCの再設定が必要である。また、注意すべきはRCCのBDCRレジスタもバックアップドメインに属しているので再設定が必要となる。これをしないとそもそもRTCにクロックが供給されず、動作しない。

    uint32_t bdcr = RCC->BDCR; // BDCRの値を退避
    
    // バックアップドメインのリセット
    HAL_PWR_EnableBkUpAccess();
    LL_RCC_ForceBackupDomainReset();
    HAL_Delay(10);
    LL_RCC_ReleaseBackupDomainReset();
    HAL_PWR_EnableBkUpAccess();
    
    RCC->BDCR = bdcr; // BDCRの値を復旧
    
    // RTCの再設定の例 (CubeMXが生成するMX_RTC_Init関数からコピペ) 
    hrtc.Instance = RTC;
    hrtc.Init.HourFormat = RTC_HOURFORMAT_24;
    hrtc.Init.AsynchPrediv = 127;
    hrtc.Init.SynchPrediv = 255;
    hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;
    hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;
    hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;
    HAL_RTC_Init(&hrtc);

Processingネタ

ProcessingでBox2D

Box2Dとは
Processingへの導入方法
  • [スケッチ] > [ライブラリをインポート] > [ライブラリを追加]
  • [Filter] に Box2D と入力
  • Box2D for Processing を選択して[Install]
使い方
  • [スケッチ] > [ライブラリをインポート] > [Box2D for Processing]
サンプルコード
  • [ファイル]>[サンプル]でJavaサンプルを開く。
  • [Contributed Libraries] > [Box2D for Processing]にサンプルあり。
その他
  • Box2D for Processing 以外にも Fisica というライブラリもある。


ProcessingでPerfume

Perfumeとは
  • 3人組のおどるうたうたい (しらんけど)
  • なぜかGitHubモーションキャプチャデータ(BVHデータ)が公開されている。
  • Processing用のサンプルコードも公開されている。
サンプルコード
  • 下記のサイトで公開されている。
  • Windows10 / Processing3.4で実行するとエラーが出る。
    • エラー: Framebuffer objects are not supported by this hardware (or driver)
  • Windows10 / Processing2.2.1でならエラーは出ない。