ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@22:37a07f844778, 2018-04-05 (annotated)

Committer:
Arare
Date:
Thu Apr 05 07:52:54 2018 +0000
Revision:
22:37a07f844778
Parent:
21:90bb12a879c2
Child:
23:ffe4f63bee36
Arare;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arare 10:a05e9a8980db 1 #include <mbed.h>
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 11:5a06fd933e55 5 #define R_C_MAX 2
Arare 10:a05e9a8980db 6
Arare 6:ca4fab2957e4 7 Serial pc(USBTX,USBRX);
Arare 6:ca4fab2957e4 8
Arare 10:a05e9a8980db 9 DigitalOut grab(p11); //0:開く 1:閉じる
Arare 10:a05e9a8980db 10 DigitalOut snap(p14); //1:装填 0:発射
Arare 10:a05e9a8980db 11 DigitalOut s_grab(p13); //0:解放 1:把持
283 14:3858894d1a6b 12 DigitalOut fuck(p17); //0:引抜 1:突出
Arare 11:5a06fd933e55 13 DigitalOut grab2(p18); //0:開く 1:閉じる
Arare 11:5a06fd933e55 14 DigitalOut snap2(p15); //1:装填 0:発射
Arare 11:5a06fd933e55 15 DigitalOut s_grab2(p16); //0:解放 1:把持
Arare 10:a05e9a8980db 16 DigitalOut ledw(p20); //射出LED
Arare 22:37a07f844778 17 DigitalIn enable(p21);
Arare 22:37a07f844778 18
Arare 2:213c12298d45 19 DigitalOut led1(LED1);
Arare 2:213c12298d45 20 DigitalOut led2(LED2);
Arare 2:213c12298d45 21 DigitalOut led3(LED3);
Arare 2:213c12298d45 22 DigitalOut led4(LED4);
283 14:3858894d1a6b 23 DigitalOut LEDN(p10);
Arare 22:37a07f844778 24 //DigitalOut out(p21);
Arare 10:a05e9a8980db 25
Arare 6:ca4fab2957e4 26 std_msgs::String action;
Arare 6:ca4fab2957e4 27 std_msgs::String fin_or_not;
Arare 6:ca4fab2957e4 28 std_msgs::String get_or_not;
Arare 10:a05e9a8980db 29
Arare 10:a05e9a8980db 30 int count=0; //何回actionしたんですか?
Arare 10:a05e9a8980db 31 int have_cock=0; //コック持ってますか？
Arare 10:a05e9a8980db 32 int have_action=0; //装填実行
Arare 6:ca4fab2957e4 33 int ready_action=0; //装填待機
Arare 6:ca4fab2957e4 34 int32_t act=101;
agari 21:90bb12a879c2 35 int ball_judgement=1;
Arare 11:5a06fd933e55 36 int finding=0;
Arare 10:a05e9a8980db 37
Arare 10:a05e9a8980db 38 Ticker MC;
Arare 6:ca4fab2957e4 39 std_msgs::Int32 pub_act;
Arare 10:a05e9a8980db 40 std_msgs::Int32 pub_b;
Arare 11:5a06fd933e55 41 std_msgs::Int32 pub_c1;
Arare 11:5a06fd933e55 42 std_msgs::Int32 pub_c2;
Arare 11:5a06fd933e55 43 std_msgs::Int32 pub_fin;
Arare 6:ca4fab2957e4 44 ros::NodeHandle nh;
Arare 6:ca4fab2957e4 45 ros::Publisher pub_action("act_pose", &pub_act);
Arare 10:a05e9a8980db 46 ros::Publisher pub_ball("act_ball", &pub_b);
Arare 11:5a06fd933e55 47 ros::Publisher pub_color1("act_color1", &pub_c1);
Arare 11:5a06fd933e55 48 ros::Publisher pub_color2("act_color2", &pub_c2);
Arare 11:5a06fd933e55 49 ros::Publisher pub_finc("shoot_fin", &pub_fin);
Arare 10:a05e9a8980db 50
Arare 6:ca4fab2957e4 51 void messageCallback(const std_msgs::Int32 &msg)
agari 3:2184f6f5c8e3 52 {
Arare 17:df95c0fb7b87 53 if(act!=9&&msg.data==9){
Arare 17:df95c0fb7b87 54 ready_action=1;
Arare 17:df95c0fb7b87 55 count=0;
Arare 17:df95c0fb7b87 56 }
Arare 6:ca4fab2957e4 57 act=msg.data;
Arare 2:213c12298d45 58 }
Arare 11:5a06fd933e55 59
Arare 11:5a06fd933e55 60 void messageCallback2(const std_msgs::Int32 &msg)
Arare 11:5a06fd933e55 61 {
283 19:06246ffe0235 62 if((act==1)||(act==3)||(act==5)||(act==10)||(act==11))ball_judgement=msg.data;
Arare 11:5a06fd933e55 63 }
Arare 11:5a06fd933e55 64
Arare 11:5a06fd933e55 65 void messageCallback3(const std_msgs::Int32 &msg)
Arare 11:5a06fd933e55 66 {
Arare 11:5a06fd933e55 67 finding=msg.data;
Arare 11:5a06fd933e55 68 }
Arare 11:5a06fd933e55 69
Arare 6:ca4fab2957e4 70 void MCL()
Arare 6:ca4fab2957e4 71 {
Arare 10:a05e9a8980db 72 pub_b.data=ball_judgement;
Arare 6:ca4fab2957e4 73 pub_action.publish(&pub_act);
Arare 11:5a06fd933e55 74 pub_color1.publish(&pub_c1);
Arare 11:5a06fd933e55 75 pub_color2.publish(&pub_c2);
Arare 10:a05e9a8980db 76 pub_ball.publish(&pub_b);
Arare 6:ca4fab2957e4 77 nh.spinOnce();
Arare 6:ca4fab2957e4 78 }
Arare 6:ca4fab2957e4 79 //定点に到着した時にTz1:a,Tz2:b,Tz3:cの文字を足回りから送る。topicはshoot_action
Arare 6:ca4fab2957e4 80 //装填用の信号はdとしておく。後で変更して、どうぞ。
Arare 6:ca4fab2957e4 81 ros::Subscriber<std_msgs::Int32> sub("shoot_action",&messageCallback);
Arare 12:6832bc92272f 82 ros::Subscriber<std_msgs::Int32> sub_TZ("tz",&messageCallback2);
Arare 11:5a06fd933e55 83 ros::Subscriber<std_msgs::Int32> sub_find("find",&messageCallback3);
Arare 6:ca4fab2957e4 84
Arare 10:a05e9a8980db 85 /////射出サイクル/////
Arare 10:a05e9a8980db 86 void throw_cock(float waittime)
Arare 10:a05e9a8980db 87 {
Arare 10:a05e9a8980db 88 if (act==2) {
Arare 10:a05e9a8980db 89 led2=1;
283 14:3858894d1a6b 90 fuck=1;
Arare 11:5a06fd933e55 91 s_grab=0;
Arare 11:5a06fd933e55 92 wait(2);
Arare 11:5a06fd933e55 93 ledw=1;
Arare 11:5a06fd933e55 94 wait(1);
Arare 11:5a06fd933e55 95 snap=0;
Arare 11:5a06fd933e55 96 wait(waittime);
Arare 11:5a06fd933e55 97 grab=0;
Arare 11:5a06fd933e55 98 wait(1);
Arare 11:5a06fd933e55 99 ledw=0;
Arare 10:a05e9a8980db 100 }
Arare 10:a05e9a8980db 101 else if (act==4) {
Arare 10:a05e9a8980db 102 led3=1;
283 14:3858894d1a6b 103 fuck=1;
Arare 11:5a06fd933e55 104 s_grab2=0;
Arare 11:5a06fd933e55 105 wait(2);
Arare 11:5a06fd933e55 106 ledw=1;
Arare 11:5a06fd933e55 107 wait(1);
Arare 11:5a06fd933e55 108 snap2=0;
Arare 11:5a06fd933e55 109 wait(waittime);
Arare 11:5a06fd933e55 110 grab2=0;
Arare 11:5a06fd933e55 111 wait(1);
Arare 11:5a06fd933e55 112 ledw=0;
Arare 10:a05e9a8980db 113 }
Arare 10:a05e9a8980db 114 else if (act==6) {
Arare 10:a05e9a8980db 115 led2=1;
Arare 10:a05e9a8980db 116 led3=1;
283 14:3858894d1a6b 117 fuck=1;
Arare 11:5a06fd933e55 118 s_grab2=0;
Arare 11:5a06fd933e55 119 wait(2);
Arare 11:5a06fd933e55 120 ledw=1;
Arare 11:5a06fd933e55 121 wait(1);
Arare 11:5a06fd933e55 122 snap2=0;
Arare 11:5a06fd933e55 123 wait(waittime);
Arare 11:5a06fd933e55 124 grab2=0;
Arare 11:5a06fd933e55 125 wait(1);
Arare 11:5a06fd933e55 126 ledw=0;
Arare 10:a05e9a8980db 127 }
Arare 10:a05e9a8980db 128 else if (act==7) {
283 14:3858894d1a6b 129 fuck=1;
283 18:0184238dccc3 130 s_grab=0;
283 18:0184238dccc3 131 wait(2);
283 18:0184238dccc3 132 ledw=1;
283 18:0184238dccc3 133 wait(1);
283 18:0184238dccc3 134 snap=0;
283 18:0184238dccc3 135 wait(waittime);
283 18:0184238dccc3 136 grab=0;
283 18:0184238dccc3 137 wait(1);
283 18:0184238dccc3 138 ledw=0;
283 18:0184238dccc3 139 }
283 18:0184238dccc3 140 else if (act==8) {
283 18:0184238dccc3 141 fuck=1;
Arare 17:df95c0fb7b87 142 s_grab2=0;
Arare 17:df95c0fb7b87 143 wait(2);
Arare 17:df95c0fb7b87 144 ledw=1;
Arare 17:df95c0fb7b87 145 wait(1);
Arare 17:df95c0fb7b87 146 snap2=0;
Arare 17:df95c0fb7b87 147 wait(waittime);
Arare 17:df95c0fb7b87 148 grab2=0;
Arare 17:df95c0fb7b87 149 wait(1);
Arare 17:df95c0fb7b87 150 ledw=0;
Arare 17:df95c0fb7b87 151 }
283 18:0184238dccc3 152 else if (act==9) {
Arare 17:df95c0fb7b87 153 fuck=1;
283 18:0184238dccc3 154 s_grab2=0;
Arare 11:5a06fd933e55 155 wait(2);
Arare 11:5a06fd933e55 156 ledw=1;
Arare 11:5a06fd933e55 157 wait(1);
283 18:0184238dccc3 158 snap2=0;
283 14:3858894d1a6b 159 wait(waittime);
283 18:0184238dccc3 160 grab2=0;
Arare 11:5a06fd933e55 161 wait(1);
Arare 11:5a06fd933e55 162 ledw=0;
Arare 10:a05e9a8980db 163 }
Arare 10:a05e9a8980db 164
Arare 10:a05e9a8980db 165 if (act==2) {
Arare 10:a05e9a8980db 166 led2=0;
Arare 10:a05e9a8980db 167 }
Arare 10:a05e9a8980db 168 else if (act==4) {
Arare 10:a05e9a8980db 169 led3=0;
Arare 10:a05e9a8980db 170 }
Arare 10:a05e9a8980db 171 else if (act==6) {
Arare 10:a05e9a8980db 172 led2=0;
Arare 10:a05e9a8980db 173 led3=0;
Arare 10:a05e9a8980db 174 }
Arare 10:a05e9a8980db 175 else if (act==7) {
Arare 10:a05e9a8980db 176 led1=0;
Arare 10:a05e9a8980db 177 led2=0;
Arare 10:a05e9a8980db 178 led3=0;
Arare 10:a05e9a8980db 179 }
Arare 10:a05e9a8980db 180 }
Arare 10:a05e9a8980db 181
Arare 10:a05e9a8980db 182
agari 1:5dd2f53a286f 183 int main(int argc, char **argv)
agari 1:5dd2f53a286f 184 {
agari 21:90bb12a879c2 185 snap=1;
agari 21:90bb12a879c2 186 snap2=1;
Arare 22:37a07f844778 187 //out=1;
Arare 10:a05e9a8980db 188
Arare 2:213c12298d45 189 nh.initNode();
Arare 2:213c12298d45 190 nh.subscribe(sub);
Arare 11:5a06fd933e55 191 nh.subscribe(sub_TZ);
Arare 11:5a06fd933e55 192 nh.subscribe(sub_find);
Arare 6:ca4fab2957e4 193 nh.advertise(pub_action);
Arare 10:a05e9a8980db 194 nh.advertise(pub_ball);
Arare 11:5a06fd933e55 195 nh.advertise(pub_color1);
Arare 11:5a06fd933e55 196 nh.advertise(pub_color2);
Arare 11:5a06fd933e55 197 nh.advertise(pub_finc);
Arare 6:ca4fab2957e4 198 MC.attach(&MCL,0.01);
Arare 6:ca4fab2957e4 199 pub_act.data=0;
Arare 11:5a06fd933e55 200 pub_fin.data=0;
Arare 10:a05e9a8980db 201 pub_b.data=0;
Arare 22:37a07f844778 202
agari 3:2184f6f5c8e3 203 while(1) {
Arare 22:37a07f844778 204 if(enable){
Arare 22:37a07f844778 205 count=0; //何回actionしたんですか?
Arare 22:37a07f844778 206 have_cock=0; //コック持ってますか？
Arare 22:37a07f844778 207 have_action=0; //装填実行
Arare 22:37a07f844778 208 ready_action=0; //装填待機
Arare 22:37a07f844778 209 act=101;
Arare 22:37a07f844778 210 ball_judgement=1;
Arare 22:37a07f844778 211 finding=0;
Arare 22:37a07f844778 212 snap=1;
Arare 22:37a07f844778 213 snap2=1;
Arare 22:37a07f844778 214 //out=1;
Arare 22:37a07f844778 215 led1=1;
Arare 22:37a07f844778 216 led2=1;
Arare 22:37a07f844778 217 led3=1;
Arare 22:37a07f844778 218 led4=1;
Arare 22:37a07f844778 219 }
Arare 22:37a07f844778 220 else{
Arare 22:37a07f844778 221 led1=0;
Arare 22:37a07f844778 222 led2=0;
Arare 22:37a07f844778 223 led3=0;
Arare 22:37a07f844778 224 led4=0;
Arare 22:37a07f844778 225 }
Arare 10:a05e9a8980db 226 ///装填///
283 16:85746e242e49 227 if(((act==1)||(act==3)||(act==5)||(act==10)||(act==11))&&(ready_action==0)) { //TZ3
Arare 11:5a06fd933e55 228 snap=1;
Arare 11:5a06fd933e55 229 snap2=1;
Arare 12:6832bc92272f 230 grab=0;
Arare 12:6832bc92272f 231 grab2=0;
283 14:3858894d1a6b 232 fuck=0;
Arare 20:81a956eaf4ae 233 s_grab=0;
Arare 20:81a956eaf4ae 234 s_grab2=0;
Arare 12:6832bc92272f 235 while(finding!=1){
Arare 12:6832bc92272f 236 led1=0;
Arare 12:6832bc92272f 237 led2=0;
Arare 12:6832bc92272f 238 led3=0;
Arare 12:6832bc92272f 239 led4=0;
283 14:3858894d1a6b 240 LEDN=1;
Arare 12:6832bc92272f 241 }
Arare 11:5a06fd933e55 242 if(ball_judgement==1){
Arare 12:6832bc92272f 243 led1=0;
Arare 12:6832bc92272f 244 led2=1;
Arare 12:6832bc92272f 245 led3=0;
Arare 12:6832bc92272f 246 led4=0;
Arare 13:84f136900461 247 wait(2);//桃井escape
Arare 12:6832bc92272f 248 grab=1;
283 14:3858894d1a6b 249 wait(0.5);
Arare 12:6832bc92272f 250 s_grab=1;
283 14:3858894d1a6b 251 wait(0.5);
283 14:3858894d1a6b 252 fuck=0;
Arare 12:6832bc92272f 253 }
Arare 11:5a06fd933e55 254 else if(ball_judgement==2){
Arare 12:6832bc92272f 255 led1=0;
Arare 12:6832bc92272f 256 led2=0;
Arare 12:6832bc92272f 257 led3=1;
Arare 12:6832bc92272f 258 led4=0;
283 14:3858894d1a6b 259
Arare 13:84f136900461 260 wait(2);//桃井escape
Arare 12:6832bc92272f 261 grab2=1;
283 14:3858894d1a6b 262 wait(0.5);
Arare 12:6832bc92272f 263 s_grab2=1;
283 14:3858894d1a6b 264 wait(0.5);
283 14:3858894d1a6b 265 fuck=0;
Arare 11:5a06fd933e55 266 }
Arare 15:c27950efe96f 267 else if(ball_judgement>=3){
Arare 12:6832bc92272f 268 led1=0;
Arare 12:6832bc92272f 269 led2=0;
Arare 12:6832bc92272f 270 led3=0;
Arare 12:6832bc92272f 271 led4=1;
Arare 13:84f136900461 272 wait(2);//桃井escape
Arare 12:6832bc92272f 273 grab=1;
Arare 12:6832bc92272f 274 grab2=1;
283 14:3858894d1a6b 275 wait(0.5);
Arare 12:6832bc92272f 276 s_grab=1;
Arare 12:6832bc92272f 277 s_grab2=1;
283 14:3858894d1a6b 278 wait(0.5);
283 14:3858894d1a6b 279 fuck=0;
Arare 11:5a06fd933e55 280 }
Arare 12:6832bc92272f 281 else{
Arare 12:6832bc92272f 282 led1=1;
Arare 12:6832bc92272f 283 led2=0;
Arare 12:6832bc92272f 284 led3=0;
Arare 12:6832bc92272f 285 led4=0;
Arare 12:6832bc92272f 286 }
Arare 11:5a06fd933e55 287 if(act==1) {
Arare 11:5a06fd933e55 288 led1=0;
Arare 11:5a06fd933e55 289 }
Arare 11:5a06fd933e55 290 else if(act==3) {
Arare 11:5a06fd933e55 291 led1=0;
Arare 11:5a06fd933e55 292 led2=0;
Arare 11:5a06fd933e55 293 }
Arare 11:5a06fd933e55 294 else if(act==5) {
Arare 11:5a06fd933e55 295 led1=0;
Arare 11:5a06fd933e55 296 led3=0;
Arare 11:5a06fd933e55 297 }
Arare 11:5a06fd933e55 298 else if(act==8) {
Arare 11:5a06fd933e55 299 led4=0;
Arare 11:5a06fd933e55 300 }
283 14:3858894d1a6b 301 LEDN=0;
Arare 11:5a06fd933e55 302 ready_action=1;
Arare 11:5a06fd933e55 303 count=0;
Arare 11:5a06fd933e55 304 pub_act.data=act;
Arare 11:5a06fd933e55 305 pub_action.publish(&pub_act);
Arare 6:ca4fab2957e4 306 }
Arare 10:a05e9a8980db 307
Arare 10:a05e9a8980db 308 ///射出///
Arare 10:a05e9a8980db 309 if((act==2)&&(count==0)) {
Arare 12:6832bc92272f 310 int f_i=0;
Arare 22:37a07f844778 311 //throw_cock(0.247); //TZ1
Arare 22:37a07f844778 312 throw_cock(0.242); //TZ1 03_29 0.247-down
Arare 10:a05e9a8980db 313
Arare 10:a05e9a8980db 314 ready_action=0;
Arare 10:a05e9a8980db 315 count=1;
Arare 10:a05e9a8980db 316 pub_act.data=act;
Arare 10:a05e9a8980db 317 pub_action.publish(&pub_act);
Arare 12:6832bc92272f 318 for(f_i=0;f_i<20;f_i++){
Arare 12:6832bc92272f 319 pub_fin.data=1;
Arare 12:6832bc92272f 320 pub_finc.publish(&pub_fin);
Arare 12:6832bc92272f 321 wait(0.01);
Arare 12:6832bc92272f 322 }
Arare 10:a05e9a8980db 323 }
Arare 10:a05e9a8980db 324
Arare 10:a05e9a8980db 325 if(((act==4)||(act==6))&&(count==0)) {
Arare 12:6832bc92272f 326 int f_i=0;
Arare 22:37a07f844778 327 //throw_cock(0.257); //TZ2
Arare 22:37a07f844778 328 throw_cock(0.238); //TZ2 03_29 0.257-tyou down
Arare 22:37a07f844778 329
Arare 10:a05e9a8980db 330 ready_action=0;
Arare 10:a05e9a8980db 331 count=1;
Arare 10:a05e9a8980db 332 pub_act.data=act;
Arare 10:a05e9a8980db 333 pub_action.publish(&pub_act);
Arare 12:6832bc92272f 334 for(f_i=0;f_i<20;f_i++){
Arare 12:6832bc92272f 335 pub_fin.data=1;
Arare 12:6832bc92272f 336 pub_finc.publish(&pub_fin);
Arare 12:6832bc92272f 337 wait(0.01);
Arare 12:6832bc92272f 338 }
Arare 10:a05e9a8980db 339 }
Arare 6:ca4fab2957e4 340
283 16:85746e242e49 341 if(((act==7)||(act==8)||(act==9))&&(count==0)) {
Arare 12:6832bc92272f 342 int f_i=0;
Arare 22:37a07f844778 343 //throw_cock(0.227); //TZ3
Arare 22:37a07f844778 344 throw_cock(0.221); //TZ3 0.227-down
Arare 10:a05e9a8980db 345
Arare 10:a05e9a8980db 346 ready_action=0;
Arare 10:a05e9a8980db 347 count=1;
Arare 10:a05e9a8980db 348 pub_act.data=act;
Arare 10:a05e9a8980db 349 pub_action.publish(&pub_act);
Arare 12:6832bc92272f 350 for(f_i=0;f_i<20;f_i++){
Arare 12:6832bc92272f 351 pub_fin.data=1;
Arare 12:6832bc92272f 352 pub_finc.publish(&pub_fin);
Arare 12:6832bc92272f 353 wait(0.01);
Arare 12:6832bc92272f 354 }
Arare 10:a05e9a8980db 355 }
Arare 10:a05e9a8980db 356
Arare 10:a05e9a8980db 357 //一連の動作が終了したかを判断する。
Arare 10:a05e9a8980db 358 if(count>0) {
Arare 10:a05e9a8980db 359 have_cock=0;
Arare 10:a05e9a8980db 360 have_action=0;
Arare 10:a05e9a8980db 361 }
agari 3:2184f6f5c8e3 362 }
Arare 10:a05e9a8980db 363 }