K AKAGAWA / Mbed 2 deprecated daruma_robo_servo

darumasan ga koronda!

Dependencies:   mbed QEI ros_lib_kinetic USBDevice

main.cpp@0:85391b8275ee, 2019-11-13 (annotated)

Committer:
ksingyuunyuu
Date:
Wed Nov 13 09:28:20 2019 +0000
Revision:
0:85391b8275ee
Darumasanga koronda Robot; Neck turn servo program

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 0:85391b8275ee 5 #include <std_msgs/Bool.h>
ksingyuunyuu 0:85391b8275ee 6 ros::NodeHandle nh;
ksingyuunyuu 0:85391b8275ee 7 // マクロ定義(ピン配置)
ksingyuunyuu 0:85391b8275ee 8 #define D0 PA_3
ksingyuunyuu 0:85391b8275ee 9 #define D1 PA_2
ksingyuunyuu 0:85391b8275ee 10 #define D2 PA_10
ksingyuunyuu 0:85391b8275ee 11 #define D3 PB_3
ksingyuunyuu 0:85391b8275ee 12 #define D4 PB_5
ksingyuunyuu 0:85391b8275ee 13 #define D5 PB_4
ksingyuunyuu 0:85391b8275ee 14 #define D6 PB_10
ksingyuunyuu 0:85391b8275ee 15 #define D7 PA_8
ksingyuunyuu 0:85391b8275ee 16 #define D8 PA_9
ksingyuunyuu 0:85391b8275ee 17 #define D9 PC_7
ksingyuunyuu 0:85391b8275ee 18 #define D10 PB_6
ksingyuunyuu 0:85391b8275ee 19 #define D11 PA_7
ksingyuunyuu 0:85391b8275ee 20 #define D12 PA_6
ksingyuunyuu 0:85391b8275ee 21 #define D13 PA_5
ksingyuunyuu 0:85391b8275ee 22 // アナログ
ksingyuunyuu 0:85391b8275ee 23 #define A0 PA_0
ksingyuunyuu 0:85391b8275ee 24 #define A1 PA_1
ksingyuunyuu 0:85391b8275ee 25 #define A2 PA_4
ksingyuunyuu 0:85391b8275ee 26 #define A3 PB_0
ksingyuunyuu 0:85391b8275ee 27 #define A4 PC_1
ksingyuunyuu 0:85391b8275ee 28 #define A5 PC_0
ksingyuunyuu 0:85391b8275ee 29
ksingyuunyuu 0:85391b8275ee 30 #define B1 PC_13
ksingyuunyuu 0:85391b8275ee 31 // その他マクロ定義
ksingyuunyuu 0:85391b8275ee 32
ksingyuunyuu 0:85391b8275ee 33 // 入出力モード、各種モードの設定
ksingyuunyuu 0:85391b8275ee 34 BusIn sw(B1);
ksingyuunyuu 0:85391b8275ee 35 DigitalOut step(D9);
ksingyuunyuu 0:85391b8275ee 36 DigitalOut dir(D10);
ksingyuunyuu 0:85391b8275ee 37 DigitalOut myled(LED1);
ksingyuunyuu 0:85391b8275ee 38 PwmOut PWM(D3); //初期化
ksingyuunyuu 0:85391b8275ee 39
ksingyuunyuu 0:85391b8275ee 40 // 関数のプロトタイプ宣言
ksingyuunyuu 0:85391b8275ee 41 void turn(const std_msgs::Bool& cmd_turn);
ksingyuunyuu 0:85391b8275ee 42 ros::Subscriber<std_msgs::Bool> sub("hurimuki", turn);
ksingyuunyuu 0:85391b8275ee 43 void fake_turn(const std_msgs::Bool& cmd_fake_turn);
ksingyuunyuu 0:85391b8275ee 44 ros::Subscriber<std_msgs::Bool> fake("fake_hurimuki", fake_turn);
ksingyuunyuu 0:85391b8275ee 45 int main() {
ksingyuunyuu 0:85391b8275ee 46 PWM.period_us(20000); //周期設定
ksingyuunyuu 0:85391b8275ee 47 nh.initNode();
ksingyuunyuu 0:85391b8275ee 48 nh.subscribe(sub);
ksingyuunyuu 0:85391b8275ee 49 nh.subscribe(fake);
ksingyuunyuu 0:85391b8275ee 50 while (1) {
ksingyuunyuu 0:85391b8275ee 51 nh.spinOnce();
ksingyuunyuu 0:85391b8275ee 52 wait_ms(1);
ksingyuunyuu 0:85391b8275ee 53 }
ksingyuunyuu 0:85391b8275ee 54 }
ksingyuunyuu 0:85391b8275ee 55 // 振り向き関数 std_msgs/Bool型"hurimuki"トピック True:振り向き False:非振り向き
ksingyuunyuu 0:85391b8275ee 56 void turn(const std_msgs::Bool& cmd_turn){
ksingyuunyuu 0:85391b8275ee 57 if(cmd_turn.data){
ksingyuunyuu 0:85391b8275ee 58 // 振り向き
ksingyuunyuu 0:85391b8275ee 59 PWM.pulsewidth_us(595); //パルス幅変更
ksingyuunyuu 0:85391b8275ee 60 wait_ms(200);
ksingyuunyuu 0:85391b8275ee 61 } else {
ksingyuunyuu 0:85391b8275ee 62 // 振り向いていない
ksingyuunyuu 0:85391b8275ee 63 PWM.pulsewidth_us(2300);
ksingyuunyuu 0:85391b8275ee 64 wait(1);
ksingyuunyuu 0:85391b8275ee 65 }
ksingyuunyuu 0:85391b8275ee 66 }
ksingyuunyuu 0:85391b8275ee 67 // 振り向き関数 std_msgs/Bool型"fake_hurimuki"トピック "hurimuki"トピックがTrueの場合、非振り向きのフェイクとして機能する
ksingyuunyuu 0:85391b8275ee 68 // True:フェイク動作 False:振り向きのまま
ksingyuunyuu 0:85391b8275ee 69 void fake_turn(const std_msgs::Bool& cmd_fake_turn){
ksingyuunyuu 0:85391b8275ee 70 if(cmd_fake_turn.data){
ksingyuunyuu 0:85391b8275ee 71 // 振り向き
ksingyuunyuu 0:85391b8275ee 72 PWM.pulsewidth_us(1000); //パルス幅変更
ksingyuunyuu 0:85391b8275ee 73 wait(1);
ksingyuunyuu 0:85391b8275ee 74 PWM.pulsewidth_us(595); //パルス幅変更
ksingyuunyuu 0:85391b8275ee 75 } else {
ksingyuunyuu 0:85391b8275ee 76 // 振り向いていない
ksingyuunyuu 0:85391b8275ee 77 PWM.pulsewidth_us(2300);
ksingyuunyuu 0:85391b8275ee 78 wait(1);
ksingyuunyuu 0:85391b8275ee 79 }
ksingyuunyuu 0:85391b8275ee 80
ksingyuunyuu 0:85391b8275ee 81 }