File content as of revision 3:e9fdd244c164:

#include "mbed.h"
#include <ros.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);          //初期化
DigitalIn button(D12);      // プッシュスイッチ
DigitalOut myled(LED1);     // プッシュスイッチ状態確認用LED
/*
servo1は1150がセンター
servo2は1500がセンター,600,2400
*/
 
// 関数のプロトタイプ宣言
//スイッチが押されたかどうか
std_msgs::Bool sw_msg;
ros::Publisher pub_sw("touched", &sw_msg);
// ヨー軸
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() {
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    // スイッチパブリッシュ
    nh.advertise(pub_sw);
    // ヨー軸サブクライブ
    nh.subscribe(servo_yaw);
    // ピッチ軸サブクライブ
    nh.subscribe(servo_pitch);
    // 振り向きサブクライブ
    nh.subscribe(sub_turn);
    while (1){
        myled = button;
        sw_msg.data = button;
        pub_sw.publish(&sw_msg);
        nh.spinOnce();
        wait_ms(20);
        // 1000ms / 20ms = 50 ...50Hzで通信を行う
        // あまりにも早すぎると通信できない 
    }
}

// サーボヨー軸制御関数 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); //パルス幅変更
}