ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@4:29bf39bd97a7, 2017-09-21 (annotated)

Committer:
agari
Date:
Thu Sep 21 15:16:32 2017 +0000
Revision:
4:29bf39bd97a7
Parent:
3:2184f6f5c8e3
Child:
5:4f1f0294d6aa
??(0921??)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
agari 0:f6fa58c56955 1 #include <mbed.h>
agari 0:f6fa58c56955 2 #include <ros.h>
agari 0:f6fa58c56955 3 #include <std_msgs/String.h>
agari 0:f6fa58c56955 4
agari 1:5dd2f53a286f 5 DigitalOut grab(p11); //0:閉じる 1:開く
agari 1:5dd2f53a286f 6 DigitalOut snap(p12); //0:縮む 1:伸びる
agari 1:5dd2f53a286f 7 std_msgs::String action;
agari 3:2184f6f5c8e3 8 int k_action=0;
agari 3:2184f6f5c8e3 9 int o_action=0;
agari 3:2184f6f5c8e3 10 int m_action=0;
agari 1:5dd2f53a286f 11 void messageCallback(const std_msgs::String &msg)
agari 1:5dd2f53a286f 12 {
agari 1:5dd2f53a286f 13 action.data=msg.data;
agari 1:5dd2f53a286f 14 }
agari 0:f6fa58c56955 15
Arare 2:213c12298d45 16 ros::NodeHandle nh;
Arare 2:213c12298d45 17 ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
Arare 2:213c12298d45 18 DigitalOut led1(LED1);
Arare 2:213c12298d45 19 DigitalOut led2(LED2);
Arare 2:213c12298d45 20 DigitalOut led3(LED3);
Arare 2:213c12298d45 21 DigitalOut led4(LED4);
Arare 2:213c12298d45 22
agari 3:2184f6f5c8e3 23 void led_on(int l1,int l2,int l3,int l4)
agari 3:2184f6f5c8e3 24 {
Arare 2:213c12298d45 25 led1=l1;
Arare 2:213c12298d45 26 led2=l2;
Arare 2:213c12298d45 27 led3=l3;
Arare 2:213c12298d45 28 led4=l4;
Arare 2:213c12298d45 29 }
agari 0:f6fa58c56955 30
agari 1:5dd2f53a286f 31 int main(int argc, char **argv)
agari 1:5dd2f53a286f 32 {
Arare 2:213c12298d45 33 nh.initNode();
Arare 2:213c12298d45 34 nh.subscribe(sub);
agari 3:2184f6f5c8e3 35 while(1) {
agari 3:2184f6f5c8e3 36 if((*(action.data)== 'k')&&(k_action==0)) {
agari 3:2184f6f5c8e3 37 snap = 1;
agari 3:2184f6f5c8e3 38 grab = 0;
agari 3:2184f6f5c8e3 39 k_action++;
agari 3:2184f6f5c8e3 40 led_on(1,0,0,0);
agari 4:29bf39bd97a7 41 wait(5);
agari 1:5dd2f53a286f 42
agari 3:2184f6f5c8e3 43 grab = 1;
agari 3:2184f6f5c8e3 44 o_action++;
agari 3:2184f6f5c8e3 45 led_on(0,1,0,0);
agari 3:2184f6f5c8e3 46 }
agari 4:29bf39bd97a7 47
agari 1:5dd2f53a286f 48
agari 3:2184f6f5c8e3 49 else if((*(action.data)=='m')&&(m_action==0)) {
agari 3:2184f6f5c8e3 50 led_on(1,1,0,0);
agari 3:2184f6f5c8e3 51 snap = 0;
agari 3:2184f6f5c8e3 52 m_action++;
agari 3:2184f6f5c8e3 53 //pc.printf("a\n");
agari 3:2184f6f5c8e3 54 wait(0.195); //TZ1:0.18 TZ2:0.18 TZ3:0.167,0.168 椅子(高度45[cm]):0.195
agari 3:2184f6f5c8e3 55 grab = 1;
agari 4:29bf39bd97a7 56 wait(3);
agari 4:29bf39bd97a7 57 snap=1;
agari 1:5dd2f53a286f 58
agari 1:5dd2f53a286f 59 }
agari 3:2184f6f5c8e3 60 //一連の動作が終了したかを判断する。
agari 3:2184f6f5c8e3 61 if((k_action==1)&&(o_action==1)&&(m_action==1)) {
agari 3:2184f6f5c8e3 62 k_action=0;
agari 3:2184f6f5c8e3 63 o_action=0;
agari 3:2184f6f5c8e3 64 m_action=0;
agari 3:2184f6f5c8e3 65 }
agari 3:2184f6f5c8e3 66 nh.spinOnce();
agari 3:2184f6f5c8e3 67 }
agari 3:2184f6f5c8e3 68
agari 0:f6fa58c56955 69 }