ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@8:5d8b23cec6d1, 2017-12-02 (annotated)

Committer:
Arare
Date:
Sat Dec 02 02:35:26 2017 +0000
Revision:
8:5d8b23cec6d1
Parent:
7:2827264ddf53
Child:
9:1dcd40da31ec
homo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arare 6:ca4fab2957e4 1 #include <mbed.h>///////////////////////レーザーとカラーセンサーなしのwait受け渡し用
agari 0:f6fa58c56955 2 #include <ros.h>
Arare 6:ca4fab2957e4 3 #include <std_msgs/Int32.h>
agari 0:f6fa58c56955 4 #include <std_msgs/String.h>
Arare 8:5d8b23cec6d1 5 #define RED_MIN 2500
Arare 8:5d8b23cec6d1 6 #define R_C_MAX 10
Arare 6:ca4fab2957e4 7 //s_grab: 1:閉じる,0:開く
Arare 6:ca4fab2957e4 8 //grab: 1close,2:open
Arare 6:ca4fab2957e4 9 //snap:1 slow 0:catch
Arare 6:ca4fab2957e4 10 //s_grab 1:close 0:open
Arare 6:ca4fab2957e4 11 Serial pc(USBTX,USBRX);
Arare 6:ca4fab2957e4 12 I2C i2c(p9, p10); // sda, scl
Arare 6:ca4fab2957e4 13
Arare 6:ca4fab2957e4 14 DigitalOut grab(p11); //0:開く 1:閉じる
Arare 6:ca4fab2957e4 15 DigitalOut snap(p14); //1:装填 0:発射
Arare 6:ca4fab2957e4 16 DigitalOut s_grab(p13); //0:otiru 1:ageru
Arare 2:213c12298d45 17 DigitalOut led1(LED1);
Arare 2:213c12298d45 18 DigitalOut led2(LED2);
Arare 2:213c12298d45 19 DigitalOut led3(LED3);
Arare 2:213c12298d45 20 DigitalOut led4(LED4);
Arare 7:2827264ddf53 21 DigitalOut led5(p20);
Arare 7:2827264ddf53 22 DigitalOut led6(p21);
Arare 7:2827264ddf53 23 DigitalOut led7(p22);
Arare 6:ca4fab2957e4 24 std_msgs::String action;
Arare 6:ca4fab2957e4 25 std_msgs::String fin_or_not;
Arare 6:ca4fab2957e4 26 std_msgs::String get_or_not;
Arare 6:ca4fab2957e4 27 int count=0;
Arare 6:ca4fab2957e4 28 //何回actionしたんですか?
Arare 6:ca4fab2957e4 29 int have_cock=0;
Arare 6:ca4fab2957e4 30 //コック持ってますか？
Arare 6:ca4fab2957e4 31 int have_action=0;
Arare 6:ca4fab2957e4 32 //装填実行
Arare 6:ca4fab2957e4 33 int ready_action=0; //装填待機
Arare 6:ca4fab2957e4 34 int r,g,b,a;
Arare 6:ca4fab2957e4 35 int val;
Arare 6:ca4fab2957e4 36 int red=0;
Arare 6:ca4fab2957e4 37 Ticker MC;
Arare 6:ca4fab2957e4 38 int32_t act=101;
Arare 6:ca4fab2957e4 39 std_msgs::Int32 pub_act;
Arare 6:ca4fab2957e4 40 ros::NodeHandle nh;
Arare 6:ca4fab2957e4 41 ros::Publisher pub_action("act_pose", &pub_act);
Arare 6:ca4fab2957e4 42 void messageCallback(const std_msgs::Int32 &msg)
agari 3:2184f6f5c8e3 43 {
Arare 6:ca4fab2957e4 44 act=msg.data;
Arare 2:213c12298d45 45 }
Arare 6:ca4fab2957e4 46 void MCL()
Arare 6:ca4fab2957e4 47 {
Arare 6:ca4fab2957e4 48 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 49 nh.spinOnce();
Arare 6:ca4fab2957e4 50 }
Arare 6:ca4fab2957e4 51 //定点に到着した時にTz1:a,Tz2:b,Tz3:cの文字を足回りから送る。topicはshoot_action
Arare 6:ca4fab2957e4 52 //装填用の信号はdとしておく。後で変更して、どうぞ。
Arare 6:ca4fab2957e4 53 ros::Subscriber<std_msgs::Int32> sub("shoot_action",&messageCallback);
Arare 6:ca4fab2957e4 54
Arare 6:ca4fab2957e4 55 //信号は一回しか受け取らないようにしておく。
agari 1:5dd2f53a286f 56 int main(int argc, char **argv)
agari 1:5dd2f53a286f 57 {
Arare 6:ca4fab2957e4 58 pub_act.data=0;
Arare 2:213c12298d45 59 nh.initNode();
Arare 2:213c12298d45 60 nh.subscribe(sub);
Arare 6:ca4fab2957e4 61 nh.advertise(pub_action);
Arare 6:ca4fab2957e4 62 MC.attach(&MCL,0.01);
Arare 6:ca4fab2957e4 63 pub_act.data=0;
Arare 8:5d8b23cec6d1 64 int r_c=0;
Arare 6:ca4fab2957e4 65 char cmd[2];
Arare 6:ca4fab2957e4 66 char cell[1]= {0x03};
Arare 6:ca4fab2957e4 67 char data[8]= {0,0,0,0,0,0,0,0};
Arare 6:ca4fab2957e4 68 cmd[0] = 0x00;
Arare 6:ca4fab2957e4 69 cmd[1] = 0x89;
Arare 6:ca4fab2957e4 70 i2c.frequency(115200);
Arare 6:ca4fab2957e4 71 val = i2c.write(84, cmd, 2);
Arare 6:ca4fab2957e4 72 cmd[0] = 0x0;
Arare 6:ca4fab2957e4 73 cmd[1] = 0x09;
Arare 6:ca4fab2957e4 74 val = i2c.write(84, cmd, 2);
Arare 7:2827264ddf53 75 //servo.period_ms(20);
Arare 6:ca4fab2957e4 76 //int jk=0;
agari 3:2184f6f5c8e3 77 while(1) {
Arare 6:ca4fab2957e4 78 //wait_ms(50);//最小値ではない
Arare 6:ca4fab2957e4 79 val = i2c.write(84, cell, 1);
Arare 6:ca4fab2957e4 80 val = i2c.read(84, data, 8);
Arare 6:ca4fab2957e4 81 red = data[0]<<8 | data[1];
Arare 6:ca4fab2957e4 82 //pc.printf("%d",red);
Arare 6:ca4fab2957e4 83 if((act==100)&&(ready_action==0)) {
Arare 6:ca4fab2957e4 84 snap=1;
Arare 6:ca4fab2957e4 85 grab=0;
Arare 7:2827264ddf53 86 led5=1;
Arare 7:2827264ddf53 87 led6=0;
Arare 7:2827264ddf53 88 led7=0;
Arare 6:ca4fab2957e4 89 if(red<RED_MIN){
Arare 7:2827264ddf53 90 led5=0;
Arare 7:2827264ddf53 91 led6=1;
Arare 7:2827264ddf53 92 led7=0;
Arare 8:5d8b23cec6d1 93 r_c++;
Arare 8:5d8b23cec6d1 94 }
Arare 8:5d8b23cec6d1 95 else r_c=0;
Arare 8:5d8b23cec6d1 96 if(r_c>R_C_MAX){
Arare 6:ca4fab2957e4 97 grab=1;
Arare 6:ca4fab2957e4 98 s_grab=0;
Arare 6:ca4fab2957e4 99 ready_action=1;
Arare 6:ca4fab2957e4 100 count=0;
Arare 6:ca4fab2957e4 101 pub_act.data=act;
Arare 6:ca4fab2957e4 102 pub_action.publish(&pub_act);
Arare 8:5d8b23cec6d1 103 r_c=0;
Arare 6:ca4fab2957e4 104 }
Arare 6:ca4fab2957e4 105 }
Arare 6:ca4fab2957e4 106 if((act==2)&&(ready_action==0)) {
Arare 6:ca4fab2957e4 107 led2=1;
Arare 6:ca4fab2957e4 108 snap=1;
Arare 6:ca4fab2957e4 109 grab=0;
Arare 6:ca4fab2957e4 110 if(red<RED_MIN){
Arare 8:5d8b23cec6d1 111 led5=0;
Arare 8:5d8b23cec6d1 112 led6=1;
Arare 8:5d8b23cec6d1 113 led7=0;
Arare 8:5d8b23cec6d1 114 r_c++;
Arare 8:5d8b23cec6d1 115 }
Arare 8:5d8b23cec6d1 116 else r_c=0;
Arare 8:5d8b23cec6d1 117 if(r_c>R_C_MAX){
Arare 6:ca4fab2957e4 118 grab=1;
Arare 6:ca4fab2957e4 119 s_grab=0;
Arare 6:ca4fab2957e4 120 ready_action=1;
Arare 6:ca4fab2957e4 121 count=0;
Arare 6:ca4fab2957e4 122 pub_act.data=act;
Arare 6:ca4fab2957e4 123 pub_action.publish(&pub_act);
Arare 8:5d8b23cec6d1 124 r_c=0;
Arare 6:ca4fab2957e4 125 }
Arare 6:ca4fab2957e4 126 }
Arare 6:ca4fab2957e4 127 if((act==5)&&(ready_action==0)) {
Arare 6:ca4fab2957e4 128 snap=1;
Arare 6:ca4fab2957e4 129 grab=0;
Arare 6:ca4fab2957e4 130 if(red<RED_MIN){
Arare 8:5d8b23cec6d1 131 led5=0;
Arare 8:5d8b23cec6d1 132 led6=1;
Arare 8:5d8b23cec6d1 133 led7=0;
Arare 8:5d8b23cec6d1 134 r_c++;
Arare 8:5d8b23cec6d1 135 }
Arare 8:5d8b23cec6d1 136 else r_c=0;
Arare 8:5d8b23cec6d1 137 if(r_c>R_C_MAX){
Arare 6:ca4fab2957e4 138 grab=1;
Arare 6:ca4fab2957e4 139 s_grab=0;
Arare 6:ca4fab2957e4 140 ready_action=1;
Arare 6:ca4fab2957e4 141 count=0;
Arare 6:ca4fab2957e4 142 pub_act.data=act;
Arare 6:ca4fab2957e4 143 pub_action.publish(&pub_act);
Arare 8:5d8b23cec6d1 144 r_c=0;
Arare 6:ca4fab2957e4 145 }
agari 5:4f1f0294d6aa 146 }
Arare 6:ca4fab2957e4 147 //get ready
Arare 6:ca4fab2957e4 148 //shashutu
Arare 7:2827264ddf53 149 if((act== 1)&&(count==0)) {
Arare 6:ca4fab2957e4 150 led1=1;
Arare 6:ca4fab2957e4 151 s_grab=1;
Arare 6:ca4fab2957e4 152 wait(3);
Arare 6:ca4fab2957e4 153 snap=0;
Arare 8:5d8b23cec6d1 154 wait(0.183); //TZ1
Arare 6:ca4fab2957e4 155 grab=0;
Arare 6:ca4fab2957e4 156 ready_action=0;
Arare 6:ca4fab2957e4 157 count=1;
Arare 6:ca4fab2957e4 158 pub_act.data=act;
Arare 6:ca4fab2957e4 159 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 160 }
Arare 6:ca4fab2957e4 161 if((act== 3)&&(count==0)) {
Arare 6:ca4fab2957e4 162 s_grab=1;
Arare 6:ca4fab2957e4 163 wait(3);
Arare 6:ca4fab2957e4 164 snap=0;
Arare 8:5d8b23cec6d1 165 wait(0.183); //TZ1
Arare 6:ca4fab2957e4 166 grab=0;
Arare 6:ca4fab2957e4 167 ready_action=0;
Arare 6:ca4fab2957e4 168 count=1;
Arare 6:ca4fab2957e4 169 pub_act.data=act;
Arare 6:ca4fab2957e4 170 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 171 }
Arare 6:ca4fab2957e4 172
Arare 6:ca4fab2957e4 173 if((act==4)&&(count==0)) {
Arare 6:ca4fab2957e4 174 led4=1;
Arare 6:ca4fab2957e4 175 s_grab=1;
Arare 6:ca4fab2957e4 176 wait(3);
Arare 6:ca4fab2957e4 177 snap=0;
Arare 8:5d8b23cec6d1 178 wait(0.182); //TZ2
Arare 6:ca4fab2957e4 179 grab=0;
Arare 6:ca4fab2957e4 180 ready_action=0;
Arare 6:ca4fab2957e4 181 count=1;
Arare 6:ca4fab2957e4 182 pub_act.data=act;
Arare 6:ca4fab2957e4 183 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 184 }
Arare 6:ca4fab2957e4 185 if((act==6)&&(count==0)) {
Arare 6:ca4fab2957e4 186 s_grab=1;
Arare 6:ca4fab2957e4 187 wait(3);
Arare 6:ca4fab2957e4 188 snap=0;
Arare 8:5d8b23cec6d1 189 wait(0.182); //TZ2
Arare 6:ca4fab2957e4 190 grab=0;
Arare 6:ca4fab2957e4 191 ready_action=0;
Arare 6:ca4fab2957e4 192 count=1;
Arare 6:ca4fab2957e4 193 pub_act.data=act;
Arare 6:ca4fab2957e4 194 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 195 }
Arare 6:ca4fab2957e4 196 if((act==7)&&(count==0)) {
Arare 6:ca4fab2957e4 197 s_grab=1;
Arare 6:ca4fab2957e4 198 wait(3);
Arare 6:ca4fab2957e4 199 snap=0;
Arare 8:5d8b23cec6d1 200 wait(0.152); //TZ3
Arare 6:ca4fab2957e4 201 grab=0;
Arare 6:ca4fab2957e4 202 ready_action=0;
Arare 6:ca4fab2957e4 203 count=1;
Arare 6:ca4fab2957e4 204 pub_act.data=act;
Arare 6:ca4fab2957e4 205 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 206 }
Arare 6:ca4fab2957e4 207 //一連の動作が終了したかを判断する。
Arare 6:ca4fab2957e4 208 if(count>0) {
Arare 6:ca4fab2957e4 209 //count=0;
Arare 6:ca4fab2957e4 210 have_cock=0;
Arare 6:ca4fab2957e4 211 have_action=0;
Arare 6:ca4fab2957e4 212 //have_cock=0;
Arare 6:ca4fab2957e4 213 //ready_action=0;
Arare 6:ca4fab2957e4 214 }
Arare 6:ca4fab2957e4 215 //if(ready_action>0){
Arare 6:ca4fab2957e4 216 // count=0;
Arare 6:ca4fab2957e4 217 // }
Arare 6:ca4fab2957e4 218
Arare 6:ca4fab2957e4 219 //if(pub_act.data!=act)pub_act.data=0;
agari 3:2184f6f5c8e3 220 }
Arare 6:ca4fab2957e4 221
agari 0:f6fa58c56955 222 }