ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@2:213c12298d45, 2017-09-21 (annotated)

Committer:
Arare
Date:
Thu Sep 21 14:18:33 2017 +0000
Revision:
2:213c12298d45
Parent:
1:5dd2f53a286f
Child:
3:2184f6f5c8e3
Arare

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;
Arare 2:213c12298d45 8
agari 1:5dd2f53a286f 9 void messageCallback(const std_msgs::String &msg)
agari 1:5dd2f53a286f 10 {
agari 1:5dd2f53a286f 11 action.data=msg.data;
agari 1:5dd2f53a286f 12 }
agari 0:f6fa58c56955 13
Arare 2:213c12298d45 14 ros::NodeHandle nh;
Arare 2:213c12298d45 15 ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
Arare 2:213c12298d45 16 DigitalOut led1(LED1);
Arare 2:213c12298d45 17 DigitalOut led2(LED2);
Arare 2:213c12298d45 18 DigitalOut led3(LED3);
Arare 2:213c12298d45 19 DigitalOut led4(LED4);
Arare 2:213c12298d45 20
Arare 2:213c12298d45 21 void led_on(int l1,int l2,int l3,int l4){
Arare 2:213c12298d45 22 led1=l1;
Arare 2:213c12298d45 23 led2=l2;
Arare 2:213c12298d45 24 led3=l3;
Arare 2:213c12298d45 25 led4=l4;
Arare 2:213c12298d45 26 }
agari 0:f6fa58c56955 27
agari 1:5dd2f53a286f 28 int main(int argc, char **argv)
agari 1:5dd2f53a286f 29 {
Arare 2:213c12298d45 30 nh.initNode();
Arare 2:213c12298d45 31 nh.subscribe(sub);
agari 1:5dd2f53a286f 32 while(1) {
agari 1:5dd2f53a286f 33 if(*(action.data)== 'k') {
agari 1:5dd2f53a286f 34 snap = 1;
agari 1:5dd2f53a286f 35 grab = 0;
Arare 2:213c12298d45 36 led_on(1,0,0,0);
agari 1:5dd2f53a286f 37 }
agari 1:5dd2f53a286f 38
agari 1:5dd2f53a286f 39 else if(*(action.data)== 'o') {
agari 1:5dd2f53a286f 40 grab = 1;
Arare 2:213c12298d45 41 led_on(0,1,0,0);
agari 1:5dd2f53a286f 42 }
agari 1:5dd2f53a286f 43
agari 1:5dd2f53a286f 44 else if(*(action.data)== 'j') {
Arare 2:213c12298d45 45 led_on(0,0,1,0);
agari 1:5dd2f53a286f 46 snap = 1;
agari 1:5dd2f53a286f 47 wait(0.15);
agari 1:5dd2f53a286f 48 grab = 1;
agari 1:5dd2f53a286f 49 }
agari 1:5dd2f53a286f 50
agari 1:5dd2f53a286f 51 else if(*(action.data)=='l') {
agari 1:5dd2f53a286f 52 snap = 0;
agari 1:5dd2f53a286f 53 grab = 0;
Arare 2:213c12298d45 54 led_on(0,0,0,1);
agari 1:5dd2f53a286f 55 } else if(*(action.data)=='m') {
Arare 2:213c12298d45 56 led_on(1,1,0,0);
agari 1:5dd2f53a286f 57 snap = 0;
agari 1:5dd2f53a286f 58 //pc.printf("a\n");
agari 1:5dd2f53a286f 59 wait(0.195); //TZ1:0.18 TZ2:0.18 TZ3:0.167,0.168 椅子(高度45[cm]):0.195
agari 1:5dd2f53a286f 60 grab = 1;
Arare 2:213c12298d45 61
agari 1:5dd2f53a286f 62 }
agari 1:5dd2f53a286f 63 nh.spinOnce();
agari 1:5dd2f53a286f 64 }
Arare 2:213c12298d45 65
agari 0:f6fa58c56955 66 }