ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@15:c27950efe96f, 2018-03-21 (annotated)

Committer:
Arare
Date:
Wed Mar 21 15:31:50 2018 +0000
Revision:
15:c27950efe96f
Parent:
14:3858894d1a6b
Child:
16:85746e242e49
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 10:a05e9a8980db 8 I2C i2c(p9, p10); // sda, scl
Arare 10:a05e9a8980db 9 I2C i2c_2(p28, p27);
Arare 6:ca4fab2957e4 10
Arare 10:a05e9a8980db 11 DigitalOut grab(p11); //0:開く 1:閉じる
Arare 10:a05e9a8980db 12 DigitalOut snap(p14); //1:装填 0:発射
Arare 10:a05e9a8980db 13 DigitalOut s_grab(p13); //0:解放 1:把持
283 14:3858894d1a6b 14 DigitalOut fuck(p17); //0:引抜 1:突出
Arare 11:5a06fd933e55 15 DigitalOut grab2(p18); //0:開く 1:閉じる
Arare 11:5a06fd933e55 16 DigitalOut snap2(p15); //1:装填 0:発射
Arare 11:5a06fd933e55 17 DigitalOut s_grab2(p16); //0:解放 1:把持
Arare 10:a05e9a8980db 18 DigitalOut ledw(p20); //射出LED
Arare 10:a05e9a8980db 19
Arare 2:213c12298d45 20 DigitalOut led1(LED1);
Arare 2:213c12298d45 21 DigitalOut led2(LED2);
Arare 2:213c12298d45 22 DigitalOut led3(LED3);
Arare 2:213c12298d45 23 DigitalOut led4(LED4);
283 14:3858894d1a6b 24 DigitalOut LEDN(p10);
Arare 10:a05e9a8980db 25 DigitalOut out(p21);
Arare 10:a05e9a8980db 26
Arare 6:ca4fab2957e4 27 std_msgs::String action;
Arare 6:ca4fab2957e4 28 std_msgs::String fin_or_not;
Arare 6:ca4fab2957e4 29 std_msgs::String get_or_not;
Arare 10:a05e9a8980db 30
Arare 10:a05e9a8980db 31 int count=0; //何回actionしたんですか?
Arare 10:a05e9a8980db 32 int have_cock=0; //コック持ってますか？
Arare 10:a05e9a8980db 33 int have_action=0; //装填実行
Arare 6:ca4fab2957e4 34 int ready_action=0; //装填待機
Arare 6:ca4fab2957e4 35 int r,g,b,a;
Arare 6:ca4fab2957e4 36 int val;
Arare 6:ca4fab2957e4 37 int red=0;
Arare 10:a05e9a8980db 38 int val2;
Arare 10:a05e9a8980db 39 int red2=0;
Arare 10:a05e9a8980db 40 int RED_MIN=0;
Arare 10:a05e9a8980db 41 int RED_MIN2=0;
Arare 6:ca4fab2957e4 42 int32_t act=101;
Arare 11:5a06fd933e55 43 int ball_judgement=0;
Arare 11:5a06fd933e55 44 int r_c=0;
Arare 11:5a06fd933e55 45 int r_c2=0;
Arare 11:5a06fd933e55 46 int finding=0;
Arare 10:a05e9a8980db 47
Arare 10:a05e9a8980db 48 Ticker MC;
Arare 6:ca4fab2957e4 49 std_msgs::Int32 pub_act;
Arare 10:a05e9a8980db 50 std_msgs::Int32 pub_b;
Arare 11:5a06fd933e55 51 std_msgs::Int32 pub_c1;
Arare 11:5a06fd933e55 52 std_msgs::Int32 pub_c2;
Arare 11:5a06fd933e55 53 std_msgs::Int32 pub_fin;
Arare 6:ca4fab2957e4 54 ros::NodeHandle nh;
Arare 6:ca4fab2957e4 55 ros::Publisher pub_action("act_pose", &pub_act);
Arare 10:a05e9a8980db 56 ros::Publisher pub_ball("act_ball", &pub_b);
Arare 11:5a06fd933e55 57 ros::Publisher pub_color1("act_color1", &pub_c1);
Arare 11:5a06fd933e55 58 ros::Publisher pub_color2("act_color2", &pub_c2);
Arare 11:5a06fd933e55 59 ros::Publisher pub_finc("shoot_fin", &pub_fin);
Arare 10:a05e9a8980db 60
Arare 6:ca4fab2957e4 61 void messageCallback(const std_msgs::Int32 &msg)
agari 3:2184f6f5c8e3 62 {
Arare 6:ca4fab2957e4 63 act=msg.data;
Arare 2:213c12298d45 64 }
Arare 11:5a06fd933e55 65
Arare 11:5a06fd933e55 66 void messageCallback2(const std_msgs::Int32 &msg)
Arare 11:5a06fd933e55 67 {
Arare 11:5a06fd933e55 68 ball_judgement=msg.data;
Arare 11:5a06fd933e55 69 }
Arare 11:5a06fd933e55 70
Arare 11:5a06fd933e55 71 void messageCallback3(const std_msgs::Int32 &msg)
Arare 11:5a06fd933e55 72 {
Arare 11:5a06fd933e55 73 finding=msg.data;
Arare 11:5a06fd933e55 74 }
Arare 11:5a06fd933e55 75
Arare 6:ca4fab2957e4 76 void MCL()
Arare 6:ca4fab2957e4 77 {
Arare 10:a05e9a8980db 78 pub_b.data=ball_judgement;
Arare 11:5a06fd933e55 79 pub_c1.data=RED_MIN2;
Arare 11:5a06fd933e55 80 pub_c2.data=r_c2;
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 6:ca4fab2957e4 92
Arare 10:a05e9a8980db 93 /////射出サイクル/////
Arare 10:a05e9a8980db 94 void throw_cock(float waittime)
Arare 10:a05e9a8980db 95 {
Arare 10:a05e9a8980db 96 if (act==2) {
Arare 10:a05e9a8980db 97 led2=1;
283 14:3858894d1a6b 98 fuck=1;
Arare 11:5a06fd933e55 99 s_grab=0;
Arare 11:5a06fd933e55 100 wait(2);
Arare 11:5a06fd933e55 101 ledw=1;
Arare 11:5a06fd933e55 102 wait(1);
Arare 11:5a06fd933e55 103 snap=0;
Arare 11:5a06fd933e55 104 wait(waittime);
Arare 11:5a06fd933e55 105 grab=0;
Arare 11:5a06fd933e55 106 wait(1);
Arare 11:5a06fd933e55 107 ledw=0;
Arare 10:a05e9a8980db 108 }
Arare 10:a05e9a8980db 109 else if (act==4) {
Arare 10:a05e9a8980db 110 led3=1;
283 14:3858894d1a6b 111 fuck=1;
Arare 11:5a06fd933e55 112 s_grab2=0;
Arare 11:5a06fd933e55 113 wait(2);
Arare 11:5a06fd933e55 114 ledw=1;
Arare 11:5a06fd933e55 115 wait(1);
Arare 11:5a06fd933e55 116 snap2=0;
Arare 11:5a06fd933e55 117 wait(waittime);
Arare 11:5a06fd933e55 118 grab2=0;
Arare 11:5a06fd933e55 119 wait(1);
Arare 11:5a06fd933e55 120 ledw=0;
Arare 10:a05e9a8980db 121 }
Arare 10:a05e9a8980db 122 else if (act==6) {
Arare 10:a05e9a8980db 123 led2=1;
Arare 10:a05e9a8980db 124 led3=1;
283 14:3858894d1a6b 125 fuck=1;
Arare 11:5a06fd933e55 126 s_grab2=0;
Arare 11:5a06fd933e55 127 wait(2);
Arare 11:5a06fd933e55 128 ledw=1;
Arare 11:5a06fd933e55 129 wait(1);
Arare 11:5a06fd933e55 130 snap2=0;
Arare 11:5a06fd933e55 131 wait(waittime);
Arare 11:5a06fd933e55 132 grab2=0;
Arare 11:5a06fd933e55 133 wait(1);
Arare 11:5a06fd933e55 134 ledw=0;
Arare 10:a05e9a8980db 135 }
Arare 10:a05e9a8980db 136 else if (act==7) {
Arare 10:a05e9a8980db 137 led1=1;
Arare 10:a05e9a8980db 138 led2=1;
Arare 10:a05e9a8980db 139 led3=1;
283 14:3858894d1a6b 140 fuck=1;
Arare 11:5a06fd933e55 141 s_grab=0;
Arare 11:5a06fd933e55 142 wait(2);
Arare 11:5a06fd933e55 143 ledw=1;
Arare 11:5a06fd933e55 144 wait(1);
Arare 11:5a06fd933e55 145 snap=0;
283 14:3858894d1a6b 146 wait(waittime);
283 14:3858894d1a6b 147 grab=0;
283 14:3858894d1a6b 148 wait(1);
283 14:3858894d1a6b 149 ledw=0;
283 14:3858894d1a6b 150
283 14:3858894d1a6b 151 fuck=1;
283 14:3858894d1a6b 152 s_grab2=0;
283 14:3858894d1a6b 153 wait(2);
283 14:3858894d1a6b 154 ledw=1;
283 14:3858894d1a6b 155 wait(1);
Arare 11:5a06fd933e55 156 snap2=0;
Arare 11:5a06fd933e55 157 wait(waittime);
Arare 11:5a06fd933e55 158 grab2=0;
Arare 11:5a06fd933e55 159 wait(1);
Arare 11:5a06fd933e55 160 ledw=0;
Arare 10:a05e9a8980db 161 }
Arare 10:a05e9a8980db 162
Arare 10:a05e9a8980db 163 if (act==2) {
Arare 10:a05e9a8980db 164 led2=0;
Arare 10:a05e9a8980db 165 }
Arare 10:a05e9a8980db 166 else if (act==4) {
Arare 10:a05e9a8980db 167 led3=0;
Arare 10:a05e9a8980db 168 }
Arare 10:a05e9a8980db 169 else if (act==6) {
Arare 10:a05e9a8980db 170 led2=0;
Arare 10:a05e9a8980db 171 led3=0;
Arare 10:a05e9a8980db 172 }
Arare 10:a05e9a8980db 173 else if (act==7) {
Arare 10:a05e9a8980db 174 led1=0;
Arare 10:a05e9a8980db 175 led2=0;
Arare 10:a05e9a8980db 176 led3=0;
Arare 10:a05e9a8980db 177 }
Arare 10:a05e9a8980db 178 }
Arare 10:a05e9a8980db 179
Arare 10:a05e9a8980db 180
agari 1:5dd2f53a286f 181 int main(int argc, char **argv)
agari 1:5dd2f53a286f 182 {
Arare 10:a05e9a8980db 183 out=1;
Arare 10:a05e9a8980db 184
Arare 2:213c12298d45 185 nh.initNode();
Arare 2:213c12298d45 186 nh.subscribe(sub);
Arare 11:5a06fd933e55 187 nh.subscribe(sub_TZ);
Arare 11:5a06fd933e55 188 nh.subscribe(sub_find);
Arare 6:ca4fab2957e4 189 nh.advertise(pub_action);
Arare 10:a05e9a8980db 190 nh.advertise(pub_ball);
Arare 11:5a06fd933e55 191 nh.advertise(pub_color1);
Arare 11:5a06fd933e55 192 nh.advertise(pub_color2);
Arare 11:5a06fd933e55 193 nh.advertise(pub_finc);
Arare 6:ca4fab2957e4 194 MC.attach(&MCL,0.01);
Arare 6:ca4fab2957e4 195 pub_act.data=0;
Arare 11:5a06fd933e55 196 pub_fin.data=0;
Arare 10:a05e9a8980db 197 pub_b.data=0;
Arare 9:1dcd40da31ec 198 int first_up=0;
Arare 6:ca4fab2957e4 199 char cmd[2];
Arare 6:ca4fab2957e4 200 char cell[1]= {0x03};
Arare 6:ca4fab2957e4 201 char data[8]= {0,0,0,0,0,0,0,0};
Arare 10:a05e9a8980db 202 char data2[8]= {0,0,0,0,0,0,0,0};
Arare 6:ca4fab2957e4 203 cmd[0] = 0x00;
Arare 6:ca4fab2957e4 204 cmd[1] = 0x89;
Arare 6:ca4fab2957e4 205 i2c.frequency(115200);
Arare 10:a05e9a8980db 206 i2c_2.frequency(115200);
Arare 6:ca4fab2957e4 207 val = i2c.write(84, cmd, 2);
Arare 10:a05e9a8980db 208 val2 = i2c_2.write(84, cmd, 2);
Arare 6:ca4fab2957e4 209 cmd[0] = 0x0;
Arare 6:ca4fab2957e4 210 cmd[1] = 0x09;
Arare 6:ca4fab2957e4 211 val = i2c.write(84, cmd, 2);
Arare 10:a05e9a8980db 212 val2 = i2c_2.write(84, cmd, 2);
Arare 10:a05e9a8980db 213
agari 3:2184f6f5c8e3 214 while(1) {
Arare 6:ca4fab2957e4 215 val = i2c.write(84, cell, 1);
Arare 10:a05e9a8980db 216 val2 = i2c_2.write(84, cell, 1);
Arare 6:ca4fab2957e4 217 val = i2c.read(84, data, 8);
Arare 10:a05e9a8980db 218 val2 = i2c_2.read(84, data2, 8);
Arare 6:ca4fab2957e4 219 red = data[0]<<8 | data[1];
Arare 10:a05e9a8980db 220 red2 = data2[0]<<8 | data2[1];
Arare 10:a05e9a8980db 221 /* if(red<RED_MIN) {
Arare 9:1dcd40da31ec 222 led1=1;
Arare 9:1dcd40da31ec 223 led2=0;
Arare 10:a05e9a8980db 224 } else {
Arare 9:1dcd40da31ec 225 led1=0;
Arare 9:1dcd40da31ec 226 led2=1;
Arare 10:a05e9a8980db 227 }*/
Arare 10:a05e9a8980db 228
Arare 10:a05e9a8980db 229 //pc.printf("%d",red); ///カラーセンサー閾値
Arare 10:a05e9a8980db 230
Arare 10:a05e9a8980db 231 ///装填///
Arare 10:a05e9a8980db 232 if(((act==1)||(act==3)||(act==5)||(act==8))&&(ready_action==0)) { //TZ3
Arare 12:6832bc92272f 233 /*if(act==1) {
Arare 11:5a06fd933e55 234 led1=1;
Arare 10:a05e9a8980db 235 }
Arare 11:5a06fd933e55 236 else if(act==3) {
Arare 11:5a06fd933e55 237 led1=1;
Arare 11:5a06fd933e55 238 led2=1;
Arare 11:5a06fd933e55 239 }
Arare 11:5a06fd933e55 240 else if(act==5) {
Arare 11:5a06fd933e55 241 led1=1;
Arare 11:5a06fd933e55 242 led3=1;
Arare 10:a05e9a8980db 243 }
Arare 11:5a06fd933e55 244 else if(act==8) {
Arare 11:5a06fd933e55 245 led4=1;
Arare 11:5a06fd933e55 246 }*/
Arare 11:5a06fd933e55 247 snap=1;
Arare 11:5a06fd933e55 248 snap2=1;
Arare 12:6832bc92272f 249 grab=0;
Arare 12:6832bc92272f 250 grab2=0;
283 14:3858894d1a6b 251 fuck=0;
Arare 12:6832bc92272f 252 while(finding!=1){
Arare 12:6832bc92272f 253 led1=0;
Arare 12:6832bc92272f 254 led2=0;
Arare 12:6832bc92272f 255 led3=0;
Arare 12:6832bc92272f 256 led4=0;
283 14:3858894d1a6b 257 LEDN=1;
Arare 12:6832bc92272f 258 }
Arare 11:5a06fd933e55 259 if(ball_judgement==1){
Arare 12:6832bc92272f 260 led1=0;
Arare 12:6832bc92272f 261 led2=1;
Arare 12:6832bc92272f 262 led3=0;
Arare 12:6832bc92272f 263 led4=0;
Arare 13:84f136900461 264 wait(2);//桃井escape
Arare 12:6832bc92272f 265 grab=1;
283 14:3858894d1a6b 266 wait(0.5);
Arare 12:6832bc92272f 267 s_grab=1;
283 14:3858894d1a6b 268 wait(0.5);
283 14:3858894d1a6b 269 fuck=0;
Arare 12:6832bc92272f 270 }
Arare 11:5a06fd933e55 271 else if(ball_judgement==2){
Arare 12:6832bc92272f 272 led1=0;
Arare 12:6832bc92272f 273 led2=0;
Arare 12:6832bc92272f 274 led3=1;
Arare 12:6832bc92272f 275 led4=0;
283 14:3858894d1a6b 276
Arare 13:84f136900461 277 wait(2);//桃井escape
Arare 12:6832bc92272f 278 grab2=1;
283 14:3858894d1a6b 279 wait(0.5);
Arare 12:6832bc92272f 280 s_grab2=1;
283 14:3858894d1a6b 281 wait(0.5);
283 14:3858894d1a6b 282 fuck=0;
Arare 11:5a06fd933e55 283 }
Arare 15:c27950efe96f 284 else if(ball_judgement>=3){
Arare 12:6832bc92272f 285 led1=0;
Arare 12:6832bc92272f 286 led2=0;
Arare 12:6832bc92272f 287 led3=0;
Arare 12:6832bc92272f 288 led4=1;
Arare 13:84f136900461 289 wait(2);//桃井escape
Arare 12:6832bc92272f 290 grab=1;
Arare 12:6832bc92272f 291 grab2=1;
283 14:3858894d1a6b 292 wait(0.5);
Arare 12:6832bc92272f 293 s_grab=1;
Arare 12:6832bc92272f 294 s_grab2=1;
283 14:3858894d1a6b 295 wait(0.5);
283 14:3858894d1a6b 296 fuck=0;
Arare 11:5a06fd933e55 297 }
Arare 12:6832bc92272f 298 else{
Arare 12:6832bc92272f 299 led1=1;
Arare 12:6832bc92272f 300 led2=0;
Arare 12:6832bc92272f 301 led3=0;
Arare 12:6832bc92272f 302 led4=0;
Arare 12:6832bc92272f 303 }
Arare 11:5a06fd933e55 304 if(act==1) {
Arare 11:5a06fd933e55 305 led1=0;
Arare 11:5a06fd933e55 306 }
Arare 11:5a06fd933e55 307 else if(act==3) {
Arare 11:5a06fd933e55 308 led1=0;
Arare 11:5a06fd933e55 309 led2=0;
Arare 11:5a06fd933e55 310 }
Arare 11:5a06fd933e55 311 else if(act==5) {
Arare 11:5a06fd933e55 312 led1=0;
Arare 11:5a06fd933e55 313 led3=0;
Arare 11:5a06fd933e55 314 }
Arare 11:5a06fd933e55 315 else if(act==8) {
Arare 11:5a06fd933e55 316 led4=0;
Arare 11:5a06fd933e55 317 }
283 14:3858894d1a6b 318 LEDN=0;
Arare 11:5a06fd933e55 319 ready_action=1;
Arare 11:5a06fd933e55 320 count=0;
Arare 11:5a06fd933e55 321 pub_act.data=act;
Arare 11:5a06fd933e55 322 pub_action.publish(&pub_act);
Arare 11:5a06fd933e55 323 r_c=0;
Arare 6:ca4fab2957e4 324 }
Arare 10:a05e9a8980db 325
Arare 10:a05e9a8980db 326 ///射出///
Arare 10:a05e9a8980db 327 if((act==2)&&(count==0)) {
Arare 12:6832bc92272f 328 int f_i=0;
Arare 11:5a06fd933e55 329 throw_cock(0.218); //TZ1
Arare 10:a05e9a8980db 330
Arare 10:a05e9a8980db 331 ready_action=0;
Arare 10:a05e9a8980db 332 count=1;
Arare 10:a05e9a8980db 333 first_up=0;
Arare 10:a05e9a8980db 334 pub_act.data=act;
Arare 10:a05e9a8980db 335 pub_action.publish(&pub_act);
Arare 12:6832bc92272f 336 for(f_i=0;f_i<20;f_i++){
Arare 12:6832bc92272f 337 pub_fin.data=1;
Arare 12:6832bc92272f 338 pub_finc.publish(&pub_fin);
Arare 12:6832bc92272f 339 wait(0.01);
Arare 12:6832bc92272f 340 }
Arare 10:a05e9a8980db 341 }
Arare 10:a05e9a8980db 342
Arare 10:a05e9a8980db 343 if(((act==4)||(act==6))&&(count==0)) {
Arare 12:6832bc92272f 344 int f_i=0;
Arare 11:5a06fd933e55 345 throw_cock(0.218); //TZ2
Arare 6:ca4fab2957e4 346
Arare 10:a05e9a8980db 347 ready_action=0;
Arare 10:a05e9a8980db 348 count=1;
Arare 10:a05e9a8980db 349 first_up=0;
Arare 10:a05e9a8980db 350 pub_act.data=act;
Arare 10:a05e9a8980db 351 pub_action.publish(&pub_act);
Arare 12:6832bc92272f 352 for(f_i=0;f_i<20;f_i++){
Arare 12:6832bc92272f 353 pub_fin.data=1;
Arare 12:6832bc92272f 354 pub_finc.publish(&pub_fin);
Arare 12:6832bc92272f 355 wait(0.01);
Arare 12:6832bc92272f 356 }
Arare 10:a05e9a8980db 357 }
Arare 6:ca4fab2957e4 358
Arare 10:a05e9a8980db 359 if((act==7)&&(count==0)) {
Arare 12:6832bc92272f 360 int f_i=0;
Arare 10:a05e9a8980db 361 throw_cock(0.172); //TZ3
Arare 10:a05e9a8980db 362
Arare 10:a05e9a8980db 363 ready_action=0;
Arare 10:a05e9a8980db 364 count=1;
Arare 10:a05e9a8980db 365 first_up=0;
Arare 10:a05e9a8980db 366 pub_act.data=act;
Arare 10:a05e9a8980db 367 pub_action.publish(&pub_act);
Arare 12:6832bc92272f 368 for(f_i=0;f_i<20;f_i++){
Arare 12:6832bc92272f 369 pub_fin.data=1;
Arare 12:6832bc92272f 370 pub_finc.publish(&pub_fin);
Arare 12:6832bc92272f 371 wait(0.01);
Arare 12:6832bc92272f 372 }
Arare 10:a05e9a8980db 373 }
Arare 10:a05e9a8980db 374
Arare 10:a05e9a8980db 375 //一連の動作が終了したかを判断する。
Arare 10:a05e9a8980db 376 if(count>0) {
Arare 10:a05e9a8980db 377 have_cock=0;
Arare 10:a05e9a8980db 378 have_action=0;
Arare 10:a05e9a8980db 379 }
agari 3:2184f6f5c8e3 380 }
Arare 10:a05e9a8980db 381 }