ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@23:ffe4f63bee36, 2018-04-09 (annotated)

Committer:
Arare
Date:
Mon Apr 09 14:48:36 2018 +0000
Revision:
23:ffe4f63bee36
Parent:
22:37a07f844778 
se

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