ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@10:a05e9a8980db, 2018-03-05 (annotated)

Committer:
Arare
Date:
Mon Mar 05 01:37:19 2018 +0000
Revision:
10:a05e9a8980db
Parent:
9:1dcd40da31ec
Child:
11:5a06fd933e55
??

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 9:1dcd40da31ec 5 #define R_C_MAX 50
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:把持
Arare 10:a05e9a8980db 14 DigitalOut ledw(p20); //射出LED
Arare 10:a05e9a8980db 15
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 10:a05e9a8980db 20
Arare 10:a05e9a8980db 21 DigitalOut out(p21);
Arare 10:a05e9a8980db 22
Arare 6:ca4fab2957e4 23 std_msgs::String action;
Arare 6:ca4fab2957e4 24 std_msgs::String fin_or_not;
Arare 6:ca4fab2957e4 25 std_msgs::String get_or_not;
Arare 10:a05e9a8980db 26
Arare 10:a05e9a8980db 27 int count=0; //何回actionしたんですか?
Arare 10:a05e9a8980db 28 int have_cock=0; //コック持ってますか？
Arare 10:a05e9a8980db 29 int have_action=0; //装填実行
Arare 6:ca4fab2957e4 30 int ready_action=0; //装填待機
Arare 6:ca4fab2957e4 31 int r,g,b,a;
Arare 6:ca4fab2957e4 32 int val;
Arare 6:ca4fab2957e4 33 int red=0;
Arare 10:a05e9a8980db 34 int val2;
Arare 10:a05e9a8980db 35 int red2=0;
Arare 10:a05e9a8980db 36 int RED_MIN=0;
Arare 10:a05e9a8980db 37 int RED_MIN2=0;
Arare 6:ca4fab2957e4 38 int32_t act=101;
Arare 10:a05e9a8980db 39 int ball_judgement=0;
Arare 10:a05e9a8980db 40
Arare 10:a05e9a8980db 41 Ticker MC;
Arare 6:ca4fab2957e4 42 std_msgs::Int32 pub_act;
Arare 10:a05e9a8980db 43 std_msgs::Int32 pub_b;
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 10:a05e9a8980db 47
Arare 6:ca4fab2957e4 48 void messageCallback(const std_msgs::Int32 &msg)
agari 3:2184f6f5c8e3 49 {
Arare 6:ca4fab2957e4 50 act=msg.data;
Arare 2:213c12298d45 51 }
Arare 6:ca4fab2957e4 52 void MCL()
Arare 6:ca4fab2957e4 53 {
Arare 10:a05e9a8980db 54 pub_b.data=ball_judgement;
Arare 6:ca4fab2957e4 55 pub_action.publish(&pub_act);
Arare 10:a05e9a8980db 56 pub_ball.publish(&pub_b);
Arare 6:ca4fab2957e4 57 nh.spinOnce();
Arare 6:ca4fab2957e4 58 }
Arare 6:ca4fab2957e4 59 //定点に到着した時にTz1:a,Tz2:b,Tz3:cの文字を足回りから送る。topicはshoot_action
Arare 6:ca4fab2957e4 60 //装填用の信号はdとしておく。後で変更して、どうぞ。
Arare 6:ca4fab2957e4 61 ros::Subscriber<std_msgs::Int32> sub("shoot_action",&messageCallback);
Arare 6:ca4fab2957e4 62
Arare 10:a05e9a8980db 63 /////射出サイクル/////
Arare 10:a05e9a8980db 64 void throw_cock(float waittime)
Arare 10:a05e9a8980db 65 {
Arare 10:a05e9a8980db 66 if (act==2) {
Arare 10:a05e9a8980db 67 led2=1;
Arare 10:a05e9a8980db 68 }
Arare 10:a05e9a8980db 69 else if (act==4) {
Arare 10:a05e9a8980db 70 led3=1;
Arare 10:a05e9a8980db 71 }
Arare 10:a05e9a8980db 72 else if (act==6) {
Arare 10:a05e9a8980db 73 led2=1;
Arare 10:a05e9a8980db 74 led3=1;
Arare 10:a05e9a8980db 75 }
Arare 10:a05e9a8980db 76 else if (act==7) {
Arare 10:a05e9a8980db 77 led1=1;
Arare 10:a05e9a8980db 78 led2=1;
Arare 10:a05e9a8980db 79 led3=1;
Arare 10:a05e9a8980db 80 }
Arare 10:a05e9a8980db 81
Arare 10:a05e9a8980db 82 s_grab=0;
Arare 10:a05e9a8980db 83 wait(2);
Arare 10:a05e9a8980db 84 ledw=1;
Arare 10:a05e9a8980db 85 wait(1);
Arare 10:a05e9a8980db 86 snap=0;
Arare 10:a05e9a8980db 87 wait(waittime);
Arare 10:a05e9a8980db 88 grab=0;
Arare 10:a05e9a8980db 89 wait(1.2);///kinect用のwait
Arare 10:a05e9a8980db 90 ledw=0;
Arare 10:a05e9a8980db 91
Arare 10:a05e9a8980db 92 if (act==2) {
Arare 10:a05e9a8980db 93 led2=0;
Arare 10:a05e9a8980db 94 }
Arare 10:a05e9a8980db 95 else if (act==4) {
Arare 10:a05e9a8980db 96 led3=0;
Arare 10:a05e9a8980db 97 }
Arare 10:a05e9a8980db 98 else if (act==6) {
Arare 10:a05e9a8980db 99 led2=0;
Arare 10:a05e9a8980db 100 led3=0;
Arare 10:a05e9a8980db 101 }
Arare 10:a05e9a8980db 102 else if (act==7) {
Arare 10:a05e9a8980db 103 led1=0;
Arare 10:a05e9a8980db 104 led2=0;
Arare 10:a05e9a8980db 105 led3=0;
Arare 10:a05e9a8980db 106 }
Arare 10:a05e9a8980db 107 }
Arare 10:a05e9a8980db 108
Arare 10:a05e9a8980db 109
agari 1:5dd2f53a286f 110 int main(int argc, char **argv)
agari 1:5dd2f53a286f 111 {
Arare 10:a05e9a8980db 112 out=1;
Arare 10:a05e9a8980db 113
Arare 2:213c12298d45 114 nh.initNode();
Arare 2:213c12298d45 115 nh.subscribe(sub);
Arare 6:ca4fab2957e4 116 nh.advertise(pub_action);
Arare 10:a05e9a8980db 117 nh.advertise(pub_ball);
Arare 6:ca4fab2957e4 118 MC.attach(&MCL,0.01);
Arare 6:ca4fab2957e4 119 pub_act.data=0;
Arare 10:a05e9a8980db 120 pub_b.data=0;
Arare 8:5d8b23cec6d1 121 int r_c=0;
Arare 10:a05e9a8980db 122 int r_c2=0;
Arare 9:1dcd40da31ec 123 int first_up=0;
Arare 6:ca4fab2957e4 124 char cmd[2];
Arare 6:ca4fab2957e4 125 char cell[1]= {0x03};
Arare 6:ca4fab2957e4 126 char data[8]= {0,0,0,0,0,0,0,0};
Arare 10:a05e9a8980db 127 char data2[8]= {0,0,0,0,0,0,0,0};
Arare 6:ca4fab2957e4 128 cmd[0] = 0x00;
Arare 6:ca4fab2957e4 129 cmd[1] = 0x89;
Arare 6:ca4fab2957e4 130 i2c.frequency(115200);
Arare 10:a05e9a8980db 131 i2c_2.frequency(115200);
Arare 6:ca4fab2957e4 132 val = i2c.write(84, cmd, 2);
Arare 10:a05e9a8980db 133 val2 = i2c_2.write(84, cmd, 2);
Arare 6:ca4fab2957e4 134 cmd[0] = 0x0;
Arare 6:ca4fab2957e4 135 cmd[1] = 0x09;
Arare 6:ca4fab2957e4 136 val = i2c.write(84, cmd, 2);
Arare 10:a05e9a8980db 137 val2 = i2c_2.write(84, cmd, 2);
Arare 10:a05e9a8980db 138
agari 3:2184f6f5c8e3 139 while(1) {
Arare 6:ca4fab2957e4 140 val = i2c.write(84, cell, 1);
Arare 10:a05e9a8980db 141 val2 = i2c_2.write(84, cell, 1);
Arare 6:ca4fab2957e4 142 val = i2c.read(84, data, 8);
Arare 10:a05e9a8980db 143 val2 = i2c_2.read(84, data2, 8);
Arare 6:ca4fab2957e4 144 red = data[0]<<8 | data[1];
Arare 10:a05e9a8980db 145 red2 = data2[0]<<8 | data2[1];
Arare 10:a05e9a8980db 146 /* if(red<RED_MIN) {
Arare 9:1dcd40da31ec 147 led1=1;
Arare 9:1dcd40da31ec 148 led2=0;
Arare 10:a05e9a8980db 149 } else {
Arare 9:1dcd40da31ec 150 led1=0;
Arare 9:1dcd40da31ec 151 led2=1;
Arare 10:a05e9a8980db 152 }*/
Arare 10:a05e9a8980db 153
Arare 10:a05e9a8980db 154 //pc.printf("%d",red); ///カラーセンサー閾値
Arare 10:a05e9a8980db 155
Arare 10:a05e9a8980db 156 ///装填///
Arare 10:a05e9a8980db 157 if(((act==1)||(act==3)||(act==5)||(act==8))&&(ready_action==0)) { //TZ3
Arare 10:a05e9a8980db 158 if(first_up==0&&red!=0) {
Arare 10:a05e9a8980db 159 RED_MIN=red-150;
Arare 10:a05e9a8980db 160 RED_MIN2=red2-150;
Arare 10:a05e9a8980db 161 first_up++;
Arare 6:ca4fab2957e4 162 }
Arare 10:a05e9a8980db 163 if(first_up>0) {
Arare 10:a05e9a8980db 164 snap=1;
Arare 10:a05e9a8980db 165 grab=0;
Arare 10:a05e9a8980db 166 if(red<RED_MIN) {
Arare 10:a05e9a8980db 167 r_c++;
Arare 10:a05e9a8980db 168 }
Arare 10:a05e9a8980db 169 if(red2<RED_MIN2) {
Arare 10:a05e9a8980db 170 r_c2++;
Arare 10:a05e9a8980db 171 }
Arare 10:a05e9a8980db 172 else{
Arare 10:a05e9a8980db 173 r_c=0;
Arare 10:a05e9a8980db 174 r_c2=0;
Arare 10:a05e9a8980db 175 }
Arare 10:a05e9a8980db 176 if(r_c>R_C_MAX||r_c2>R_C_MAX) {
Arare 10:a05e9a8980db 177 if(ball_judgement==0){
Arare 10:a05e9a8980db 178 if(r_c2>R_C_MAX)ball_judgement=1;
Arare 10:a05e9a8980db 179 }
Arare 10:a05e9a8980db 180 else if(ball_judgement==1){
Arare 10:a05e9a8980db 181 if(r_c-r_c2>-R_C_MAX/2&&r_c-r_c2<R_C_MAX/2)ball_judgement=2;
Arare 10:a05e9a8980db 182 }
Arare 10:a05e9a8980db 183 if(act==1) {
Arare 10:a05e9a8980db 184 led1=1;
Arare 10:a05e9a8980db 185 }
Arare 10:a05e9a8980db 186 else if(act==3) {
Arare 10:a05e9a8980db 187 led1=1;
Arare 10:a05e9a8980db 188 led2=1;
Arare 10:a05e9a8980db 189 }
Arare 10:a05e9a8980db 190 else if(act==5) {
Arare 10:a05e9a8980db 191 led1=1;
Arare 10:a05e9a8980db 192 led3=1;
Arare 10:a05e9a8980db 193 }
Arare 10:a05e9a8980db 194 else if(act==8) {
Arare 10:a05e9a8980db 195 led4=1;
Arare 10:a05e9a8980db 196 }
Arare 10:a05e9a8980db 197
Arare 10:a05e9a8980db 198
Arare 10:a05e9a8980db 199 wait(1.5);
Arare 10:a05e9a8980db 200 grab=1;
Arare 10:a05e9a8980db 201 wait(1);
Arare 10:a05e9a8980db 202 s_grab=1;
Arare 10:a05e9a8980db 203 wait(2);
Arare 10:a05e9a8980db 204
Arare 10:a05e9a8980db 205 if(act==1) {
Arare 10:a05e9a8980db 206 led1=0;
Arare 10:a05e9a8980db 207 }
Arare 10:a05e9a8980db 208 else if(act==3) {
Arare 10:a05e9a8980db 209 led1=0;
Arare 10:a05e9a8980db 210 led2=0;
Arare 10:a05e9a8980db 211 }
Arare 10:a05e9a8980db 212 else if(act==5) {
Arare 10:a05e9a8980db 213 led1=0;
Arare 10:a05e9a8980db 214 led3=0;
Arare 10:a05e9a8980db 215 }
Arare 10:a05e9a8980db 216 else if(act==8) {
Arare 10:a05e9a8980db 217 led4=0;
Arare 10:a05e9a8980db 218 }
Arare 10:a05e9a8980db 219
Arare 10:a05e9a8980db 220
Arare 10:a05e9a8980db 221
Arare 10:a05e9a8980db 222
Arare 10:a05e9a8980db 223 ready_action=1;
Arare 10:a05e9a8980db 224 count=0;
Arare 10:a05e9a8980db 225 pub_act.data=act;
Arare 10:a05e9a8980db 226 pub_action.publish(&pub_act);
Arare 10:a05e9a8980db 227 r_c=0;
Arare 10:a05e9a8980db 228 }
Arare 6:ca4fab2957e4 229 }
agari 5:4f1f0294d6aa 230 }
Arare 10:a05e9a8980db 231
Arare 10:a05e9a8980db 232 ///射出///
Arare 10:a05e9a8980db 233 if((act==2)&&(count==0)) {
Arare 10:a05e9a8980db 234
Arare 10:a05e9a8980db 235 throw_cock(0.21); //TZ1
Arare 10:a05e9a8980db 236
Arare 10:a05e9a8980db 237 ready_action=0;
Arare 10:a05e9a8980db 238 count=1;
Arare 10:a05e9a8980db 239 first_up=0;
Arare 10:a05e9a8980db 240 pub_act.data=act;
Arare 10:a05e9a8980db 241 pub_action.publish(&pub_act);
Arare 10:a05e9a8980db 242 }
Arare 10:a05e9a8980db 243
Arare 10:a05e9a8980db 244 if(((act==4)||(act==6))&&(count==0)) {
Arare 10:a05e9a8980db 245
Arare 10:a05e9a8980db 246 throw_cock(0.205); //TZ2
Arare 6:ca4fab2957e4 247
Arare 10:a05e9a8980db 248 ready_action=0;
Arare 10:a05e9a8980db 249 count=1;
Arare 10:a05e9a8980db 250 first_up=0;
Arare 10:a05e9a8980db 251 pub_act.data=act;
Arare 10:a05e9a8980db 252 pub_action.publish(&pub_act);
Arare 10:a05e9a8980db 253 }
Arare 6:ca4fab2957e4 254
Arare 10:a05e9a8980db 255 if((act==7)&&(count==0)) {
Arare 10:a05e9a8980db 256
Arare 10:a05e9a8980db 257 throw_cock(0.172); //TZ3
Arare 10:a05e9a8980db 258
Arare 10:a05e9a8980db 259 ready_action=0;
Arare 10:a05e9a8980db 260 count=1;
Arare 10:a05e9a8980db 261 first_up=0;
Arare 10:a05e9a8980db 262 pub_act.data=act;
Arare 10:a05e9a8980db 263 pub_action.publish(&pub_act);
Arare 10:a05e9a8980db 264 }
Arare 10:a05e9a8980db 265
Arare 10:a05e9a8980db 266 //一連の動作が終了したかを判断する。
Arare 10:a05e9a8980db 267 if(count>0) {
Arare 10:a05e9a8980db 268 have_cock=0;
Arare 10:a05e9a8980db 269 have_action=0;
Arare 10:a05e9a8980db 270 }
agari 3:2184f6f5c8e3 271 }
Arare 10:a05e9a8980db 272 }