Diff: main.cpp

Revision:

0:85391b8275ee

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 13 09:28:20 2019 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include "QEI.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
+// その他マクロ定義
+
+// 入出力モード、各種モードの設定
+BusIn sw(B1);
+DigitalOut step(D9);
+DigitalOut dir(D10);
+DigitalOut myled(LED1);
+PwmOut PWM(D3); //初期化
+
+// 関数のプロトタイプ宣言
+void turn(const std_msgs::Bool& cmd_turn);
+ros::Subscriber<std_msgs::Bool> sub("hurimuki", turn);
+void fake_turn(const std_msgs::Bool& cmd_fake_turn);
+ros::Subscriber<std_msgs::Bool> fake("fake_hurimuki", fake_turn);
+int main() {
+    PWM.period_us(20000);  //周期設定
+    nh.initNode();
+    nh.subscribe(sub);
+    nh.subscribe(fake);
+    while (1) {
+        nh.spinOnce();
+        wait_ms(1);
+    }
+}
+// 振り向き関数 std_msgs/Bool型"hurimuki"トピック True:振り向き False:非振り向き
+void turn(const std_msgs::Bool& cmd_turn){
+    if(cmd_turn.data){
+        // 振り向き
+        PWM.pulsewidth_us(595); //パルス幅変更
+        wait_ms(200);
+    } else {
+        // 振り向いていない
+        PWM.pulsewidth_us(2300);
+        wait(1);
+    }
+}
+// 振り向き関数 std_msgs/Bool型"fake_hurimuki"トピック "hurimuki"トピックがTrueの場合、非振り向きのフェイクとして機能する
+// True:フェイク動作 False:振り向きのまま
+void fake_turn(const std_msgs::Bool& cmd_fake_turn){
+    if(cmd_fake_turn.data){
+        // 振り向き
+        PWM.pulsewidth_us(1000); //パルス幅変更
+        wait(1);
+        PWM.pulsewidth_us(595); //パルス幅変更
+    } else {
+        // 振り向いていない
+        PWM.pulsewidth_us(2300);
+        wait(1);
+    }
+    
+}