File content as of revision 2:5f8d83f6c91e:

#include "mbed.h"
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include "QEI.h"
#include <std_msgs/Int16.h>
#include <std_msgs/Bool.h>
ros::NodeHandle nh;
// マクロ定義(ピン配置)
#define D0  PA_3
#define D1  PA_2
#define D2  PA_10
#define D3  PB_3
#define D4  PB_5
#define D5  PB_4
#define D6  PB_10
#define D7  PA_8
#define D8  PA_9
#define D9  PC_7
#define D10 PB_6
#define D11 PA_7
#define D12 PA_6
#define D13 PA_5
// アナログ
#define A0  PA_0
#define A1  PA_1
#define A2  PA_4
#define A3  PB_0
#define A4  PC_1
#define A5  PC_0

#define B1  PC_13
// その他マクロ定義

// 入出力モード、各種モードの設定
PwmOut servo1(D3); //初期化
PwmOut servo2(D9); //初期化

/*
servo1は1150がセンター
servo2は1500がセンター,600,2400
*/

// 関数のプロトタイプ宣言
// ヨー軸
void move_yaw(const std_msgs::Int16& cmd_yaw);
ros::Subscriber<std_msgs::Int16> servo_yaw("servo_yaw", move_yaw);
// ピッチ軸
void move_pitch(const std_msgs::Int16& cmd_pitch);
ros::Subscriber<std_msgs::Int16> servo_pitch("servo_pitch", move_pitch);
// 振り向き
void turn(const std_msgs::Bool& cmd_turn);
ros::Subscriber<std_msgs::Bool> sub_turn("hurimuki", turn);
int main() {
    //servo1.period_us(20000);  //周期設定
    //servo2.period_us(20000);  //周期設定
    nh.initNode();
    // ヨー軸サブクライブ
    nh.subscribe(servo_yaw);
    // ピッチ軸サブクライブ
    nh.subscribe(servo_pitch);
    // 振り向きサブクライブ
    nh.subscribe(sub_turn);
    while (1){
        nh.spinOnce();
        wait_ms(1);
    }
}
// サーボヨー軸制御関数 std_msgs/Int16型"servo_yaw"トピック(引数:int16型整数 -90～90)
// ※-90～90を超えると、何も動かない
void move_yaw(const std_msgs::Int16& cmd_yaw){
    int servo1_pulse;
    if(cmd_yaw.data > 90 || cmd_yaw.data < -90){
        wait(1);
    }else{
        servo1_pulse = 1500 + (cmd_yaw.data * 10);
        servo1.pulsewidth_us(servo1_pulse); //パルス幅変更
        wait(1);
    }
}
// サーボピッチ軸制御関数 std_msgs/Int16型"servo_yaw"トピック(引数:int16型整数 -90～90)
// ※-90～90を超えると、何も動かない
void move_pitch(const std_msgs::Int16& cmd_pitch){
    int servo2_pulse;
    if(cmd_pitch.data > 90 || cmd_pitch.data < -90){
        wait(1);
    }else{
        servo2_pulse = 1500 + (cmd_pitch.data * 10);
        servo2.pulsewidth_us(servo2_pulse); //パルス幅変更
        wait(1);
    }
}

// 振り向き関数 std_msgs/Bool型"hurimuki"トピック True:振り向き False:非振り向き
void turn(const std_msgs::Bool& cmd_turn){
    int servo1_pulse;
    if(cmd_turn.data){
        // 振り向き
        servo1_pulse = 2400;
        wait_ms(200);
    } else {
        // 振り向いていない
        servo1_pulse = 600;
        wait(1);
    }
    servo1.pulsewidth_us(servo1_pulse); //パルス幅変更
}