mbedとrosでドローンを飛ばす(2)~ブラシレスモータ~

こんにちは。takepiyoです。今回はブラシレスモータを動かせるようにした後、pcからduty比をpublishすることで速度調整してみようと思います。

# 使用するモノ
- ブラシレスモータ:昔買ったキットについてきたやつです。単品でも売られていました。    
https://www.amazon.co.jp/-/en/ARRIS-S2205-2300KV-Brushless-Racing/dp/B071V4VDJC/ref=sr_1_1?dchild=1&keywords=arris+s2205&qid=1605317243&sr=8-1
- esc:昔買ったキットについてきたやつです。単品でも売られていました。   
https://www.amazon.co.jp/-/en/Brushless-Amplifier-170-330-Version-Assembly/dp/B01GJDUQIC/ref=sr_1_2?dchild=1&keywords=arris+esc&qid=1605317312&sr=8-2
- リポバッテリー　4セル


- 電源用基盤 
  昔買ったキットについてきたやつです。リポバッテリーから5vを取り出すために使います。
- ドローンのフレーム　昔買ったキットについてきたやつです。単品でも売られていました。
https://www.amazon.co.jp/ARRIS-%E3%82%A2%E3%83%AA%E3%82%B9-FPV250-250%E3%83%9F%E3%83%8B%E3%82%AF%E3%83%AF%E3%83%83%E3%83%89%E3%82%B3%E3%83%97%E3%82%BF%E3%83%BC%E3%83%95%E3%83%AC%E3%83%BC%E3%83%A0FPV%E7%A9%BA%E6%92%AE%E6%A9%9F/dp/B00T8VQ0TC

はんだ付けして、組み立てて下の写真のようになりました。

![IMG_1441.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/870287/147c8ec3-f23c-c13b-6848-8f60a1e87124.jpeg)

# escについて
escのデータシートが見つからないので、他の人のブログを参考に、周期10msのpwmを入力してみたら動きました。ブラシレスのescはデューティー比10%で初期化がかかるそうなので、`mbed_init()`でそのような初期化処理を行っています。
https://emb.macnica.co.jp/articles/3797/
http://l52secondary.blog.fc2.com/blog-entry-21.html


```esc.cpp
#include "mbed.h"

#define PERIOD  10
#define MIN_DUTY 0.0  //暫定
#define MAX_DUTY 1.0  //暫定

// mbed variables
DigitalOut led1 = LED1;
PwmOut motor1(p21);

void init_mbed()
{
    motor1.period_ms(PERIOD);
    write_duty(0.1);  //esc is initialized by 10% duty
    wait_ms(1000);
}

void write_duty(float value)
{
    if(value > MAX_DUTY){value = MAX_DUTY;}
    if(value < MIN_DUTY){value = MIN_DUTY;}
    motor1.write(value);
}

int main()
{
    init_mbed();
    while(1)
    {
        write_duty(0.2);
        led2 = !led2;
    }
}
```
# pcから速度調整
pc側からデューティー比を調整するプログラムを作成しました。

``` esc_ros.cpp
#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt32.h>

#define PERIOD  10
#define MIN_DUTY 0.0  //Period 10ms
#define MAX_DUTY 1.0  //Period 10ms

// mbed variables
DigitalOut led1 = LED1;
DigitalOut led2 = LED2;
PwmOut motor1(p21);

// ros variables
ros::NodeHandle nh;
std_msgs::Float32 duty;
ros::Publisher pub("now_duty", &duty);
std_msgs::UInt32 cnt;
ros::Publisher chatter("chatter", &cnt);

void write_duty(float value)
{
    if(value > MAX_DUTY){value = MAX_DUTY;}
    if(value < MIN_DUTY){value = MIN_DUTY;}
    motor1.write(value);
}

void update_duty(const std_msgs::Float32& input_duty)
{
    write_duty(input_duty.data);
    pub.publish(&input_duty);
    chatter.publish(&cnt);
    led1 = !led1;   
    cnt.data = cnt.data + 1;
}
ros::Subscriber<std_msgs::Float32> sub("input_duty", &update_duty);

void init_mbed()
{
    motor1.period_ms(PERIOD);
    write_duty(0.1);  //esc is initialized by 10% duty
    wait_ms(1000);
}

void init_ros()
{
    nh.initNode();
    nh.advertise(pub);
    nh.subscribe(sub);
    nh.advertise(chatter);
    cnt.data = 0;
    duty.data = 0.0;
}

int main()
{
    init_ros();
    init_mbed();
    while(1)
    {
        nh.spinOnce();
        wait_ms(10);
        led2 = !led2;
    }
}
```
コンパイル&書き込みを行い、mbedとシリアル通信を開始します。

```
roscore
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=921600
```
publishしてみます

```
rostopic pub -1 input_duty std_msgs/Float32 '0.2'
```
値を色々変えてみて、速度を調整することができました！

# 有効なduty比の範囲を調査
publishする値を小刻みに変えながら、上記のesc_ros.cppの`MIN_DUTY, MAX_DUTY`を決定します。

〜結果〜
`MIN_DUTY: 0.11`
`MAX_DUTY: 0.21`

# ps3で速度調整してみる
https://qiita.com/srs/items/9114bb3c27a148e0b855
この記事を参考にさせていただき、joyスティックからduty_inputに値をpublishするノードを作製しました。
左スティックの縦の入力をduty比として使用します。

```joy_state_publisher.cpp
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Float32.h>

// geometry_msgs::Twist cmd_vel;
std_msgs::Float32 duty;
void joy_callback(const sensor_msgs::Joy& joy_msg)
{
  duty.data = joy_msg.axes[1];
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "joy_state_publisher");
  ros::NodeHandle nh;
  // ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
  ros::Publisher cmd_pub = nh.advertise<std_msgs::Float32>("input_duty", 1000);
  ros::Subscriber joy_sub = nh.subscribe("joy", 10, joy_callback);

  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    cmd_pub.publish(duty);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```
launchファイルも作製しました。

```joy.launch
<?xml version="1.0"?>
<launch>

  <node name="joy_node" pkg="joy" type="joy_node"/>
  <node name="joy_state_publisher" pkg="drone_mbed" type="joy_state_publisher"/>

  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="921600"/>
  </node>

</launch>
```
```CmakeLists.txtに追加
add_executable(joy_state_publisher src/joy_state_publisher.cpp)

target_link_libraries(joy_state_publisher
  ${catkin_LIBRARIES}
)
```

```terminal
catkin build --this
```

```terminal
roslaunch drone_mbed joy.launch
```
ps3コンで速度調節ができました！

mbedの方も少し変更を加えたので一応載せます。

```joy.cpp
/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */

#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt32.h>

#define PERIOD  10
#define MIN_DUTY 0.10  //Period 10ms
#define MAX_DUTY 0.21  //Period 10ms

// mbed variables
DigitalOut led1 = LED1;
DigitalOut led2 = LED2;
PwmOut motor1(p21);

// ros variables
ros::NodeHandle nh;
std_msgs::Float32 duty;
ros::Publisher pub("now_duty", &duty);
std_msgs::UInt32 cnt;
ros::Publisher chatter("chatter", &cnt);

void write_duty(std_msgs::Float32 input_duty)
{
    input_duty.data = (MAX_DUTY - MIN_DUTY) * input_duty.data + MIN_DUTY;
    motor1.write(input_duty.data);
    pub.publish(&input_duty);
}

void update_duty(const std_msgs::Float32& input_duty)
{
    write_duty(input_duty);
    chatter.publish(&cnt);
    led1 = !led1;   
    cnt.data = cnt.data + 1;
}
ros::Subscriber<std_msgs::Float32> sub("input_duty", &update_duty);

void init_mbed()
{
    std_msgs::Float32 initial_duty;
    initial_duty.data = 0.1;
    motor1.period_ms(PERIOD);
    write_duty(initial_duty);  //esc is initialized by 10% duty
    wait_ms(1000);
}

void init_ros()
{
    nh.initNode();
    nh.advertise(pub);
    nh.subscribe(sub);
    nh.advertise(chatter);
    cnt.data = 0;
    duty.data = 0.0;
}

int main()
{
    init_ros();
    init_mbed();
    while(1)
    {
        nh.spinOnce();
        wait_ms(1);
        led2 = !led2;
    }
    return 0;
}
```

次回はxbeeと加速度センサを使えるようにします。
ここまで読んでいただきありがとうございました。