こんにちは。takepiyoです。前回までで、必要な部品の単体テストが終わったので、基盤に実装していきます。単純な回路なので回路図とかは書いていません。

使用部品

ユニバーサル基板
https://www.amazon.co.jp/-/en/gp/product/B077BW5MM8/ref=ppx_yo_dt_b_asin_title_o01_s00?ie=UTF8&psc=1

ピンヘッダーオスメス　
https://www.amazon.co.jp/-/en/gp/product/B07Q4VTFQS/ref=ppx_yo_dt_b_asin_title_o01_s01?ie=UTF8&psc=1

写真たち

一応大きめの基盤を買ったら大きすぎましたが、輪ゴムで固定できたので良いとします。(笑)

(リポバッテリーを3セルに変えました)

それらしい見た目になってきました！

テスト

回路が正常に動作するかを確認します。

加速度センサの値をpcにxbeeでpcに送信

下記プログラムで動作しました。

main.cpp

# include "mbed.h"
# include <ros.h>
# include <BMI088.h>
# include <Esc.h>
# include <std_msgs/String.h>
# include <std_msgs/Float32.h>
# include <std_msgs/Float32MultiArray.h>
# include <geometry_msgs/Accel.h>

# include <bits/stdc++.h>
using namespace std;

// mbed variables
DigitalOut led1 = LED1;
DigitalOut led2 = LED2;
DigitalOut led3 = LED3;

Esc motor[4] = {p25, p24, p23, p22}; //モータの処理をクラスで実装
BMI088 bmi088;

// ros variables
ros::NodeHandle nh;
std_msgs::Float32MultiArray duties;
ros::Publisher duties_pub("now_duty", &duties);
std_msgs::String echo;
ros::Publisher debugger("debug_message", &echo); 
geometry_msgs::Accel accel;
ros::Publisher acc_gyro("acc_gyro", &accel);

void publish_string(string message)
{
    int length = message.length();
    char char_array[length + 1];
    strcpy(char_array, message.c_str());
    echo.data = char_array;
    debugger.publish(&echo);
}

void get_acc_gyro()
{   
    float ax = 0, ay = 0, az = 0;
    float gx = 0, gy = 0, gz = 0;
    bmi088.getAcceleration(&ax, &ay, &az);
    bmi088.getGyroscope(&gx, &gy, &gz);
    accel.linear.x = ax;
    accel.linear.y = ay;
    accel.linear.z = az;
    accel.angular.x = gx;
    accel.angular.y = gy;
    accel.angular.z = gz;
    // bmi088.getAcceleration(&accel.linear.x, &accel.linear.y, &accel.linear.z);
    // bmi088.getGyroscope(&accel.angular.x, &accel.angular.y, &accel.angular.z);  //何故かこれだと型がミスマッチのエラーがでる。
    acc_gyro.publish(&accel);
}

void init_mbed()
{
    led3 = 0;
    // publish_string("initialize mbed...");
    while (1) 
    {
        if (bmi088.isConnection()) {
            bmi088.initialize();
            // publish_string("BMI088 is connected");
            break;
        } else {
            // publish_string("BMI088 is not connected");
        }
        led3 = !led3;
        wait_ms(200);
    }
    led3 = 1;    
    duties.data_length = 4;
    duties.data = (float *)malloc(sizeof(float)*4);
}

void init_ros()
{
    nh.initNode();
    nh.advertise(duties_pub);
    nh.advertise(debugger);
    nh.advertise(acc_gyro);
    // publish_string("finish ros_init!!!");
    // nh.subscribe(sub);
}

int main()
{
    led1 = 0;
    init_ros();
    init_mbed();
    led1 = 1;
    // publish_string("start loop!");
    while(1)
    {
        nh.spinOnce();
        led2 = !led2;
        get_acc_gyro();
        // publish_string("loop!");
        wait_ms(1000);
    }
    return 0;
}

linear: 
  x: -0.00718505866826
  y: 0.255069583654
  z: 9.84712314606
angular: 
  x: 0.0106526445597
  y: -0.0042610578239
  z: -1.47219538689
---
linear: 
  x: -0.326920181513
  y: 0.116757206619
  z: 9.77347660065
angular: 
  x: -0.0106526445597
  y: 0.15339808166
  z: -0.0234358180314

途中で

[ERROR] [1605634745.237656]: Mismatched protocol version in packet (b'\xb7'): lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [1605634745.242887]: Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
[INFO] [1605634746.457533]: wrong checksum for topic id and msg
[ERROR] [1605634746.474066]: Mismatched protocol version in packet (b'\x97'): lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [1605634746.478865]: Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
[INFO] [1605634747.692189]: wrong checksum for topic id and msg
[INFO] [1605634748.921360]: wrong checksum for topic id and msg

というエラーが大量に発生しました。(前回joyスティックをpublishしたときもでた)
https://hogepad.com/mbed-rosserial%e3%81%aemismatch-orotocol-version%e3%82%a8%e3%83%a9%e3%83%bc%e3%81%ae%e5%af%be%e7%ad%96/
ここを参考にwait_ms(1000)にしたら治りました。

joyスティックでブラシレスモータを制御

joyスティックの情報をpublishするノード

joy_state_publisher.cpp

# include <ros/ros.h>
# include <sensor_msgs/Joy.h>
# include "std_msgs/MultiArrayLayout.h"
# include "std_msgs/MultiArrayDimension.h"
# include <std_msgs/Float32MultiArray.h>

# define MOTOR_NUM 4

std_msgs::Float32MultiArray duties;
ros::Publisher cmd_pub;
ros::Subscriber joy_sub;

void joy_callback(const sensor_msgs::Joy& joy_msg)
{
  duties.data[0] = joy_msg.axes[0];
  duties.data[1] = joy_msg.axes[1];
  duties.data[2] = joy_msg.axes[3];
  duties.data[3] = joy_msg.axes[4];
  cmd_pub.publish(duties);
}

int main(int argc, char** argv)
{
  duties.data.resize(MOTOR_NUM);
  ros::init(argc, argv, "joy_state_publisher");
  ros::NodeHandle nh;
  cmd_pub = nh.advertise<std_msgs::Float32MultiArray>("input_duties", 1000);
  joy_sub = nh.subscribe("joy", 1000, joy_callback);

  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

mbed側のプログラム

main.cpp

# include "mbed.h"
# include <ros.h>
# include <BMI088.h>
# include <Esc.h>
# include <std_msgs/String.h>
# include "std_msgs/MultiArrayLayout.h"
# include "std_msgs/MultiArrayDimension.h"
# include <std_msgs/Float32MultiArray.h>

# include <geometry_msgs/Accel.h>

# include <vector>
# include <bits/stdc++.h>
using namespace std;

# define MOTOR_NUM 4

// mbed variables
DigitalOut led1 = LED1;
DigitalOut led2 = LED2;
DigitalOut led3 = LED3;

Esc motor[MOTOR_NUM] = {p25, p24, p23, p22};
BMI088 bmi088;

// ros variables
ros::NodeHandle nh;
std_msgs::Float32MultiArray duties;
ros::Publisher duties_pub("now_duty", &duties);
std_msgs::String echo;
ros::Publisher debugger("debug_message", &echo); 
geometry_msgs::Accel accel;
ros::Publisher acc_gyro("acc_gyro", &accel);

void publish_string(string message)
{
    int length = message.length();
    char char_array[length + 1];
    strcpy(char_array, message.c_str());
    echo.data = char_array;
    debugger.publish(&echo);
}

void get_acc_gyro()
{   
    float ax = 0, ay = 0, az = 0;
    float gx = 0, gy = 0, gz = 0;
    bmi088.getAcceleration(&ax, &ay, &az);
    bmi088.getGyroscope(&gx, &gy, &gz);
    accel.linear.x = ax;
    accel.linear.y = ay;
    accel.linear.z = az;
    accel.angular.x = gx;
    accel.angular.y = gy;
    accel.angular.z = gz;
    // bmi088.getAcceleration(&accel.linear.x, &accel.linear.y, &accel.linear.z);
    // bmi088.getGyroscope(&accel.angular.x, &accel.angular.y, &accel.angular.z);
    acc_gyro.publish(&accel);
}

void update_motor_rotation(const std_msgs::Float32MultiArray& duties)
{
    for(int i=0; i<MOTOR_NUM; i++)
    {
        motor[i].update(duties.data[i]);
    }
}
ros::Subscriber<std_msgs::Float32MultiArray> duties_sub("input_duties", &update_motor_rotation);

void init_mbed()
{
    led3 = 0;
    // publish_string("initialize mbed...");
    while (1) 
    {
        if (bmi088.isConnection()) {
            bmi088.initialize();
            // publish_string("BMI088 is connected");
            break;
        } else {
            // publish_string("BMI088 is not connected");
        }
        led3 = !led3;
        wait_ms(200);
    }
    led3 = 1;    
    duties.data_length = MOTOR_NUM;
    duties.data = (float *)malloc(sizeof(float)*MOTOR_NUM);
}

void init_ros()
{
    nh.initNode();
    nh.advertise(duties_pub);
    nh.advertise(debugger);
    nh.advertise(acc_gyro);
    // publish_string("finish ros_init!!!");
    nh.subscribe(duties_sub);
}

int main()
{
    led1 = 0;
    init_ros();
    init_mbed();
    led1 = 1;
    // publish_string("start loop!");
    while(1)
    {
        nh.spinOnce();
        led2 = !led2;
        get_acc_gyro();
        // publish_string("loop!");
        wait_ms(10);
    }
    return 0;
}

ps3コントローラからモータの速度を変えることができました。

制御無しで飛ばせるようになったので、次回からいよいよ姿勢制御を実装します。
ここまで読んでいただきありがとうございました。