K AKAGAWA / Mbed 2 deprecated daruma_robo_servo_copy_2dof_pushsw

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI ros_lib_kinetic USBDevice

Files at this revision

API Documentation at this revision

Revision 3:e9fdd244c164, committed 2020-01-14

Comitter:
ksingyuunyuu
Date:
Tue Jan 14 04:26:39 2020 +0000
Parent:
2:5f8d83f6c91e
Commit message:
push sw version;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 5f8d83f6c91e -r e9fdd244c164 main.cpp
--- a/main.cpp	Thu Dec 19 02:55:49 2019 +0000
+++ b/main.cpp	Tue Jan 14 04:26:39 2020 +0000
@@ -1,7 +1,5 @@
 #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;
@@ -27,20 +25,24 @@
 #define A3  PB_0
 #define A4  PC_1
 #define A5  PC_0
-
+ 
 #define B1  PC_13
 // その他マクロ定義
-
+ 
 // 入出力モード、各種モードの設定
-PwmOut servo1(D3); //初期化
-PwmOut servo2(D9); //初期化
-
+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);
@@ -50,10 +52,12 @@
 // 振り向き
 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.getHardware()->setBaud(115200);
     nh.initNode();
+    // スイッチパブリッシュ
+    nh.advertise(pub_sw);
     // ヨー軸サブクライブ
     nh.subscribe(servo_yaw);
     // ピッチ軸サブクライブ
@@ -61,10 +65,16 @@
     // 振り向きサブクライブ
     nh.subscribe(sub_turn);
     while (1){
+        myled = button;
+        sw_msg.data = button;
+        pub_sw.publish(&sw_msg);
         nh.spinOnce();
-        wait_ms(1);
+        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){
@@ -89,7 +99,7 @@
         wait(1);
     }
 }
-
+ 
 // 振り向き関数 std_msgs/Bool型"hurimuki"トピック True:振り向き False:非振り向き
 void turn(const std_msgs::Bool& cmd_turn){
     int servo1_pulse;