ロボット：息子と一緒に Makers：So-netブログ

	ブログをはじめるログイン

【備忘録】 micro-ROS の micro_ros_agent と micro-ROS static library の作り方　 [ロボット]

micro-ROS の micro_ros_agent と micro-ROS の static library を作ってみたので、その手順を忘れないように記録として残しておきます。基本的には、次のサイトの手順通りです。

環境は、Ubuntu 22.04 LTS で ROS2 のバージョンは humble です。

Teensy with Arduino
Creating custom static micro-ROS library

■ micro_ros_setup の設定

micro_ros_agent と micro-ROS static library をビルドするための micro_ros_setup の環境を設定します。

# Source the ROS 2 installation
source /opt/ros/$ROS_DISTRO/setup.bash

# Create a workspace and download the micro-ROS tools
mkdir microros_ws
cd microros_ws
git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup

# Update dependencies using rosdep
sudo apt update && rosdep update
rosdep install --from-paths src --ignore-src -y

# Install pip if you need
sudo apt-get install python3-pip

# Build micro-ROS tools and source them
colcon build
source install/local_setup.bash

■ micro_ros_agent のビルド

micro_ros_agent のビルド環境をダウンロードし、ビルドします。

# Download micro-ROS agent packages
ros2 run micro_ros_setup create_agent_ws.sh

# Build micro_ros_agent
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash

私の環境では、次のエラー出てビルドが失敗してしまいました。

$HOME/microros_ws/src/uros/micro-ROS-Agent/micro_ros_agent/src/agent/graph_manager/graph_typesupport.cpp:47:54: error: cannot bind non-const lvalue reference of type ‘bool&’ to a value of type ‘char’
   47 |     m_typeSize = 4 + callbacks_->max_serialized_size(full_bounded);
      |                                                      ^~~~~~~~~~~~
$HOME/microros_ws/src/uros/micro-ROS-Agent/micro_ros_agent/src/agent/graph_manager/graph_typesupport.cpp:139:54: error: cannot bind non-const lvalue reference of type ‘bool&’ to a value of type ‘char’
  139 |     m_typeSize = 4 + callbacks_->max_serialized_size(full_bounded);
      |

これは次のファイルの47行目と139行目を次のように変更したらビルドが通りました。

$HOME/microros_ws/src/uros/micro-ROS-Agent/micro_ros_agent/src/agent/graph_manager/graph_typesupport.cpp

 46     // char full_bounded;
 47     bool full_bounded;
....
139     // char full_bounded;
140     bool full_bounded;
....

micro_ros_agent は次のコマンドで起動することができます。

# run micro_ros_agent by a serial communication
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0

■ micro-ROS static library の環境準備

micro-ROS static library の開発環境をダウンロードします。

# Download micro-ROS firmware packages
ros2 run micro_ros_setup create_firmware_ws.sh generate_lib

ビルドするための設定ファイルを２つ作成します。

touch my_custom_toolchain.cmake
touch my_custom_colcon.meta

それぞれのファイルの設定例を示します。今回はSPRESENSEを前提とした設定ファイルです。

my_custom_colcon.meta

{
    "names": {
        "tracetools": {
            "cmake-args": [
                "-DTRACETOOLS_DISABLED=ON",
                "-DTRACETOOLS_STATUS_CHECKING_TOOL=OFF"
            ]
        },
        "rosidl_typesupport": {
            "cmake-args": [
                "-DROSIDL_TYPESUPPORT_SINGLE_TYPESUPPORT=ON"
            ]
        },
        "rcl": {
            "cmake-args": [
                "-DBUILD_TESTING=OFF",
                "-DRCL_COMMAND_LINE_ENABLED=OFF",
                "-DRCL_LOGGING_ENABLED=OFF"
            ]
        }, 
        "rcutils": {
            "cmake-args": [
                "-DENABLE_TESTING=OFF",
                "-DRCUTILS_NO_FILESYSTEM=OFF",
                "-DRCUTILS_NO_THREAD_SUPPORT=OFF",
                "-DRCUTILS_NO_64_ATOMIC=ON",
                "-DRCUTILS_AVOID_DYNAMIC_ALLOCATION=ON"
            ]
        },
        "microxrcedds_client": {
            "cmake-args": [
                "-DUCLIENT_PIC=OFF",
                "-DUCLIENT_PROFILE_UDP=OFF",
                "-DUCLIENT_PROFILE_TCP=OFF",
                "-DUCLIENT_PROFILE_DISCOVERY=OFF",
                "-DUCLIENT_PROFILE_SERIAL=OFF",
                "-UCLIENT_PROFILE_STREAM_FRAMING=ON",
                "-DUCLIENT_PROFILE_CUSTOM_TRANSPORT=ON"
            ]
        },
        "rmw_microxrcedds": {
            "cmake-args": [
                "-DRMW_UXRCE_MAX_NODES=1",
                "-DRMW_UXRCE_MAX_PUBLISHERS=5",
                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                "-DRMW_UXRCE_MAX_SERVICES=1",
                "-DRMW_UXRCE_MAX_CLIENTS=1",
                "-DRMW_UXRCE_MAX_HISTORY=4",
                "-DRMW_UXRCE_TRANSPORT=custom"
            ]
        }
    }
}

my_custom_toolchain.cmake

set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_CROSSCOMPILING 1)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)

# SET HERE THE PATH TO YOUR C99 AND C++ COMPILERS
set(CMAKE_C_COMPILER $ENV{TOOLCHAIN_PREFIX}gcc)
set(CMAKE_CXX_COMPILER $ENV{TOOLCHAIN_PREFIX}g++)

set(NUTTX_INCLUDE_DIR "$ENV{HOME}/.arduino15/packages/SPRESENSE/tools\
/spresense-sdk/3.0.1/spresense/release/nuttx/include")
set(ARDUINO_INCLUDE_DIR "$ENV{HOME}/.arduino15/packages/SPRESENSE/hardware/\
spresense/3.0.1/cores/spresense")

set(CMAKE_C_COMPILER_WORKS 1 CACHE INTERNAL "")
set(CMAKE_CXX_COMPILER_WORKS 1 CACHE INTERNAL "")

# SET HERE YOUR BUILDING FLAGS
set(FLAGS "-c -fno-builtin -mabi=aapcs -Wall -fno-strict-aliasing -Os \
-fno-strength-reduce -fomit-frame-pointer -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 \
-mfloat-abi=hard -mthumb -nostdlib -pipe -ggdb -gdwarf-3 -ffunction-sections \
-fdata-sections -DCONFIG_WCHAR_BUILTIN -DCONFIG_HAVE_DOUBLE -D__NuttX__ \
-D'RCUTILS_LOG_MIN_SEVERITY=RCUTILS_LOG_MIN_SEVERITY_NONE\
'" CACHE STRING "" FORCE)

set(CMAKE_C_FLAGS_INIT "-w -MMD -std=gnu11 ${FLAGS} -DCLOCK_MONOTONIC=0 \
-D'__attribute__(x)='" CACHE STRING "" FORCE)
set(CMAKE_CXX_FLAGS_INIT "-w -MMD -std=gnu++11 ${FLAGS} -fno-exceptions \
-fno-rtti -fpermissive -DCLOCK_MONOTONIC=0 \
-D'__attribute__(x)='" CACHE STRING "" FORCE)

include_directories(BEFORE SYSTEM ${NUTTX_INCLUDE_DIR} ${ARDUINO_CINLUDE_DIR})

set(__BIG_ENDIAN__ 0)

$TOOLCHAIN_PREFIX は、".bashrc"の中で定義されています。

.bashrc

export TOOLCHAIN_PREFIX='$HOME/.arduino15/packages/
SPRESENSE/tools/gcc-arm-none-eabi/9.2.1/linux/bin/arm-none-eabi-'

micro-ROS static library をビルドします。

# Build micro-ROS static library
ros2 run micro_ros_setup build_firmware.sh $(pwd)/my_custom_toolchain.cmake $(pwd)/my_custom_colcon.meta

ビルドに成功すると、次の場所に static library と必要な include ファイルが生成されます。

firmware/build/libmicroros.a
firmware/include

これでひととおり micro-ROS で開発する環境を整えることができました！

ロボットプログラミングROS2の実装・実践 -実用ロボットの開発- (設計技術シリーズ96)

作者: 内木場文男
出版社/メーカー: 科学情報出版株式会社
発売日: 2021/07/21
メディア: 単行本

ROS２の基礎とJetBotの制御: ROS2 Foxyを使ったプログラミング

作者: 大西和則
出版社/メーカー:
発売日: 2021/06/19
メディア: Kindle版

タグ：ROS2 micro-ROS

2023-06-04 22:47 nice!(9) コメント(0)
共通テーマ：趣味・カルチャー

ROS2/Humble で Turtlebot3 Navigation がうまく動かなかった話　 [ロボット]

すっかりブログをサボっていましたが、そろそろ再開しようと思います。忙しくなると、ついついブログの存在を忘れてしまうんですよねぇ。

最近は、ROS2/micro-ROS をさわりはじめています。ROSとは「Robot OS」の略で、ホストPCとロボット間の通信や、ロボットから送られるセンシングデータを使ってホストPC側で、SLAM や Navigation などを行えるライブラリならびにツール群です。

こちらの記事を参照して、ROS2/humble を使って Turtlebot3 のナビゲーションのシミュレーションを試そうと思ったのですが、うまく動きません。具体的には、SLAMで作成した地図が rviz2 で表示されないのです。
＃上の画像はうまくいったあとの画像です。

エラーメッセージを手がかりに、いろいろと調べてようやくたどり着いた解決方法がこちら。

AMCL failing when starting Navigation Simulation
https://github.com/ROBOTIS-GIT/turtlebot3/issues/884

具体的には、"/opt/ros/humble/share/turtlebot3_navigation2/param/burger.yaml" の中の "robot_model_type" を変更すれば回避できます。

	変更前：robot_model_type:"differential" 変更後：robot_model_type: "nav2_amcl::DifferentialMotionModel"

Github の Issues で解決策が見つかるあたり、まだまだ発展途上のプラットフォームですね。ただ可能性を感じるテクノロジーですので、これから追いかけていきたいと思います。
（＾＾）／～

ROS2とPythonで作って学ぶAIロボット入門 (KS理工学専門書)

出版社/メーカー: 講談社
発売日: 2022/09/01
メディア: 単行本（ソフトカバー）

ロボットプログラミングROS2入門 (エンジニア入門)

作者: 岡田浩之
出版社/メーカー: 科学情報出版
発売日: 2022/10/07
メディア: Kindle版

タグ：Slam Navigation2 ROS2

2022-10-27 20:00
共通テーマ：趣味・カルチャー

【悲報】SPR倒立振子の反省点途中まとめ（＋モータードライバ死亡(´；ω；｀)）　[ロボット]

SPRESENSEで倒立振子にチャレンジしようと思い作っていたモータードライバを焦がしてしまいました。うっかり電源を逆に差した瞬間にプシュー…(・・)

今、手持ちの予備のモータードライバはなく、相変わらず私はテレワークなので、買い出しに行くきっかけもなく、しばらくお預けになりそうです。惜しいところまで来たのですが残念。

ただ、ここで倒立振り子を作る上での注意点ならびに設計の肝が分かってきたので、少しまとめたいと思います。

■　倒立振子の重心は下のほうに！
メカを検討する際は倒立振り子の重心は下の方にいくようにしましょう。私の機体は上に回路やバッテリなどを当初置くようにしていたのですが、トップヘビーになりすぎてバランスがとれませんでした。

冷静に考えればすぐに分かるのですが、重心に上にあると倒れるときの速度が早く、反応の鈍いDCモーターでは追従できません。センサーの反応が早くてもモーターが反応できないので制御はできません。

■　センサーの位置は重心近くに！
センサーの位置が上にあると、倒れるときに加速度センサーの値が大きくなり、制御が過剰に反応します。倒立振子の場合、細かい角度に反応するようにするためPIDの係数は大きくなりがちです。そのため、ちょっとの変化に大きく反応してしまいます。

重心に近ければ加速度センサーではなくジャイロの変化が支配的になるので、制御しやすくなります。

■　DCモーターには電圧可変出力のモータードライバで
今回、DCモーターの制御はFETを経由してPWMで出力するドライバを使いました。PWMだと微小な値の場合、DCモーターを起動する電力が不足するため、最初に十分な電圧を加え、その後、パルスを送るようにする必要がありました。

このような制御だと連続して前後に変化する値の場合は、DCモーターの反応が悪いので一瞬ブレーキがかかるようになってしまい、スムーズな動きを実現するのは極めて難しかったです。

またモータードライバとPWMの関係もリニアではないため、非線形にモーターを制御する必要があります。倒立振子のように微妙なバランスを取るために微小パワーで正負制御する用途には向かないようです。

DCモーターを制御する場合は、DRV8830のようにインターフェースから数値を指定して出力電圧を変化させるモータードライバを選択したほうが無難かも知れません。

■　制御間隔を短くすればいいもんでもない
DCモーターは反応が悪いです。ですのでセンサー処理がいくら早くフィードバックしてもモーターがついてこれません。今回使ったモータードライバはモーターの駆動に癖があるのでそれで時間がかかってしまいます。

また、逆方向に動かそうとしても、PWMでは慣性力をはるかに上回るパワー（パワー０になる期間があるため）が伝えられないため、俊敏に動かせません。モータードライバの癖にあわせて制御間隔を調整する必要がありそうです。

■　PID制御式の選択
今回は、PID制御の式は次の２種類について試してみました。

一般的なPID制御式

\[
Power(t) = KP (\theta (t) - offset) + KD \int_0^t (\theta(t) - offset)\,dt + KI \frac{d (\theta(t) - offset)}{dt}\
\]

センサー値を使ったPID制御式

\[
Power(t) = K1 (\theta(t) - offset) + K2 \omega(t) + K3 \nu(t) + K4 \int_0^t \nu(t)\,dt
\]

ここでθは車体の傾き。オフセットは傾きの初期値、ω は角速度、v は並進速度になります。

両方ためしてみたのですが、どうもセンサー値を使ったPIDの制御式のほうが安定するようです。ただ、センサー値に敏感に反応するのでセンサーは重心に近いところに置いたほうが無難です。

■　フィルターはMadgwickのほうが良い？
SPRESENSEはサブコアがあるのでフィルタ処理は計算し放題だったので、Kalman、Madgwick、Complemntary の三種類を同時に処理して値を返すようにしました。

Kalmanフィルターは遅延があるので、トップヘビー＋FET型モータードライバというハンディキャップがある中ではかなり苦しみました。

Madgwickフィルターは微小な変化には反応が早いので倒立振子には向いてそうです。ただ、大きな変化では収束に時間がかかるので、扱いにはやや注意が必要です。

以上の反省点をふまえ、SPR倒立振子２を検討したいと思います！
( ･ิω･ิ)

Arduino電子工作: ２輪自立ロボットカーの製作

作者: 大西　和則
出版社/メーカー:
発売日: 2019/08/21
メディア: Kindle版

OSOYOO アルデュイーノ UNO R3 二輪自立ロボットカープログラミング教育スターターキット Android 対応

出版社/メーカー: オソヨー(OSOYOO)
メディア: おもちゃ＆ホビー

倒立振子制御学習用ロボットビュートバランサー2 [学習教材] [vstone]

出版社/メーカー: Vstone(ヴイストン株式会社)
メディア:

タグ：モーター制御倒立振子 Spresense

2020-10-05 23:58 nice!(24) コメント(0)
共通テーマ：趣味・カルチャー

【SPRESENSE】BMI160用のセンサーフュージョン・フィルターを検討する（２）　[ロボット]

前回、SPRESENSEでセンサーフュージョン用のフィルターを検討しましたが、グラフでは効果がよく分からないので、今回は少し工夫をこらして Processing で表示しながら反応を見ることにしました。

Processingで作ったプログラムのグラフィック画面を次のようにしてみました。

2020-06-14 (1).png

Spresense のスケッチは次のようなものです。前回のものと出力形式以外は全く変わっていません。

#include <Wire.h>
#include <math.h>
#include <BMI160Gen.h>
#include <Kalman.h>
#include <MadgwickAHRS.h>

#define PRINT_PROCESSING

#define BAUDRATE  115200
#define SENSE_RATE   100
#define GYRO_RANGE   250
#define ACCL_RANGE     2

#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)

Kalman kalmanRoll;
Kalman kalmanPitch;
Madgwick madgwick;

float convertRawGyro(int gRaw) {
  // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
  float lsb_omega = float(0x7FFF) / GYRO_RANGE;
  return gRaw / lsb_omega;  // deg/sec
}

float convertRawAccel(int aRaw) {
  // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
  float lsb_g = float(0x7FFF) / ACCL_RANGE;
  return aRaw / lsb_g;
}

static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;
static float comp_roll = 0, comp_pitch = 0;
static unsigned long last_mills = 0; 
void print_roll_pitch() 
{
     
  // read raw accl measurements from device
  int rawXAcc, rawYAcc, rawZAcc; // x, y, z
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);

  float rad_a_roll = atan2(accY, accZ);
  float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ));
  float accl_roll = rad_to_deg(rad_a_roll);
  float accl_pitch = rad_to_deg(rad_a_pitch);

  // read raw gyro measurements from device
  int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
  BMI160.readGyro(rawRoll, rawPitch, rawYaw);
  float omega_roll  = convertRawGyro(rawRoll);
  float omega_pitch = convertRawGyro(rawPitch);
  float omega_yaw   = convertRawGyro(rawYaw);
  
  unsigned long cur_mills = micros();
  unsigned long duration = cur_mills - last_mills;
  last_mills = cur_mills;
  double dt = duration / 1000000.0; // us->s  
  if (dt > 0.1) return;

  // Gyro data
  gyro_roll  += omega_roll  * dt; // (ms->s) omega x time = degree
  gyro_pitch += omega_pitch * dt;
  gyro_yaw   += omega_yaw   * dt;
  
  // Complementary filter data
  comp_roll  = 0.93 * (comp_roll  + omega_roll  * dt) + 0.07 * accl_roll; 
  comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; 

  // Kalman filter data
  float kalm_roll  = kalmanRoll.getAngle(accl_roll, omega_roll, dt);
  float kalm_pitch = kalmanPitch.getAngle(accl_pitch, omega_pitch, dt); 

  // Madgwick filter data
  madgwick.updateIMU2(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ, dt);
  // madgwick.updateIMU(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ);
  float madw_roll  = madgwick.getRoll();
  float madw_pitch = madgwick.getPitch();
  float madw_yaw   = madgwick.getYaw();

#ifdef PRINT_PROCESSING
  static int n = 0;
  if (n != 50) {
    ++n; return; 
  }
  n = 0;

  Serial.print(accl_roll);
  Serial.print(",");
  Serial.print(accl_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(gyro_roll);
  Serial.print(",");
  Serial.print(gyro_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(comp_roll);
  Serial.print(",");
  Serial.print(comp_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(kalm_roll);
  Serial.print(",");
  Serial.print(kalm_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(madw_roll);
  Serial.print(",");
  Serial.print(madw_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
#endif
  Serial.println();
}


void setup() {
  Serial.begin(115200);
  BMI160.begin(BMI160GenClass::I2C_MODE);
  madgwick.begin(SENSE_RATE);

  BMI160.setGyroRate(SENSE_RATE);
  BMI160.setAccelerometerRate(SENSE_RATE);
  BMI160.setGyroRange(GYRO_RANGE);
  BMI160.setAccelerometerRange(ACCL_RANGE);

  BMI160.autoCalibrateGyroOffset();
  BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  delay(100);

  /* Set kalman and gyro starting angle */
  int rawXAcc, rawYAcc, rawZAcc;
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);
  
  float roll  = rad_to_deg(atan(accY / accZ));
  float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ)));

  // Set starting angle
  kalmanRoll.setAngle(roll); 
  kalmanPitch.setAngle(pitch);
  gyro_roll  = comp_roll  = roll;
  gyro_pitch = gyro_pitch = pitch;
}

void loop() {
  print_roll_pitch();
}

Processing のスケッチは次のようになっています。

import processing.serial.*;
Serial myPort;  // Create object from Serial class

float [] Acc = new float[3];
float [] Gyr = new float[3];
float [] Cmp = new float[3];
float [] Klm = new float[3];
float [] Mdw = new float[3];

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[1024];

PFont font;
final int VIEW_SIZE_X = 1100, VIEW_SIZE_Y = 600;

void settings() {
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
}  

void setup() {
  myPort = new Serial(this, "COM15", 115200); 
  font = loadFont("CourierNew36.vlw"); 
}


void readSensors() {
  if(myPort.available() > 0) {
    if (myPort.readBytesUntil(lf, inBuffer) > 0) {
      String inputString = new String(inBuffer);
      String [] inputStringArr = split(inputString, ',');
      
      Acc[0] = float(inputStringArr[0]);
      Acc[1] = float(inputStringArr[1]);
      Acc[2] = float(inputStringArr[2]);
      Gyr[0] = float(inputStringArr[3]);
      Gyr[1] = float(inputStringArr[4]);
      Gyr[2] = float(inputStringArr[5]);
      Cmp[0] = float(inputStringArr[6]);
      Cmp[1] = float(inputStringArr[7]);
      Cmp[2] = float(inputStringArr[8]);
      Klm[0] = float(inputStringArr[9]);
      Klm[1] = float(inputStringArr[10]);
      Klm[2] = float(inputStringArr[11]);
      Mdw[0] = float(inputStringArr[12]);
      Mdw[1] = float(inputStringArr[13]);
      Mdw[2] = float(inputStringArr[14]);
    }
  }
}


void normalize3DVec(float [] vector) {
  float R;
  R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  vector[0] /= R;
  vector[1] /= R;  
  vector[2] /= R;  
}


void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);
  
  //Z+ (to the drawing area)
  fill(#00ff00);
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  //Z-
  fill(#0000ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);
  
  //X-
  fill(#ff0000);
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);
  
  //X+
  fill(#ffff00);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);
  
  //Y-
  fill(#ff00ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);
  
  //Y+
  fill(#00ffff);
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  endShape();
}


void drawCube(int n, float vec[]) {  
  pushMatrix();
    translate(n, 450, 0);
    scale(3,3,3);
    
    rotateX(HALF_PI * vec[0]);
    rotateZ(HALF_PI * -vec[1]);
    
    buildBoxShape();
  popMatrix();
}

void draw() {  
  readSensors();
  normalize3DVec(Acc);
  normalize3DVec(Gyr);
  normalize3DVec(Cmp);
  normalize3DVec(Klm);
  normalize3DVec(Mdw);
  
  background(#000000);
  fill(#ffffff);

  textFont(font, 24);
  text("Sensor Fusion Test using Spresense with BMI160", 180, 100);
  
  textFont(font, 18);
  text("Accelerometer :\n" + Acc[0] + "\n" + Acc[1], 50, 180);
  drawCube(150, Acc);

  text("Gyroscope :\n" + Gyr[0] + "\n" + Gyr[1], 250, 180);
  drawCube(350, Gyr);
  
  text("Comp. Filter :\n" + Cmp[0] + "\n" + Cmp[1], 450, 180);
  drawCube(550, Cmp);

  text("Kalman Filter :\n" + Klm[0] + "\n" + Klm[1], 650, 180);
  drawCube(750, Klm);
  
  text("Madgwick Filter :\n" + Mdw[0] + "\n" + Mdw[1], 850, 180);
  drawCube(950, Mdw);
}

百聞は一見にしかず、ぜひ動画で見てみてください。どのフィルタがどのような性質を持っているかグラフィックで見てみるとひと目で分かります！ 😀
（＾＾）／～

倒立振子で学ぶ制御工学

出版社/メーカー: 森北出版
発売日: 2017/03/03
メディア: 単行本（ソフトカバー）

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサーモジュールIIC SPIブロード

出版社/メーカー: SODIAL
メディア: その他

タグ：倒立振子 Spresense BMI160

2020-06-14 23:59 nice!(21) コメント(0)
共通テーマ：趣味・カルチャー

【SPRESENSE】BMI160用のセンサーフュージョン・フィルターを検討する（１）　[ロボット]

しばらく Python で遊んでいましたが、倒立振子も忘れたわけではありません。今回は、BMI160用のフィルターを検討したいと思います。

今回、試してみるフィルターは次の３つです。

・ Complementary Filter
・ Kalman Filter
・ Madgwick Filter

それぞれに対して簡単に紹介します。

■ Complementary Filter
Complementary Filter は、ジャイロデータの積分値と加速度データで割り出したロールとピッチをある比（0.93:0.02）で足したものを出力とするものです。非常に単純で、応答速度が早いですが、ノイズも大きいのが欠点です。英語ですがこちらの記事が参考になります。

Reading a IMU Without Kalman: The Complementary Filter

■ Kalman Filter
Kalman Filter は、もう説明が必要のないくらい有名なフィルターです。Arduino用のライブラリがありますので、こちらをダウンロードして使いました。

Kalman Filter Library

カルマンフィルターの詳細を知りたい方はこちらの記事が参考になります。

カルマンフィルタの考え方

■ Madgwick Filter
Madgwick Filter は、少しマイナーですがカルマンフィルター同様に自己位置推定によく使われるフィルターです。カルマンフィルターより応答が早いのが特長ですが、収束が遅いと言われています。Madgwick Filter も Arduino ライブラリがあるのでこちらを使用しました。

Madgwick

Madgwick フィルターの詳細を知りたい方はこちらの記事が参考になると思います。

Madgwick Filterを読んでみた

■ Madgwick Filter を改造する
使用した Madgwick Filter はデータのサンプリング周波数が固定値でしか与えられません。時間保証ができるのであればよいのですが、SPRESENSEはOS上で走っているので若干のジッターは避けられません。ですので、サンプリング間隔を計算の度に与えられる関数を追加しました。

// MadgwickAHRS.h に追加
void updateIMU2(float gx, float gy, float gz, float ax, float ay, float az, double dt);

// MadgwickAHRS.cpp に追加
void Madgwick::updateIMU2(
           float gx, float gy, float gz, float ax, float ay, float az, double dt) {
	invSampleFreq = 1.0f / (1.0f / dt);
    	updateIMU(gx, gy, gz, ax, ay, az);
}

■　SPRESENSE用スケッチを書く
それぞれのフィルターがどのように振る舞うか調べるために次のスケッチを書いてみました。

#include <Wire.h>
#include <math.h>
#include <BMI160Gen.h>
#include <Kalman.h>
#include <MadgwickAHRS.h>

#define PRINT_ROLL
// #define PRINT_PITCH

#define BAUDRATE  115200
#define SENSE_RATE   100
#define GYRO_RANGE   250
#define ACCL_RANGE     2

#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)

Kalman kalmanRoll;
Kalman kalmanPitch;
Madgwick madgwick;

float convertRawGyro(int gRaw) {
  // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
  float lsb_omega = float(0x7FFF) / GYRO_RANGE;
  return gRaw / lsb_omega;  // deg/sec
}

float convertRawAccel(int aRaw) {
  // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
  float lsb_g = float(0x7FFF) / ACCL_RANGE;
  return aRaw / lsb_g;
}

static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;
static float comp_roll = 0, comp_pitch = 0;
static unsigned long last_mills = 0; 
void print_roll_pitch() 
{     
  // read raw accl measurements from device
  int rawXAcc, rawYAcc, rawZAcc; // x, y, z
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);

  float rad_a_roll = atan2(accY, accZ);
  float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ));
  float accl_roll = rad_to_deg(rad_a_roll);
  float accl_pitch = rad_to_deg(rad_a_pitch);

  // read raw gyro measurements from device
  int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
  BMI160.readGyro(rawRoll, rawPitch, rawYaw);
  float omega_roll  = convertRawGyro(rawRoll);
  float omega_pitch = convertRawGyro(rawPitch);
  float omega_yaw   = convertRawGyro(rawYaw);
  
  unsigned long cur_mills = micros();
  unsigned long duration = cur_mills - last_mills;
  last_mills = cur_mills;
  double dt = duration / 1000000.0; // us->s  
  if (dt > 0.1) return;

  // Gyro data
  gyro_roll  += omega_roll  * dt; // (ms->s) omega x time = degree
  gyro_pitch += omega_pitch * dt;
  gyro_yaw   += omega_yaw   * dt;
  
  // Complementary filter data
  comp_roll  = 0.93 * (comp_roll  + omega_roll  * dt) + 0.07 * accl_roll; 
  comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; 

  // Kalman filter data
  float kalm_roll  = kalmanRoll.getAngle(accl_roll, omega_roll, dt);
  float kalm_pitch = kalmanPitch.getAngle(accl_pitch, omega_pitch, dt); 

  // Madgwick filter data
  madgwick.updateIMU2(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ, dt);
  float madw_roll  = madgwick.getRoll();
  float madw_pitch = madgwick.getPitch();
  float madw_yaw   = madgwick.getYaw();

#ifdef PRINT_ROLL
  Serial.print(cur_mills);  // sampling time
  Serial.print(",");
  Serial.print(accl_roll);  // roll data calculated by the accelerometer
  Serial.print(",");
  Serial.print(gyro_roll);  // roll data calculated by the gyroscope
  Serial.print(",");
  Serial.print(comp_roll);  // roll data calculated by the complementary filter
  Serial.print(",");
  Serial.print(kalm_roll);  // roll data calculated by the kalman filter
  Serial.print(",");
  Serial.print(madw_roll);  // roll data calculated by the madgwick filter
#endif

#ifdef PRINT_PITCH
  Serial.print(cur_mills);    // sampling time
  Serial.print(",");
  Serial.print(accl_pitch);  // pitch data calculated by the acclerometer
  Serial.print(",");
  Serial.print(gyro_pitch);  // pitch data calculated by the gyroscope 
  Serial.print(",");
  Serial.print(comp_pitch);  // pitch data calculated by the complemtary filter
  Serial.print(",");
  Serial.print(kalm_pitch);  // pitch data calculated by the kalman fitler
  Serial.print(",");
  Serial.print(madw_pitch);  // pitch data calculated by the madgwick filter
#endif
  Serial.println();
}

void setup() {
  Serial.begin(115200);
  BMI160.begin(BMI160GenClass::I2C_MODE);
  madgwick.begin(SENSE_RATE);

  BMI160.setGyroRate(SENSE_RATE);
  BMI160.setAccelerometerRate(SENSE_RATE);
  BMI160.setGyroRange(GYRO_RANGE);
  BMI160.setAccelerometerRange(ACCL_RANGE);

  BMI160.autoCalibrateGyroOffset();
  BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  delay(100);

  /* Set kalman and gyro starting angle */
  int rawXAcc, rawYAcc, rawZAcc;
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);
  
  float roll  = rad_to_deg(atan(accY / accZ));
  float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ)));

  // Set starting angle
  kalmanRoll.setAngle(roll); 
  kalmanPitch.setAngle(pitch);
  gyro_roll  = comp_roll  = roll;
  gyro_pitch = gyro_pitch = pitch;
}

void loop() {
  print_roll_pitch();

}

■　出力データを確認する
ロール方向、ピッチ方向にそれぞれ筐体を振ってみてデータを収集しました。

スライド2.png

それぞれのデータはこちらです。

2020-06-07 (3).png

どのフィルタもジャイロのドリフトはキャンセルしてくれているのは分かりますが、グラフだけではどれが良いのかピンと来ませんね。もう少しうまく評価できる方法を考えてみたいと思います。🤨
(-ω-;)

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサーモジュールIIC SPIブロード

出版社/メーカー: SODIAL
メディア: その他

トコトンやさしい自動運転の本 (今日からモノ知りシリーズ)

作者: クライソン・トロンナムチャイ
出版社/メーカー: 日刊工業新聞社
発売日: 2018/03/29
メディア: 単行本

タグ：Spresense BMI160 倒立振子

2020-06-07 23:40 nice!(26) コメント(4)
共通テーマ：趣味・カルチャー

SPRESENSE と BMI160アドオンボードで傾きを測ってみた！（１）　[ロボット]

前回、SPRESENSEでモーターの回転数を測ってみました。今回は、スイッチサイエンスから出ている BMI160アドオンボードを使って、倒立振子の車体の傾きを検出してみたいと思います。

SPRESENSE用3軸加速度・3軸ジャイロ・気圧・温度センサアドオンボード BMP280 BMI160搭載
https://www.switch-science.com/catalog/5258/

このボードはジャイロと加速度センサーが搭載されています。今回はそれぞれのセンサーで出力される値を比較してみたいと思います。加速度センサーとジャイロセンサーの座標軸は次のようになっています。

加速度センサーの座標軸

スライド1.png

ジャイロセンサーの座標軸

スライド2.png

ジャイロセンサーからは角加速度が出力されますので、角度は時間積分で求まります。

\[
\theta = \int_{0}^{t} \omega(t) \, dt
\]

加速度センサーで角度を出すには、重力を使います。重力加速度は常に下向きになりますので、角度は例えばZ成分とY成分のatan で求まります。プログラミング時は角度とラジアンの単位の違いに注意してください。

スライド3.png

あともう一点、注意しないといけないのは、置いた場所が水平とは限らないことです。何もしないと各軸への重力影響が取り除けず正しい角度が算出できません。開始時に各軸の重力加速度を計測をしておき、制御時はその影響を取り除く必要があります。（プログラム中では"Calibration"と呼んでいます）

これらのことを踏まえて、次のようなスケッチを書いてみました。

#include <Wire.h>
#include <math.h>
#include <BMI160Gen.>

#define BAUDRATE  115200
#define SENSE_RATE   200
#define GYRO_RANGE   250
#define ACCL_RANGE     2

#define COUNT_FOR_CALIB 1024

#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)

#define PRINT_ACCL
#define PRINT_GYRO
// #define PRINT_YAW

uint16_t adjust_usec;
static float calib_accel_x = 0, calib_accel_y = 0, calib_accel_z = 0;   

float convertRawGyro(int gRaw) 
{
  // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
  float lsb_omega = float(0x7FFF) / GYRO_RANGE;
  return gRaw / lsb_omega;  // deg/sec
}

float convertRawAccel(int aRaw) 
{
  // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
  float lsb_g = float(0x7FFF) / ACCL_RANGE;
  return aRaw / lsb_g;
}

void print_pitch_roll() 
{

  static uint16_t n = 0;
  static float g_angle_roll = 0, g_angle_pitch = 0, g_angle_yaw = 0;
  static float a_angle_roll = 0, a_angle_pitch = 0, a_angle_yaw = 0;      
 
  static unsigned long last_mills = 0; 
  
  // read raw gyro measurements from device
  int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
  BMI160.readGyro(rawRoll, rawPitch, rawYaw);
  
  // read raw accl measurements from device
  int rawXAcc, rawYAcc, rawZAcc; // x, y, z
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);

  unsigned long cur_mills = millis();
  unsigned long duration = cur_mills - last_mills;
  last_mills = cur_mills;
  float fduration = duration / 1000.0; // ms->s
  
  // convert the raw gyro data to degrees/second
  float omega_roll  = convertRawGyro(rawRoll);
  float omega_pitch = convertRawGyro(rawPitch);
  float omega_yaw   = convertRawGyro(rawYaw);
  
  g_angle_roll  += omega_roll  * fduration; // (ms->s) omega x time = degree
  g_angle_pitch += omega_pitch * fduration;
  g_angle_yaw   += omega_yaw   * fduration; // for the reference

  // convert the raw acclerometer data to m/s^2
  float cur_a_x = convertRawAccel(rawXAcc) - calib_accel_x;
  float cur_a_y = convertRawAccel(rawYAcc) - calib_accel_y;
  float cur_a_z = convertRawAccel(rawZAcc) - calib_accel_z;
  cur_a_x = deg_to_rad(cur_a_x);
  cur_a_y = deg_to_rad(cur_a_y);
  cur_a_z = deg_to_rad(cur_a_z);
  a_angle_roll = atan(cur_a_y / cur_a_z);
  a_angle_pitch = atan(-cur_a_x / cur_a_z);
  a_angle_roll = rad_to_deg(a_angle_roll);
  a_angle_pitch = rad_to_deg(a_angle_pitch);
  
   
#ifdef PRINT_GYRO 
  // Serial.print("Duration:");
  // Serial.print(duration);
  // Serial.print(",");
  Serial.print("GYRO_roll:");
  Serial.print(g_angle_roll);
  Serial.print(",");
  Serial.print("GYRO_pitch:");
  Serial.print(g_angle_pitch);
  Serial.print(",");
#ifdef PRINT_YAW
  Serial.print("GYRO_yaw:");
  Serial.print(g_angle_yaw);
  Serial.print(",");
#endif
#endif

  // display tab-separated accel x/y/z values
#ifdef PRINT_ACCL
  Serial.print("ACC_roll:");
  Serial.print(a_angle_roll);
  Serial.print(",");
  Serial.print("ACC_pitch:");
  Serial.print(a_angle_pitch);
#endif

  Serial.println();
  usleep(adjust_usec); // 2msec is the process time for displaying the values.
}


void setup() {
  Serial.begin(BAUDRATE);

  BMI160.begin(BMI160GenClass::I2C_MODE);

  BMI160.setGyroRate(SENSE_RATE);
  BMI160.setAccelerometerRate(SENSE_RATE);
  BMI160.setGyroRange(GYRO_RANGE);
  BMI160.setAccelerometerRange(ACCL_RANGE);

  BMI160.autoCalibrateGyroOffset();
  BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  // Serial.println("Calibrating...");
  unsigned long sampling_rate = 1000 / SENSE_RATE;

  // Calcurate offset
  for (int i = 0; i < COUNT_FOR_CALIB; ++i) {
    int xAcc, yAcc, zAcc;
    BMI160.readAccelerometer(xAcc, yAcc, zAcc);
    calib_accel_x += convertRawAccel(xAcc);
    calib_accel_y += convertRawAccel(yAcc);
    calib_accel_z += convertRawAccel(zAcc)-1.0;
  }
  
  // mean value
  calib_accel_x /= COUNT_FOR_CALIB;
  calib_accel_y /= COUNT_FOR_CALIB;
  calib_accel_z /= COUNT_FOR_CALIB;
  
  calib_accel_z *= 1000; // The offset supports +/-495.3(mg)
  Serial.println("calib_a_x:" + String(calib_accel_x, 3));
  Serial.println("calib_a_y:" + String(calib_accel_y, 3));
  Serial.println("calib_a_z:" + String(calib_accel_z, 3));

  // SENSE_RATE is Hz and 2 millsec is the process time for this sketch
  adjust_usec = (1000/SENSE_RATE - 2) * 1000; 
  digitalWrite(LED0, HIGH);
}

void loop() {
  print_pitch_roll(); 
}

このスケッチで計測した結果がこちらです。

ジャイロの変化は非常に滑らかなのに対し、加速度センサーはノイズが多いです。一方、ジャイロは"gyro_roll"の最後にオフセットがかかってしまっていますが、加速度センサーはその影響はなさそうです。（拡大して見ると"gyro_pitch"もオフセットを持ってしまっているようです）

どちらも一長一短ですね。この２つのセンサーの特長をうまく利用するために、次はフィルター処理を試してみたいと思います。😀
（＾＾）／～

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

ROHM センサAdd-onボード SPRESENSE-SENSOR-EVK-701

出版社/メーカー: ローム(ROHM)
メディア: Tools & Hardware

SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサーモジュールIIC SPIブロード

出版社/メーカー: SODIAL
メディア: その他

タグ：倒立振子 BMI160 Spresense

2020-05-06 23:59 nice!(25) コメント(0)
共通テーマ：趣味・カルチャー

SPRESENSE とホール素子でモーターの回転数を計ってみる（２）　[ロボット]

SPRESENSEで回転数を測るための環境は整ったので、今回は実際に測定をしてみたいと思います。

まずはスケッチから。モーターを analogWrite で駆動して、ホール素子の電圧の立ち上がりを割り込みでキャッチして回転数をカウントしています。なるべく誤差が出ないように回転数の測定は１０秒間と長めに行っていました。

モーターは供給電圧が低い状態では、ギアの駆動抵抗が高くて動かせないので、5msec フル電圧で動かすという小技を使っています。

#define AIN01 20
#define AIN02 21
#define AIN11 18
#define AIN12 19
#define DASH 5

#define DET_PIN_1 14
#define DET_PIN_2 15
#define MEASURE_TIME 10

static uint16_t cnt_1 = 0;
static uint16_t cnt_2 = 0;
static uint8_t level = 5;
static boolean accel = true;
static boolean finish = false;

void count_1() {
  ++cnt_1;
}

void count_2() {
  ++cnt_2;
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(DET_PIN_1, INPUT_PULLDOWN);
  pinMode(DET_PIN_2, INPUT_PULLDOWN);

#ifdef INA219
  ina219.begin();
#endif

  attachInterrupt(digitalPinToInterrupt(DET_PIN_1), count_1, RISING);
  attachInterrupt(digitalPinToInterrupt(DET_PIN_2), count_2, RISING);
}


void motor_drive(uint8_t n, bool cw) {

  if (n > 255) n = 255;
  if (n <   0) n =   0;

#ifdef DASH
  if (n < 200) {
    if (cw) analogWrite(AIN01, 255);
    else    analogWrite(AIN02, 255);
    if (cw) analogWrite(AIN11, 255);
    else    analogWrite(AIN12, 255);
    delay(DASH);
  }
#endif

  if (cw) {
    analogWrite(AIN01, n);
    analogWrite(AIN02, 0);
    analogWrite(AIN11, n);
    analogWrite(AIN12, 0);

  } else {
    analogWrite(AIN01, 0);
    analogWrite(AIN02, n);
    analogWrite(AIN11, 0);
    analogWrite(AIN12, n);
  }
}

void countingRot(int n) {
    static unsigned long mtime;
    interrupts();
    mtime = millis();
    sleep(MEASURE_TIME);
    mtime = millis() - mtime;
    noInterrupts();
    Serial.print(String(n) + ",");
    Serial.print(String(mtime) + ",");
    Serial.print(String(cnt_1) + ",");
    Serial.println(String(cnt_2));
    cnt_1 = 0;
    cnt_2 = 0;
}

void loop() {
  int i = 0;
  unsigned long mtime;

  do {
    motor_drive(i, true);
    countingRot(i);
    i += 5;
  } while (i <= 250);

  i = 255;
  do {
    motor_drive(i, true);
    countingRot(i);
    i -= 5;
  } while (i >= 0);

  i = 0;
  do {
    motor_drive(i, false);
    countingRot(i);
    i += 5;
  } while (i <= 250);

  i = 255;
  do {
    motor_drive(i, false);
    countingRot(i);
    i -= 5;
  } while (i > 0);

  while(1);

}

測定時のログ出力の様子です。左から、PWM出力値、測定時間（ms)、左モーター回転数、右モーター回転数です。

気になる実際の測定値はこちら。まず時計方向、反時計方向の測定値です。

左右のモーター間の回転数のばらつきは想定したよりもありませんでしたが、時計方向と反時計方向の違いが顕著です。これは恐らく電気系の問題ではなく、ギヤなどメカ的な問題かも知れません。いずれにしても制御のときは考慮する必要がありそうですね。

あと長時間測定をしていたので、バッテリーの供給電力の問題が出るかなと思ったのですが思いの外、違いはなさそうです。（バッテリーにもよるとは思いますが…）

次はセンサー処理について検討をしていきたいと思います。
(　･ิω･ิ)

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

Arduino電子工作: ２輪自立ロボットカーの製作

作者: 大西　和則
出版社/メーカー:
発売日: 2019/08/21
メディア: Kindle版

最新版モータ技術のすべてがわかる本 (史上最強カラー図解)

作者: 赤津観
出版社/メーカー: ナツメ社
発売日: 2012/09/04
メディア: 単行本

タグ：Spresense motor DRV8835 THS119 NJM2115D

2020-04-26 23:59 nice!(24) コメント(2)
共通テーマ：趣味・カルチャー

SPRESENSE とホール素子でモーターの回転数を計ってみる（１）　[ロボット]

SPRESENSEでモーターの続きです。前回、ホール素子でモーターの回転数を計るための回路を検討してみました。今回はいよいよそれを倒立振り子のメカに組み込んでみました。

ホール素子のホルダーは３Dプリンタで作成しました。アームとパーツの二つの部品で構成されています。格安のオンボロ３Dプリンタですが、ちょっとした工作をするには便利です。

ホルダーにホール素子を両面テープで貼り付けます。

これを倒立振り子のメカに組み付けます。車輪に 1cm x 1cm のテープ磁石をつけます。あまり幅が大きいと間違ってカウントしてしまうので磁石の大きさは重要です。

NJM2115Dは、オペアンプを２回路内蔵していますので、ホール素子のアンプ回路を次のようにくみ上げました。

スライド4.PNG

一生懸命書いたのですが、分かりにくくなってしまったので回路図も示します。＾＾；

スライド5.PNG

ブレッドボードに実際に組み上げたのがこちらです。図よりも見やすいかも…

SPRESENSE側の接続は次のようになっています。

実際に動かしてみたところ、順調にカウントしてくれてました。

次は、計測用のスケッチを書いて回転数を計測をしてみたいと思います！
( ｰ`дｰ´)

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

SODIAL(R) 10 x 3ピンユニポーラホール効果センサ磁気検出器2.5,20V

出版社/メーカー: SODIAL
メディア: その他

JRC(新日本無線) 2回路低雑音オペアンプ NJM5532DD (5個セット)

出版社/メーカー: JRC(新日本無線)
メディア:

Longruner　5個セット　arduino用　電圧DC　5V　ステッピングモーター+28BYJ-48　ULN2003ドライバテストモジュールボード　電作キット　LK67

出版社/メーカー: Longruner
メディア:

タグ：Spresense THS119 DRV8835 NJM2115D motor

2020-04-24 23:59 nice!(29) コメント(0)
共通テーマ：趣味・カルチャー

【備忘録】SPRESENSE でホール素子「THS119」を使う場合の注意点　 [ロボット]

SPRESENSEに限った話ではないのですが、ホール素子「THS119」を使う場合に少しはまったので、その注意点を備忘録として残しておきます。

THS119をSPRESENSEで扱うための回路は前回紹介しましたが、SPRESENSEには、次のように接続しました。

スライド2.PNG

スケッチは次のような極めて簡単なものです。

#define DET_PIN 21

void setup() {
  Serial.begin(115200);
  pinMode(DET_PIN, INPUT_PULLDOWN);

  sleep(1);
}

unsigned long n = 0;
void loop() {
  int val = digitalRead(DET_PIN);
  if (val == HIGH) {
    Serial.println(String(n));
  }
  ++n;
}

当初なるべく車輪の中心から遠いところでカウントしたいと思い（測定間隔が稼げるので）、テープ磁石の端でカウントしようと思ったのですが、まったくセンスしません。

ありゃ？回路失敗したか？と思ったのですが、いろいろ試行錯誤をしたところ、テープ磁石の真ん中近くだとうまくセンスしてくれることがわかりました。

どうも、素子に対して垂直に磁束が入らないとカウントしないみたいです。

スライド3.PNG

測定用のメカを３Dプリンタで試作したのですが、作り直しとなってしまいました。
トホホ　(´・ω・｀)

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

uxcell ホール効果センサー磁気検出器ホール効果センサ 10個入りブラック 19 x 4 x 1.5mm 2.5-20V

出版社/メーカー: uxcell
メディア: その他

コクヨマグネット強力マグネットプレート片面・粘着剤付き 6枚耐荷重500g マク-S381

出版社/メーカー: コクヨ(KOKUYO)
メディア: オフィス用品

タグ：ホール素子 THS119 Spresense

2020-04-16 23:59 nice!(23) コメント(0)
共通テーマ：趣味・カルチャー

SPRESENSE用モータードライバ・アドオンボードを作ってみた（２）　[ロボット]

自作 SPRESENSE用モータードライバ・アドオンボードの試運転をしてみました。少し前に制作した倒立振り子の筐体に取り付けて二つのモーターを駆動してみます。

アドオンボードの仕様を再掲します。

使用したスケッチは次のような単純なものです。

#define AIN01 21
#define AIN02 20
#define AIN11 19
#define AIN12 18
#define DELAY 3000
#define DASH 5


void motor_drive(uint8_t i, bool cw) {
#ifdef DASH
  if (cw) {
    analogWrite(AIN01, 255);
    analogWrite(AIN02, 0);
    analogWrite(AIN11, 255);
    analogWrite(AIN12, 0);
  }  else  {
    analogWrite(AIN01, 0);
    analogWrite(AIN02, 255);
    analogWrite(AIN11, 0);
    analogWrite(AIN12, 255);
  }
  delay(DASH);
#endif
  if (cw) {
    analogWrite(AIN01, i);
    analogWrite(AIN02, 0);
    analogWrite(AIN11, i);
    analogWrite(AIN12, 0);

  } else {
    analogWrite(AIN01, 0);
    analogWrite(AIN02, i);
    analogWrite(AIN11, 0);
    analogWrite(AIN12, i);
  }
  Serial.println(i);
  delay(DELAY);  
}

void setup() {
  Serial.begin(115200);
}

void loop() {
  int i;

  for (i = 0; i <= 250; i+=10) {
    motor_drive(i, true);
  }

  for (i = 250; i >= 0; i-=10) {
    motor_drive(i, true);
  }

  for (i = 0; i <= 250; i+=10) {
    motor_drive(i, false);
  }

  for (i = 250; i >= 0; i-=10) {
    motor_drive(i, false);
  }

  while(1);
}