概要

SPRESENSEを用いて足先がタイヤの4脚ロボット Wheel-Legged Robot を製作しました。
SPRESENSEでのロボット製作は初めてでしたが、多彩なインターフェースやマルチコアCPUを利用しさらには事前にモニター提供いただいたSPRESENSE用のBLE通信ボードや9軸IMUセンサも活用して無事に作り上げることができました。

構成

製作した Wheel-Legged Robot の構成は以下のとおりです。

　
　
機体ベースは3Dプリントして組みたてました。

部品

Spresense メインボード
Spresense 拡張ボード
BLE1507(NUS firmware)
9軸加速度計・ジャイロ・コンパスセンサ (BMI270・AK09918) Addon ボード
シリアルサーボ STS3032
シリアルサーボ XL330-M077-T
5V 降圧DCDCモジュール
7.4V 2セル LiPoバッテリ

技術要素

本ロボットに使用した技術やデバイスについて記載します。

シリアルサーボ STS3032

ロボットの足駆動にはシリアルサーボ STS3032 を使用しました。
1足に対して２個のサーボ合計8個使用。
　
　
STS3032駆動ライブラリとして以下を使用させていただきました。
Feetech社製シリアルサーボをマイコンのUART IOで直接駆動が可能となります。

Feetechのサーボをマイコンから直接動かす

ここではソフトウェアシリアルでGPIO 2ピンでデータ送信してSTS3032を制御しました。ボーレートは1Mbps。
STS3032からのデータ受信はしていません。

ここにPOSTが表示されます

シリアルサーボ XL330-M077-T

足先のタイヤ駆動としてシリアルサーボ XL330-M077-Tを使用しました。
設定をDrive Mode：0x08、Operating Mode：PWM control として、連続回転モードで使用します。
無負荷時で300rpm以上の高速で回転します。

ここにPOSTが表示されます

　
専用のArduinoライブラリが用意されており、以下の動画やマニュアルを参考に導入しました。
https://emanual.robotis.com/docs/en/parts/controller/openrb-150/
　
ここではSPRESENSEのUART2でXL330を制御しました。ボーレートは2Mbps。
自作したサーボ用の変換基板を介してXL330とのデータ送受信をします。

　
　
通信インターフェースを別にしたことでSPRESENSEでSTS3032とXL330を独立に制御することが可能となりました。

ここにPOSTが表示されます

BLE1507

SPRESENSEメインボードと拡張ボードと合わせてBLE通信ボード BLE1507 もモニター提供いただきました。
このボードを用いてBLEによる遠隔ロボット操作を目指します。

ここにPOSTが表示されます

　
BLE1507はSPRESENSEメインボードに載せてUART2でデータ送受信します。
コードやBLE通信方法については以下を参考にしました。

BLE1507 用 Nordic UART Service FW について

以下の動画ではBLE1507を用いてスマホとのBLE通信で文字データを送受信しています。
スマホアプリ nRF Toolbox を使用しています。

ここに動画が表示されます

　
スマホからの文字データでロボットの動作モードや制御定数の指定を行います。
nRF Toolbox ではマクロで送信データに対応したボタンを設定できるので便利です。

　
BLE1507ボード―SPRESENSE間は UART2での通信となるためシリアルサーボXL330との通信と干渉してしまいます。
そこでXL330のArduinoライブラリをソフトウェアシリアル対応させるのは難しいと考え、ここではBLE1507をソフトウェアシリアル対応するべくボードを加工しました。

BLE1507のシリアル RX, TXピンのピンヘッダを除去して、RXをD16ピンにTXをGNDに配線接続しました。
今回はBLEボードからの送信は行わないためTXはGND接続としました。

この加工によってソフトウェアシリアル(RX：GPIO 16)によるBLE1507通信動作が実現できました。

ここにPOSTが表示されます

9軸センサ (BMI270・AK09918) Addon ボード

モニター提供品4点目として9軸加速度計・ジャイロ・コンパスセンサ (BMI270・AK09918) Addon ボードをいただきました。
6軸のIMUセンサ BMI270と３軸磁気センサAK09918が搭載されたSPRESENSE用センサボードです。

　
BLE通信ボード BLE1507との同梱が可能か心配でしたが無事に干渉なく載りました。

　
本ロボットでは6軸のIMUセンサ BMI270を用いて姿勢の検知を実施します。
BMI270のライブラリ導入方法は以下の記事を参考にしました。

SONY Spresense用の6軸センサーBMI270ボードをArduino IDEで動かしてみた

以下では X-Y軸の傾斜センサ動作を楽しみました。

ここにPOSTが表示されます

　
BMI270とSPRESENSE間はI2C通信でアドレスは9軸センサボードのジャンパ設定に合わせてBMI2_I2C_SEC_ADDR から BMI2_I2C_PRIM_ADDRに変更しました。
傾斜はIMUから得られた加速度と角速度からカルマンフィルタを通して算出しています。
カルマンフィルタライブラリとして以下を使用しました。
　https://github.com/TKJElectronics/KalmanFilter

電源

電源として7.4V 2セル LiPoバッテリを使用しました。
バッテリを5V DCDCモジュールで降圧してSPRESENSEに給電しました。

シリアルサーボの電源はそれぞれ以下通りです。

STS 3032：7.4V (LiPoバッテリ直結)
XL330：5V (降圧DCDC出力)

バッテリは機体裏に両面テープ固定しました。
各サーボの配線はユニバーサル基板とコネクタでまとめています。

動作

本ロボット Wheel-Legged Robot の動作をまとめます。

走行・変形

タイヤによる走行に加えて足駆動により形状変更が可能です。

ここに動画が表示されます

歩行

足駆動による四足歩行動作が可能です。
足は足先の座標を指定し逆運動学(IK)でサーボの角度を算出して制御

ここに動画が表示されます

姿勢保持

IMUセンサによって機体の姿勢を検知して水平保持します。

ここに動画が表示されます

　
IMUセンサから機体の傾きと角速度を検知して足の高さを制御し機体の水平を保っています。
それぞれの足の高さをセンサのX軸、Y軸の角度と角速度に応じていわゆるPD制御しています。

各軸の角度 $\theta$ と角速度 $\omega$ で各足の高さ $h$ を制御します。

$h=h_{-1}+K_p \cdot \theta+K_d \cdot \omega$

$h_{-1}$ は直前に算出した足高さ
$K_p$ 、 $K_d$ は係数
　

ここに動画が表示されます

倒立 (番外編)

今回のコードにまとめて盛り込むことはできませんでしたが倒立振子動作も可能です。

ここに動画が表示されます

動作まとめ

多種多様な動作をSPRESENSEによって混在させることに成功しました。

ここに動画が表示されます

ソースコード

コーディングはArduino IDEで実施し以下のように２個のコアを使用しました。

Mainコア：BLE1507から受信した動作モードやパラメータを受けてサーボ制御。IMUセンサ制御。
Sub1コア：BLEボード BLE1507 での通信管理。

Main


Main.ino
#ifdef SUBCORE
#error "Core selection is wrong!!"
#endif

#include <MP.h>
#include <SoftwareSerial.h>
#include <EEPROM.h>
#include "BMI270_Arduino.h"
#include <Kalman.h>
#include <Dynamixel2Arduino.h>

#define DXL_SERIAL   Serial2

const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL);

int sub1 = 1;

SoftwareSerial mySerial(-1, 2);    // RX,TX

int8_t   id;
int8_t   idMode = 10; 
int8_t   idPeriod = 20; 
int8_t   idHeight = 30; 
int8_t   idKp = 40; 
int8_t   idKd = 50; 
int8_t   idX = 60; 
int8_t   idUpHeight = 70;
int8_t   idStride = 80; 

int8_t   idStop = 90;
int8_t   idAd = 91;
int8_t   idBack = 92;
int8_t   idR = 93;
int8_t   idL = 94;
int8_t   idHi = 95;


int ret = 0;
int Data = 0, Mode = 0;


const int Stretch = 1;
const int Step = 2;
const int Advance = 3;
const int Back = 4;
const int Imu = 5;

const int rover = 11;
const int sedan = 12;
const int batmobile = 13;

//           ID 0  1   2   3  4    5   6  7
int offset[] = {0, 55, 0, 68, 0, -13, 28, -81}; 

//leg length
float L1 = 50.0, L2 = 50.0;//unit:mm


int period = 120, periodRx, height = 65, heightRx;
int upHeight = 10, upHeightRx, stride = 10, strideRx;
int x = 0, xRx;


float hX = 0.0, hY = 0.0, Kp = 2.0, Kd = 4.0;
int KpRx, KdRx;

Kalman kalmanX, kalmanY;
float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY;
unsigned long oldTime = 0, loopTime, nowTime;
float dt;
float theta_X = 0.0, theta_Y = 0.0;
float theta_Xdot = 0.0, theta_Ydot = 0.0;

BMI270Class BMI270;

/* Other functions */
int8_t configure_sensor(struct bmi2_dev *dev);

struct bmi2_sens_float sensor_data;

/* Sensor configuration */
int8_t configure_sensor(){
  int8_t rslt;
  uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_GYRO };

  struct bmi2_sens_config config[2];

  /* Configure the type of feature. */
  config[0].type = BMI2_ACCEL;

  /* NOTE: The user can change the following configuration parameters according to their requirement. */
  /* Set Output Data Rate */
  config[0].cfg.acc.odr = BMI2_ACC_ODR_800HZ;

  /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */
  config[0].cfg.acc.range = BMI2_ACC_RANGE_2G;

  /* The bandwidth parameter is used to configure the number of sensor samples that are averaged
   * if it is set to 2, then 2^(bandwidth parameter) samples
   * are averaged, resulting in 4 averaged samples.
   * Note1 : For more information, refer the datasheet.
   * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but
   * this has an adverse effect on the power consumed.
   */
  config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4;

  /* Enable the filter performance mode where averaging of samples
   * will be done based on above set bandwidth and ODR.
   * There are two modes
   *  0 -> Ultra low power mode
   *  1 -> High performance mode(Default)
   * For more info refer datasheet.
   */
  config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE;

  /* Configure the type of feature. */
  config[1].type = BMI2_GYRO;

  /* The user can change the following configuration parameters according to their requirement. */
  /* Set Output Data Rate */
  config[1].cfg.gyr.odr = BMI2_GYR_ODR_800HZ;

  /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */
  config[1].cfg.gyr.range = BMI2_GYR_RANGE_250;

  /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */
  config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE;

  /* Enable/Disable the noise performance mode for precision yaw rate sensing
   * There are two modes
   *  0 -> Ultra low power mode(Default)
   *  1 -> High performance mode
   */
  config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE;

  /* Enable/Disable the filter performance mode where averaging of samples
   * will be done based on above set bandwidth and ODR.
   * There are two modes
   *  0 -> Ultra low power mode
   *  1 -> High performance mode(Default)
   */
  config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE;

  /* Set the accel configurations. */
  rslt = BMI270.set_sensor_config(config, 2);
  if (rslt != BMI2_OK) return rslt;

  rslt = BMI270.sensor_enable(sens_list, 2);
  if (rslt != BMI2_OK) return rslt;

  return rslt;
}


//加速度センサから傾きデータ取得 [deg]
void getAccGyro() {
  BMI270.bmi2_get_sensor_float(&sensor_data);

  theta_X  = atan2(sensor_data.acc.y, sensor_data.acc.z) * 57.29578f;
  theta_Y  = atan2(-sensor_data.acc.x, sensor_data.acc.z) * 57.29578f;
  theta_Xdot = sensor_data.gyr.x;
  theta_Ydot = sensor_data.gyr.y;
}

void setup() {
  Serial.begin(115200, SERIAL_8N1);
  mySerial.begin(1000000);
  Serial2.begin(2000000, SERIAL_8N1);
  
  MP.begin(sub1);
  MP.RecvTimeout(MP_RECV_POLLING); //Polling 受信待ちなし
  delay(100);

  int8_t rslt = BMI270.begin(BMI270_I2C,BMI2_I2C_PRIM_ADDR);
  rslt = configure_sensor();
  delay(200); 

  for(int i = 0; i < 10; i++){
    getAccGyro();
    delay(2);
  }
  kalmanX.setAngle(theta_X);
  kalmanY.setAngle(theta_Y);

  EEPROM.get(idPeriod, periodRx);
  if(periodRx != 0)  period = periodRx;
  EEPROM.get(idHeight, heightRx);
  if(heightRx != 0)  height = heightRx;
  EEPROM.get(idX, xRx);
  if(xRx != 0)  x = xRx;
  EEPROM.get(idUpHeight, upHeightRx);
  if(upHeightRx != 0)  upHeight = upHeightRx;
  EEPROM.get(idStride, strideRx);
  if(strideRx != 0)  stride = strideRx;

  EEPROM.get(idKp, KpRx);
  if(KpRx != 0)  Kp = KpRx / 10.0;
  EEPROM.get(idKd, KdRx);
  if(KdRx != 0)  Kd = KdRx / 10.0;

  fRIK(x, height);
  fLIK(x, height);
  rLIK(x, height);
  rRIK(x, height);


  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(2000000);
  // 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.torqueOff(0);
  dxl.torqueOff(1);
  dxl.torqueOff(2);
  dxl.torqueOff(3);
  dxl.setOperatingMode(0, OP_PWM);
  dxl.setOperatingMode(1, OP_PWM);
  dxl.setOperatingMode(2, OP_PWM);
  dxl.setOperatingMode(3, OP_PWM);
  dxl.torqueOn(0);
  dxl.torqueOn(1);
  dxl.torqueOn(2);
  dxl.torqueOn(3);

  dxl.setGoalPWM(0, 0.0, UNIT_PERCENT);
  dxl.setGoalPWM(1, 0.0, UNIT_PERCENT);
  dxl.setGoalPWM(2, 0.0, UNIT_PERCENT);
  dxl.setGoalPWM(3, 0.0, UNIT_PERCENT);
}


void loop() {
  ret = MP.Recv(&id, &Data, sub1);
  if (ret == idMode) {
    if (Data == idAd) { //前進
      dxl.setGoalPWM(0, 20.0, UNIT_PERCENT);
      dxl.setGoalPWM(1, -20.0, UNIT_PERCENT);
      dxl.setGoalPWM(2, 20.0, UNIT_PERCENT);
      dxl.setGoalPWM(3, -20.0, UNIT_PERCENT);
    }else if(Data == idBack){ //後進
      dxl.setGoalPWM(0, -20.0, UNIT_PERCENT);
      dxl.setGoalPWM(1, 20.0, UNIT_PERCENT);
      dxl.setGoalPWM(2, -20.0, UNIT_PERCENT);
      dxl.setGoalPWM(3, 20.0, UNIT_PERCENT);
    }else if(Data == idR){ //右
      dxl.setGoalPWM(0, -20.0, UNIT_PERCENT);
      dxl.setGoalPWM(1, -20.0, UNIT_PERCENT);
      dxl.setGoalPWM(2, -20.0, UNIT_PERCENT);
      dxl.setGoalPWM(3, -20.0, UNIT_PERCENT);
    }else if(Data == idL){ //左
      dxl.setGoalPWM(0, 20.0, UNIT_PERCENT);
      dxl.setGoalPWM(1, 20.0, UNIT_PERCENT);
      dxl.setGoalPWM(2, 20.0, UNIT_PERCENT);
      dxl.setGoalPWM(3, 20.0, UNIT_PERCENT);
    }else if(Data == idHi){ //高速前進
      dxl.setGoalPWM(0, 80.0, UNIT_PERCENT);
      dxl.setGoalPWM(1, -80.0, UNIT_PERCENT);
      dxl.setGoalPWM(2, 80.0, UNIT_PERCENT);
      dxl.setGoalPWM(3, -80.0, UNIT_PERCENT);
    }else if(Data == idStop){ //停止
      dxl.setGoalPWM(0, 0.0, UNIT_PERCENT);
      dxl.setGoalPWM(1, 0.0, UNIT_PERCENT);
      dxl.setGoalPWM(2, 0.0, UNIT_PERCENT);
      dxl.setGoalPWM(3, 0.0, UNIT_PERCENT);
    }else{
      Mode = Data;
    }
  }else if (ret == idPeriod) {
    period = Data;
    EEPROM.put(idPeriod, period);
  }else if (ret == idHeight) {
    height = Data;
    EEPROM.put(idHeight, height);
  }else if (ret == idKp) {
    Kp = Data / 10.0;
    EEPROM.put(idKp, Data);
  }else if (ret == idKd) {
    Kd = Data / 10.0;
    EEPROM.put(idKd, Data);
  }else if (ret == idX) {
    x = Data;
    EEPROM.put(idX, Data);
  }else if (ret == idUpHeight) {
    upHeight = Data;
    EEPROM.put(idUpHeight, Data);
  }else if (ret == idStride) {
    stride = Data;
    EEPROM.put(idStride, Data);
  }
  
  
  if(Mode == Stretch){ //屈伸
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period * 8){
      tim=millis()-time_mSt;
      tt=float(tim * 2 * PI / (period * 8));
      fRIK(x, height + upHeight * sin(tt));
      fLIK(x, height + upHeight * sin(tt));
      rLIK(x, height + upHeight * sin(tt));
      rRIK(x, height + upHeight * sin(tt));
    }
  }else if(Mode == Step){ //足踏み
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x, height - upHeight * sin(tt));
      rLIK(x, height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x, height - upHeight * cos(tt));
      rLIK(x, height - upHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(x, height - upHeight * sin(tt));
      fLIK(x, height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(x, height - upHeight * cos(tt));
      fLIK(x, height - upHeight * cos(tt));
    }
  }else if(Mode == Advance){ //前進
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * cos(tt), height - upHeight * sin(tt));
      fLIK(x + stride * cos(tt), height);
      rLIK(x + stride * cos(tt), height - upHeight * sin(tt));
      rRIK(x - stride * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * sin(tt), height - upHeight * cos(tt));
      fLIK(x - stride * sin(tt), height);
      rLIK(x - stride * sin(tt), height - upHeight * cos(tt));
      rRIK(x + stride * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * cos(tt), height);
      fLIK(x - stride * cos(tt), height - upHeight * sin(tt));
      rLIK(x - stride * cos(tt), height);
      rRIK(x + stride * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * sin(tt), height);
      fLIK(x + stride * sin(tt), height - upHeight * cos(tt));
      rLIK(x + stride * sin(tt), height);
      rRIK(x - stride * sin(tt), height - upHeight * cos(tt));
    }
  }else if(Mode == Back){ //後進
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * cos(tt), height - upHeight * sin(tt));
      fLIK(x - stride * cos(tt), height);
      rLIK(x - stride * cos(tt), height - upHeight * sin(tt));
      rRIK(x + stride * cos(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * sin(tt), height - upHeight * cos(tt));
      fLIK(x + stride * sin(tt), height);
      rLIK(x + stride * sin(tt), height - upHeight * cos(tt));
      rRIK(x - stride * sin(tt), height);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x - stride * cos(tt), height);
      fLIK(x + stride * cos(tt), height - upHeight * sin(tt));
      rLIK(x + stride * cos(tt), height);
      rRIK(x - stride * cos(tt), height - upHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(x + stride * sin(tt), height);
      fLIK(x - stride * sin(tt), height - upHeight * cos(tt));
      rLIK(x - stride * sin(tt), height);
      rRIK(x + stride * sin(tt), height - upHeight * cos(tt));
    }
  }else if(Mode == Imu){    
    nowTime = micros();
    loopTime = nowTime - oldTime;
    oldTime = nowTime;
    
    dt = (float)loopTime / 1000000.0; //sec
  
    getAccGyro();
  
    //カルマンフィルタ 姿勢 傾き
    kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt);
    kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt);
    
    //カルマンフィルタ 姿勢 角速度
    kalAngleDotX = kalmanX.getRate();
    kalAngleDotY = kalmanY.getRate();


    Serial.print(Kp);
    Serial.print(",");
    Serial.println(Kd);

    
    hX = hX + Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 100.0;
    hY = hY + Kp * kalAngleX / 90.0 + Kd * kalAngleDotX / 100.0;


    float hs = hX - hY;
    hs = constrain(hs, -30, 30);
    
    float hd = hX + hY;
    hd = constrain(hd, -30, 30);


    fRIK(x, height - hd);
    fLIK(x, height - hs);
    rRIK(x, height + hs);
    rLIK(x, height + hd);
  }else if(Mode == rover){
    fRIK(0, 50);
    fLIK(0, 50);
    rLIK(0, 50);
    rRIK(0, 50);
  }else if(Mode == sedan){
    fRIK(80, 50);
    fLIK(80, 50);
    rLIK(70, 60);
    rRIK(70, 60);
  }else if(Mode == batmobile){
    fRIK(98, 15);
    fLIK(98, 15);
    rLIK(30, 15);
    rRIK(30, 15);
  }else if(Mode == 0){ //初期姿勢
    fRIK(x, height);
    fLIK(x, height);
    rLIK(x, height);
    rRIK(x, height);

    dxl.setGoalPWM(0, 0.0, UNIT_PERCENT);
    dxl.setGoalPWM(1, 0.0, UNIT_PERCENT);
    dxl.setGoalPWM(2, 0.0, UNIT_PERCENT);
    dxl.setGoalPWM(3, 0.0, UNIT_PERCENT);

    Serial.print(x);
    Serial.print(",");
    Serial.println(height);
  }
}

//参考 https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9
void sts_moveToPos(byte id, int position) {
  // コマンドパケットを作成
  byte message[13];
  message[0] = 0xFF;  // ヘッダ
  message[1] = 0xFF;  // ヘッダ
  message[2] = id;    // サーボID
  message[3] = 9;     // パケットデータ長
  message[4] = 3;     // コマンド（3は書き込み命令）
  message[5] = 42;    // レジスタ先頭番号
  message[6] = position & 0xFF; // 位置情報バイト下位
  message[7] = (position >> 8) & 0xFF; // 位置情報バイト上位
  message[8] = 0x00;  // 時間情報バイト下位
  message[9] = 0x00;  // 時間情報バイト上位
  message[10] = 0x00; // 速度情報バイト下位
  message[11] = 0x00; // 速度情報バイト上位

  // チェックサムの計算
  byte checksum = 0;
  for (int i = 2; i < 12; i++) {
    checksum += message[i];
  }
  message[12] = ~checksum; // チェックサム

  // コマンドパケットを送信
  for (int i = 0; i < 13; i++) {
    mySerial.write(message[i]);
  }
}


void servo_write(int ch, float ang){ 
  int sig = 2047 + offset[ch] + int((2048.0 / 180.0) * ang);
  sts_moveToPos(ch, sig);
  delayMicroseconds(500);
}



//-------------------------------------------
//IK
//-------------------------------------------
void fRIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);
  phi = atan2(-x, z);

  th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2));

  servo_write(4, th1 * 180.0 / PI);
  servo_write(6, th2 * 180.0 / PI);
}

void fLIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);
  phi = atan2(-x, z);
  
  th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2));

  servo_write(5, -(th1 * 180.0 / PI));
  servo_write(7, -(th2 * 180.0 / PI));
}

void rRIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);
  phi = atan2(-x, z);

  th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2));

  servo_write(1, -(th1 * 180.0 / PI));
  servo_write(3, -(th2 * 180.0 / PI));
}

void rLIK(float x, float z){
  float phi,ld, th1, th2;
  
  ld = sqrt(x*x + z*z);
  phi = atan2(-x, z);
  
  th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2));

  servo_write(0, th1 * 180.0 / PI);
  servo_write(2, th2 * 180.0 / PI);
}

UART2 (Serial2)でタイヤ用のシリアルサーボXL330を制御
SoftwareSerial (Tx：GPIO2)で足用シリアルサーボSTS3032を制御

BLE通信の文字を受けて動作モードや各種パラメータを設定
動作モードで歩行やバランス動作、タイヤ移動を制御
パラメータで歩行時の足の移動量やスピード、バランス動作時のPD制御係数などを設定

Sub1


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

#include <MP.h>
#include <SoftwareSerial.h>

SoftwareSerial mySerial(16, -1);    // RX,TX

int8_t   idMode = 10; 
int8_t   idPeriod = 20; 
int8_t   idHeight = 30; 
int8_t   idKp = 40; 
int8_t   idKd = 50; 
int8_t   idX = 60;
int8_t   idUpHeight = 70;  
int8_t   idStride = 80; 

int8_t   idStop = 90;
int8_t   idAd = 91;
int8_t   idBack = 92;
int8_t   idR = 93;
int8_t   idL = 94;
int8_t   idHi = 95;


String rcv;
int Data = 0;

void setup() {
  mySerial.begin(115200);
  delay(300);
  
  MP.begin();
}

void loop() {
  while(mySerial.available() != 0){
    rcv = mySerial.readStringUntil('\n');
    
    if (rcv.indexOf('t') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idPeriod, Data);
    }else if (rcv.indexOf('h') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idHeight, Data);
    }else if (rcv.indexOf('p') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idKp, Data);
    }else if (rcv.indexOf('d') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idKd, Data);
    }else if (rcv.indexOf('x') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idX, Data);
    }else if (rcv.indexOf('u') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idUpHeight, Data);
    }else if (rcv.indexOf('s') != -1) {
      Data = rcv.substring(1).toInt();
      MP.Send(idStride, Data);
    }else{
      Data = rcv.toInt();
      MP.Send(idMode, Data);
    }
  }
}

SoftwareSerial (Rx：GPIO16)でBLE1507と通信
スマホからの数字や文字を受信して、それぞれ対応したIDでMainコアにデータを渡す。

おわりに

ここではSPRESENSEによる足先がタイヤの4脚ロボット Wheel-Legged Robot の製作を楽しみました。

これまでSPRESENSEを用いてロボットを製作しようという発想はなかったのですが、本コンテストに"ロボット部門"が設立されていることから真正面からSPRESENSEによるロボット製作に挑みました。

SPRESENSE自身の多様なインターフェースとマルチコアCPUによって多機能化が容易であり、専用のセンサや通信ボードのよる拡張で見事にロボットを作りきることができました。

前回のコンテスト(2022)ではSPRESENSEの省エネ機能を活用してバッテリレス監視システムを製作しました。
今回はSPRESENSEの豊富な物理資産を活用して多機能ロボットを製作し、SPRESENSEのハードウェアとしての有用性を強く実感することができました。

次回はいよいよSPRESENSEのAI機能について学習し活用方法を考えられればと夢が広がっております。
このような気づきの機会を与えてくださったコンテストやサイトの運営様・関係者様そして素晴らしいデバイスを提供くださった企業様に感謝申し上げます。

コメント無し