Revision 1:6cb12474aa9f, committed 2019-12-19

Comitter:

ksingyuunyuu

Date:

Thu Dec 19 01:56:28 2019 +0000

Parent:

0:85391b8275ee

Child:

2:5f8d83f6c91e

Commit message:

2dof_servomotor_program; first commit

Changed in this revision

--- a/main.cpp	Wed Nov 13 09:28:20 2019 +0000
+++ b/main.cpp	Thu Dec 19 01:56:28 2019 +0000
@@ -2,6 +2,7 @@
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
 #include "QEI.h"
+#include <std_msgs/Int16.h>
 #include <std_msgs/Bool.h>
 ros::NodeHandle nh;
 // マクロ定義(ピン配置)
@@ -31,51 +32,67 @@
 // その他マクロ定義
 
 // 入出力モード、各種モードの設定
-BusIn sw(B1);
-DigitalOut step(D9);
-DigitalOut dir(D10);
-DigitalOut myled(LED1);
-PwmOut PWM(D3); //初期化
+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("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);  //周期設定
+    //servo1.period_us(20000);  //周期設定
+    //servo2.period_us(20000);  //周期設定
     nh.initNode();
-    nh.subscribe(sub);
-    nh.subscribe(fake);
-    while (1) {
+    nh.subscribe(servo_yaw);
+    nh.subscribe(servo_pitch);
+    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);
+// 振り向き関数 std_msgs/Int16型"servo_yaw"トピック
+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/Bool型"fake_hurimuki"トピック "hurimuki"トピックがTrueの場合、非振り向きのフェイクとして機能する
-// True:フェイク動作 False:振り向きのまま
-void fake_turn(const std_msgs::Bool& cmd_fake_turn){
-    if(cmd_fake_turn.data){
+// 振り向き関数 std_msgs/Int16型"servo_yaw"トピック
+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){
         // 振り向き
-        PWM.pulsewidth_us(1000); //パルス幅変更
-        wait(1);
-        PWM.pulsewidth_us(595); //パルス幅変更
+        servo1_pulse = 2400;
+        wait_ms(200);
     } else {
         // 振り向いていない
-        PWM.pulsewidth_us(2300);
+        servo1_pulse = 600;
         wait(1);
     }
-    
-}
+    servo1.pulsewidth_us(servo1_pulse); //パルス幅変更
+}
\ No newline at end of file