SPRESENSEのマルチコアを活かして、非同期で計算とサーボの通信をやる

Meridian計画というオープンソースを活かして、非同期でUnityと通信しながら、サーボモーターを制御するシステムをSPRESENSEで作る予定です。

"Meridan"はヒューマノイドロボットの制御や開発を円滑にするためのオープンソースプロジェクトです。
すでに浸透している既存のハードウェアやルール、多く利用されているソフトウェアに準じながらそれらをつなぎ合わせる"経絡"となる仕組みを用意することで、既存製品にはない柔軟性の高いシステム構成や開発環境をつくっていく、というプロジェクトです。

Meridanでは従来、マイコン２つ(ESP32とTeensy)を使って製作している専用基板があるのですが、それをSPRESENSEのマルチコアを使って１つのマルチコアマイコンに移植したいと思います。
その他、センサー関連も残り３つのサブコアに割り振ることで、それぞれをCPUのリソース最大限で実行出来るのではと考えています。

それに伴って、下記の要素技術の実験を行いました。

ただ、もう一歩のところで、用意していたパーツが使えないことが分かり、方針変更を余儀なくされている状況です。

iS110BにUDPのサンプルがなかった誤算、UDPを諦めてTCPで繋いでみたものの、手持ちのiS110B (ver.A)は、そもそも、WiFiとUART2を併用出来ない仕様だった(ver.Cなら可能とのこと、ただし入手不可)
手持ちの有線用のArduino用のEthernetシールド(W5100)がSPRESENSEとマッチしなかった(Ethernetシールド2(W5500)なら動作可能のとの情報も)

とりあえず、この記事では、通信の部分は置いておいて、とにかく非同期で、制御とサーボモーターの通信を独立して実行するものを作りました。
SPRESENSEの有用性は、確認出来ました！

共有メモリを用いて、サブコア同士でMeridianプロトコル(Meridian90)の情報を共有出来ることを利用し、モーターの計算と、モーターとの通信の2つのプロセスをマルチコアで分担させ非同期でかつ、それぞれの最速の処理速度で実行させる。

メインコア : 共有メモリを確保する、サブコアを２つ立ち上げてサブコアに共有メモリのアドレスを通知
サブコア1 : Dynamxielとひたすら通信する (no-waitでリソースを通信に全振り)
サブコア2 : Dynamixelが動く軌道(sin波)をひたすら計算(1ms = 1000Hzで更新)

Spresense - 1個
Spresense拡張ボード - 1個
DYNAMIXEL Shield for Arduino MKR series - 1個
Dynamixel XL430 - 1個
電源等 - 適量

配線は以下の通り。

共有メモリを確保する、サブコアを２つ立ち上げてサブコアに共有メモリのアドレスを通知


// メインコア
// 共有メモリを確保する、サブコアを２つ立ち上げてサブコアに共有メモリのアドレスを通知

#ifdef SUBCORE
#error "Core selection is wrong!!"
#endif

#include <MP.h>

#define MSG_SIZE 90 //Meridim配列の長さ設定（デフォルトは90）
int16_t *meridian_addr; // meridian用共有メモリアドレス

int subcore1 = 1;
int subcore2 = 2;

void setup()
{
  int i;

  Serial.begin(115200);
  while (!Serial);

  /* Boot SubCore */
  MP.begin(subcore1);
  MP.begin(subcore2);

  /* Allocate Shared Memory */
  uint16_t *addr = (uint16_t *)MP.AllocSharedMemory(MSG_SIZE);
  if (!addr) {
    printf("Error: out of memory\n");
    return;
  }

  // 先頭アドレスの表示
  printf("SharedMemory Address=@%08lx\n", (uint32_t)addr);

  // 共有メモリの先頭アドレスをグローバルで保持
  meridian_addr = &addr[0];

  // 共有メモリを0x0000で初期化

  for (i = 0; i < MSG_SIZE; i++) {
    addr[i] = 0x0000;
  }

  /////////////////////////////////////////////

  // サブコアへ共有メモリのアドレスを通知
  int8_t msgid = 10;
  MP.Send(msgid, addr, subcore1);

  /* Receive from SubCore */
  void *raddr;
  MP.Recv(&msgid, &raddr, subcore1);

  /* shared memory check */
  for (i = 0; i < MSG_SIZE; i++) {
    if (addr[i] != 0x0000) {
      printf("Error: @%08lx\n", (uint32_t)&addr[i]);
      while (1);
    }
  }
  printf("SharedMemory[1] Check: OK!\n");
  
  /////////////////////////////////////////////

  // サブコアへ共有メモリのアドレスを通知
  MP.Send(msgid, addr, subcore2);

  /* Receive from SubCore */
  MP.Recv(&msgid, &raddr, subcore2);

  /* shared memory check */
  for (i = 0; i < MSG_SIZE; i++) {
    if (addr[i] != 0x0000) {
      printf("Error: @%08lx\n", (uint32_t)&addr[i]);
      while (1);
    }
  }
  printf("SharedMemory[2] Check: OK!\n");

  /////////////////////////////////////////////

  // この後、ずっと使うので破棄はしない
  /* Free Shared Memory */
  //MP.FreeSharedMemory(addr);
}

void loop()
{
  //なにもしない
}

こちらのコアでは、Dynamxielとひたすら通信する (no-waitでリソースを通信に全振り)


// サブコア1
// こちらのコアでは、ひたすらDynamixelと通信する
// 

#if (SUBCORE != 1)
#error "Core selection is wrong!!"
#endif

#include <MP.h>
#include <Dynamixel2Arduino.h>

//Please see eManual Control Table section of your DYNAMIXEL.
//This example is written based on DYNAMIXEL X series(excluding XL-320)
#define ID_ADDR                 7
#define ID_ADDR_LEN             1
#define BAUDRATE_ADDR           8
#define BAUDRATE_ADDR_LEN       1
#define PROTOCOL_TYPE_ADDR      13
#define PROTOCOL_TYPE_ADDR_LEN  1
#define TIMEOUT 10    //default communication timeout 10ms
#define OPERATING_MODE_ADDR         11
#define OPERATING_MODE_ADDR_LEN     1
#define TORQUE_ENABLE_ADDR          64
#define TORQUE_ENABLE_ADDR_LEN      1
#define LED_ADDR                    65
#define LED_ADDR_LEN                1
#define GOAL_POSITION_ADDR          116
#define GOAL_POSITION_ADDR_LEN      4
#define PRESENT_POSITION_ADDR       132
#define PRESENT_POSITION_ADDR_LEN   4
#define POSITION_CONTROL_MODE       3

uint8_t turn_on = 1;
uint8_t turn_off = 0;

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;

uint8_t operatingMode = POSITION_CONTROL_MODE;

#define DXL_SERIAL   Serial2
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN

#define MSG_SIZE 90 //Meridim配列の長さ設定（デフォルトは90）
int16_t *meridian_addr; // meridian用共有メモリアドレス

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use Control table item names
using namespace ControlTableItem;

void setup()
{
  int8_t msgid;
  uint16_t *addr;

  MP.begin();

  // メインコアから共有メモリの先頭アドレスの受け取り
  MP.Recv(&msgid, &addr);

  // グローバルへ先頭アドレスを保持
  meridian_addr = &addr[0];

  // メインコアへ先頭アドレスを返事
  MP.Send(msgid, addr);

  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(1000000);
  
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Turn off torque when configuring items in EEPROM area
  dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT);
  
  // Set Operating Mode
  dxl.write(DXL_ID, OPERATING_MODE_ADDR, (uint8_t*)&operatingMode, OPERATING_MODE_ADDR_LEN, TIMEOUT);

  // Turn on torque
  dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT);
  
}

// ■ degreeをDynamixel値に変換 ----------------------------------------------------
int Deg2Dxl(float degree) { //degreeにはidl_d[i] * idl_pn[i]、id_nにはidl_n[i]を入れる(左の場合は左半身系)
  return (int)((degree * 2048 / 180) + 2048) ;
}

void loop()
{
  int goalPosition1;
  goalPosition1 = Deg2Dxl( (float)meridian_addr[21] / 100 );
  dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
  
}

Dynamixelが動く軌道(sin波)をひたすら計算(1ms = 1000Hzで更新)


// サブコア2
// こちらのコアでは、ひたすらサーボモーターの値の計算を行う
//

#if (SUBCORE != 2)
#error "Core selection is wrong!!"
#endif

#include <MP.h>

#define MSG_SIZE 90 //Meridim配列の長さ設定（デフォルトは90）
int16_t *meridian_addr; // meridian用共有メモリアドレス

void setup()
{
  int8_t msgid;
  uint16_t *addr;

  MP.begin();

  // メインコアから共有メモリの先頭アドレスの受け取り
  MP.Recv(&msgid, &addr);

  // グローバルへ先頭アドレスを保持
  meridian_addr = &addr[0];

  // メインコアへ先頭アドレスを返事
  MP.Send(msgid, addr);

}

void loop()
{
  for(int i = 0; i< 3600 ;i++){
    meridian_addr[21] = 9000 * sin( 2 * M_PI * i / 3600);
    delay(1);
  }
}

サーボモーターの角度がsin波を描くようになめらかに動いています。
サーボモーターは、1つだけしか繋がっておらず、1Mbpsの通信速度においては、no-waitで最速で通信しています。
制御と計算が非同期でかつ、それぞれに引っ張られない形で動作しています。

ここにPOSTが表示されます