K AKAGAWA / Mbed 2 deprecated daruma_robo_servo_copy_2dof

Dependencies:   mbed QEI ros_lib_kinetic USBDevice

main.cpp@1:6cb12474aa9f, 2019-12-19 (annotated)

Committer:
ksingyuunyuu
Date:
Thu Dec 19 01:56:28 2019 +0000
Revision:
1:6cb12474aa9f
Parent:
0:85391b8275ee
Child:
2:5f8d83f6c91e
2dof_servomotor_program; first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ksingyuunyuu 0:85391b8275ee 1 #include "mbed.h"
ksingyuunyuu 0:85391b8275ee 2 #include <ros.h>
ksingyuunyuu 0:85391b8275ee 3 #include <geometry_msgs/Twist.h>
ksingyuunyuu 0:85391b8275ee 4 #include "QEI.h"
ksingyuunyuu 1:6cb12474aa9f 5 #include <std_msgs/Int16.h>
ksingyuunyuu 0:85391b8275ee 6 #include <std_msgs/Bool.h>
ksingyuunyuu 0:85391b8275ee 7 ros::NodeHandle nh;
ksingyuunyuu 0:85391b8275ee 8 // マクロ定義(ピン配置)
ksingyuunyuu 0:85391b8275ee 9 #define D0 PA_3
ksingyuunyuu 0:85391b8275ee 10 #define D1 PA_2
ksingyuunyuu 0:85391b8275ee 11 #define D2 PA_10
ksingyuunyuu 0:85391b8275ee 12 #define D3 PB_3
ksingyuunyuu 0:85391b8275ee 13 #define D4 PB_5
ksingyuunyuu 0:85391b8275ee 14 #define D5 PB_4
ksingyuunyuu 0:85391b8275ee 15 #define D6 PB_10
ksingyuunyuu 0:85391b8275ee 16 #define D7 PA_8
ksingyuunyuu 0:85391b8275ee 17 #define D8 PA_9
ksingyuunyuu 0:85391b8275ee 18 #define D9 PC_7
ksingyuunyuu 0:85391b8275ee 19 #define D10 PB_6
ksingyuunyuu 0:85391b8275ee 20 #define D11 PA_7
ksingyuunyuu 0:85391b8275ee 21 #define D12 PA_6
ksingyuunyuu 0:85391b8275ee 22 #define D13 PA_5
ksingyuunyuu 0:85391b8275ee 23 // アナログ
ksingyuunyuu 0:85391b8275ee 24 #define A0 PA_0
ksingyuunyuu 0:85391b8275ee 25 #define A1 PA_1
ksingyuunyuu 0:85391b8275ee 26 #define A2 PA_4
ksingyuunyuu 0:85391b8275ee 27 #define A3 PB_0
ksingyuunyuu 0:85391b8275ee 28 #define A4 PC_1
ksingyuunyuu 0:85391b8275ee 29 #define A5 PC_0
ksingyuunyuu 0:85391b8275ee 30
ksingyuunyuu 0:85391b8275ee 31 #define B1 PC_13
ksingyuunyuu 0:85391b8275ee 32 // その他マクロ定義
ksingyuunyuu 0:85391b8275ee 33
ksingyuunyuu 0:85391b8275ee 34 // 入出力モード、各種モードの設定
ksingyuunyuu 1:6cb12474aa9f 35 PwmOut servo1(D3); //初期化
ksingyuunyuu 1:6cb12474aa9f 36 PwmOut servo2(D9); //初期化
ksingyuunyuu 1:6cb12474aa9f 37
ksingyuunyuu 1:6cb12474aa9f 38 /*
ksingyuunyuu 1:6cb12474aa9f 39 servo1は1150がセンター
ksingyuunyuu 1:6cb12474aa9f 40 servo2は1500がセンター,600,2400
ksingyuunyuu 1:6cb12474aa9f 41 */
ksingyuunyuu 0:85391b8275ee 42
ksingyuunyuu 0:85391b8275ee 43 // 関数のプロトタイプ宣言
ksingyuunyuu 1:6cb12474aa9f 44 void move_yaw(const std_msgs::Int16& cmd_yaw);
ksingyuunyuu 1:6cb12474aa9f 45 ros::Subscriber<std_msgs::Int16> servo_yaw("servo_yaw", move_yaw);
ksingyuunyuu 1:6cb12474aa9f 46 void move_pitch(const std_msgs::Int16& cmd_pitch);
ksingyuunyuu 1:6cb12474aa9f 47 ros::Subscriber<std_msgs::Int16> servo_pitch("servo_pitch", move_pitch);
ksingyuunyuu 1:6cb12474aa9f 48
ksingyuunyuu 0:85391b8275ee 49 void turn(const std_msgs::Bool& cmd_turn);
ksingyuunyuu 0:85391b8275ee 50 ros::Subscriber<std_msgs::Bool> sub("hurimuki", turn);
ksingyuunyuu 0:85391b8275ee 51 int main() {
ksingyuunyuu 1:6cb12474aa9f 52 //servo1.period_us(20000); //周期設定
ksingyuunyuu 1:6cb12474aa9f 53 //servo2.period_us(20000); //周期設定
ksingyuunyuu 0:85391b8275ee 54 nh.initNode();
ksingyuunyuu 1:6cb12474aa9f 55 nh.subscribe(servo_yaw);
ksingyuunyuu 1:6cb12474aa9f 56 nh.subscribe(servo_pitch);
ksingyuunyuu 1:6cb12474aa9f 57 while (1){
ksingyuunyuu 0:85391b8275ee 58 nh.spinOnce();
ksingyuunyuu 0:85391b8275ee 59 wait_ms(1);
ksingyuunyuu 0:85391b8275ee 60 }
ksingyuunyuu 0:85391b8275ee 61 }
ksingyuunyuu 1:6cb12474aa9f 62 // 振り向き関数 std_msgs/Int16型"servo_yaw"トピック
ksingyuunyuu 1:6cb12474aa9f 63 void move_yaw(const std_msgs::Int16& cmd_yaw){
ksingyuunyuu 1:6cb12474aa9f 64 int servo1_pulse;
ksingyuunyuu 1:6cb12474aa9f 65 if(cmd_yaw.data > 90 || cmd_yaw.data < -90){
ksingyuunyuu 1:6cb12474aa9f 66 wait(1);
ksingyuunyuu 1:6cb12474aa9f 67 }else{
ksingyuunyuu 1:6cb12474aa9f 68 servo1_pulse = 1500 + (cmd_yaw.data * 10);
ksingyuunyuu 1:6cb12474aa9f 69 servo1.pulsewidth_us(servo1_pulse); //パルス幅変更
ksingyuunyuu 0:85391b8275ee 70 wait(1);
ksingyuunyuu 0:85391b8275ee 71 }
ksingyuunyuu 0:85391b8275ee 72 }
ksingyuunyuu 1:6cb12474aa9f 73 // 振り向き関数 std_msgs/Int16型"servo_yaw"トピック
ksingyuunyuu 1:6cb12474aa9f 74 void move_pitch(const std_msgs::Int16& cmd_pitch){
ksingyuunyuu 1:6cb12474aa9f 75 int servo2_pulse;
ksingyuunyuu 1:6cb12474aa9f 76 if(cmd_pitch.data > 90 || cmd_pitch.data < -90){
ksingyuunyuu 1:6cb12474aa9f 77 wait(1);
ksingyuunyuu 1:6cb12474aa9f 78 }else{
ksingyuunyuu 1:6cb12474aa9f 79 servo2_pulse = 1500 + (cmd_pitch.data * 10);
ksingyuunyuu 1:6cb12474aa9f 80 servo2.pulsewidth_us(servo2_pulse); //パルス幅変更
ksingyuunyuu 1:6cb12474aa9f 81 wait(1);
ksingyuunyuu 1:6cb12474aa9f 82 }
ksingyuunyuu 1:6cb12474aa9f 83 }
ksingyuunyuu 1:6cb12474aa9f 84
ksingyuunyuu 1:6cb12474aa9f 85 // 振り向き関数 std_msgs/Bool型"hurimuki"トピック True:振り向き False:非振り向き
ksingyuunyuu 1:6cb12474aa9f 86 void turn(const std_msgs::Bool& cmd_turn){
ksingyuunyuu 1:6cb12474aa9f 87 int servo1_pulse;
ksingyuunyuu 1:6cb12474aa9f 88 if(cmd_turn.data){
ksingyuunyuu 0:85391b8275ee 89 // 振り向き
ksingyuunyuu 1:6cb12474aa9f 90 servo1_pulse = 2400;
ksingyuunyuu 1:6cb12474aa9f 91 wait_ms(200);
ksingyuunyuu 0:85391b8275ee 92 } else {
ksingyuunyuu 0:85391b8275ee 93 // 振り向いていない
ksingyuunyuu 1:6cb12474aa9f 94 servo1_pulse = 600;
ksingyuunyuu 0:85391b8275ee 95 wait(1);
ksingyuunyuu 0:85391b8275ee 96 }
ksingyuunyuu 1:6cb12474aa9f 97 servo1.pulsewidth_us(servo1_pulse); //パルス幅変更
ksingyuunyuu 1:6cb12474aa9f 98 }