LoginSignup
2
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@takepiyo(take piyo)

mbedとrosでドローンを飛ばす(4)~基盤実装と動作確認~

Posted at

こんにちは。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

写真たち

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

IMG_1448.JPG

IMG_1451.JPG

IMG_1454.JPG
(リポバッテリーを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コントローラからモータの速度を変えることができました。

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

2
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?