ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

main.cpp@9:1dcd40da31ec, 2017-12-02 (annotated)

Committer:
Arare
Date:
Sat Dec 02 04:14:02 2017 +0000
Revision:
9:1dcd40da31ec
Parent:
8:5d8b23cec6d1
Child:
10:a05e9a8980db
kosida

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 9:1dcd40da31ec 5 int RED_MIN=0;
Arare 9:1dcd40da31ec 6 #define R_C_MAX 50
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 9:1dcd40da31ec 65 int first_up=0;
Arare 6:ca4fab2957e4 66 char cmd[2];
Arare 6:ca4fab2957e4 67 char cell[1]= {0x03};
Arare 6:ca4fab2957e4 68 char data[8]= {0,0,0,0,0,0,0,0};
Arare 6:ca4fab2957e4 69 cmd[0] = 0x00;
Arare 6:ca4fab2957e4 70 cmd[1] = 0x89;
Arare 6:ca4fab2957e4 71 i2c.frequency(115200);
Arare 6:ca4fab2957e4 72 val = i2c.write(84, cmd, 2);
Arare 6:ca4fab2957e4 73 cmd[0] = 0x0;
Arare 6:ca4fab2957e4 74 cmd[1] = 0x09;
Arare 6:ca4fab2957e4 75 val = i2c.write(84, cmd, 2);
Arare 7:2827264ddf53 76 //servo.period_ms(20);
Arare 6:ca4fab2957e4 77 //int jk=0;
agari 3:2184f6f5c8e3 78 while(1) {
Arare 6:ca4fab2957e4 79 //wait_ms(50);//最小値ではない
Arare 6:ca4fab2957e4 80 val = i2c.write(84, cell, 1);
Arare 6:ca4fab2957e4 81 val = i2c.read(84, data, 8);
Arare 6:ca4fab2957e4 82 red = data[0]<<8 | data[1];
Arare 9:1dcd40da31ec 83 if(first_up==0&&red!=0){
Arare 9:1dcd40da31ec 84 RED_MIN=red-500;
Arare 9:1dcd40da31ec 85 first_up++;}
Arare 9:1dcd40da31ec 86 if(red<RED_MIN){
Arare 9:1dcd40da31ec 87 led1=1;
Arare 9:1dcd40da31ec 88 led2=0;
Arare 9:1dcd40da31ec 89 //led5=1;
Arare 9:1dcd40da31ec 90 //led6=1;
Arare 9:1dcd40da31ec 91 //led7=1;
Arare 9:1dcd40da31ec 92 }
Arare 9:1dcd40da31ec 93 else{
Arare 9:1dcd40da31ec 94 led1=0;
Arare 9:1dcd40da31ec 95 led2=1;
Arare 9:1dcd40da31ec 96 //led5=0;
Arare 9:1dcd40da31ec 97 //led6=0;
Arare 9:1dcd40da31ec 98 //led7=0;
Arare 9:1dcd40da31ec 99 }
Arare 6:ca4fab2957e4 100 //pc.printf("%d",red);
Arare 9:1dcd40da31ec 101 if(first_up>0){
Arare 6:ca4fab2957e4 102 if((act==100)&&(ready_action==0)) {
Arare 6:ca4fab2957e4 103 snap=1;
Arare 6:ca4fab2957e4 104 grab=0;
Arare 6:ca4fab2957e4 105 if(red<RED_MIN){
Arare 8:5d8b23cec6d1 106 r_c++;
Arare 8:5d8b23cec6d1 107 }
Arare 8:5d8b23cec6d1 108 else r_c=0;
Arare 8:5d8b23cec6d1 109 if(r_c>R_C_MAX){
Arare 6:ca4fab2957e4 110 grab=1;
Arare 6:ca4fab2957e4 111 s_grab=0;
Arare 6:ca4fab2957e4 112 ready_action=1;
Arare 6:ca4fab2957e4 113 count=0;
Arare 6:ca4fab2957e4 114 pub_act.data=act;
Arare 6:ca4fab2957e4 115 pub_action.publish(&pub_act);
Arare 8:5d8b23cec6d1 116 r_c=0;
Arare 6:ca4fab2957e4 117 }
Arare 6:ca4fab2957e4 118 }
Arare 6:ca4fab2957e4 119 if((act==2)&&(ready_action==0)) {
Arare 9:1dcd40da31ec 120 //led2=1;
Arare 6:ca4fab2957e4 121 snap=1;
Arare 6:ca4fab2957e4 122 grab=0;
Arare 6:ca4fab2957e4 123 if(red<RED_MIN){
Arare 8:5d8b23cec6d1 124 r_c++;
Arare 8:5d8b23cec6d1 125 }
Arare 8:5d8b23cec6d1 126 else r_c=0;
Arare 8:5d8b23cec6d1 127 if(r_c>R_C_MAX){
Arare 6:ca4fab2957e4 128 grab=1;
Arare 6:ca4fab2957e4 129 s_grab=0;
Arare 6:ca4fab2957e4 130 ready_action=1;
Arare 6:ca4fab2957e4 131 count=0;
Arare 6:ca4fab2957e4 132 pub_act.data=act;
Arare 6:ca4fab2957e4 133 pub_action.publish(&pub_act);
Arare 8:5d8b23cec6d1 134 r_c=0;
Arare 6:ca4fab2957e4 135 }
Arare 6:ca4fab2957e4 136 }
Arare 6:ca4fab2957e4 137 if((act==5)&&(ready_action==0)) {
Arare 6:ca4fab2957e4 138 snap=1;
Arare 6:ca4fab2957e4 139 grab=0;
Arare 6:ca4fab2957e4 140 if(red<RED_MIN){
Arare 8:5d8b23cec6d1 141 r_c++;
Arare 8:5d8b23cec6d1 142 }
Arare 8:5d8b23cec6d1 143 else r_c=0;
Arare 8:5d8b23cec6d1 144 if(r_c>R_C_MAX){
Arare 6:ca4fab2957e4 145 grab=1;
Arare 6:ca4fab2957e4 146 s_grab=0;
Arare 6:ca4fab2957e4 147 ready_action=1;
Arare 6:ca4fab2957e4 148 count=0;
Arare 6:ca4fab2957e4 149 pub_act.data=act;
Arare 6:ca4fab2957e4 150 pub_action.publish(&pub_act);
Arare 8:5d8b23cec6d1 151 r_c=0;
Arare 6:ca4fab2957e4 152 }
agari 5:4f1f0294d6aa 153 }
Arare 6:ca4fab2957e4 154 //get ready
Arare 6:ca4fab2957e4 155 //shashutu
Arare 7:2827264ddf53 156 if((act== 1)&&(count==0)) {
Arare 9:1dcd40da31ec 157 //led1=1;
Arare 9:1dcd40da31ec 158 //led6=1;
Arare 9:1dcd40da31ec 159 //led7=1;
Arare 6:ca4fab2957e4 160 s_grab=1;
Arare 9:1dcd40da31ec 161 wait(1);
Arare 9:1dcd40da31ec 162 led5=1;
Arare 9:1dcd40da31ec 163 wait(9);
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 9:1dcd40da31ec 171 led5=0;
Arare 9:1dcd40da31ec 172 //led6=0;
Arare 9:1dcd40da31ec 173 //led7=0;
Arare 6:ca4fab2957e4 174 }
Arare 6:ca4fab2957e4 175 if((act== 3)&&(count==0)) {
Arare 9:1dcd40da31ec 176 //led6=1;
Arare 9:1dcd40da31ec 177 //led7=1;
Arare 6:ca4fab2957e4 178 s_grab=1;
Arare 9:1dcd40da31ec 179 wait(1);
Arare 9:1dcd40da31ec 180 led5=1;
Arare 9:1dcd40da31ec 181 wait(9);
Arare 6:ca4fab2957e4 182 snap=0;
Arare 8:5d8b23cec6d1 183 wait(0.183); //TZ1
Arare 6:ca4fab2957e4 184 grab=0;
Arare 6:ca4fab2957e4 185 ready_action=0;
Arare 6:ca4fab2957e4 186 count=1;
Arare 6:ca4fab2957e4 187 pub_act.data=act;
Arare 6:ca4fab2957e4 188 pub_action.publish(&pub_act);
Arare 9:1dcd40da31ec 189 led5=0;
Arare 9:1dcd40da31ec 190 //led6=0;
Arare 9:1dcd40da31ec 191 //led7=0;
Arare 6:ca4fab2957e4 192 }
Arare 6:ca4fab2957e4 193
Arare 6:ca4fab2957e4 194 if((act==4)&&(count==0)) {
Arare 9:1dcd40da31ec 195 //led4=1;
Arare 9:1dcd40da31ec 196 //led6=1;
Arare 9:1dcd40da31ec 197 //led7=1;
Arare 6:ca4fab2957e4 198 s_grab=1;
Arare 9:1dcd40da31ec 199 wait(1);
Arare 9:1dcd40da31ec 200 led5=1;
Arare 9:1dcd40da31ec 201 wait(9);
Arare 6:ca4fab2957e4 202 snap=0;
Arare 8:5d8b23cec6d1 203 wait(0.182); //TZ2
Arare 6:ca4fab2957e4 204 grab=0;
Arare 6:ca4fab2957e4 205 ready_action=0;
Arare 6:ca4fab2957e4 206 count=1;
Arare 6:ca4fab2957e4 207 pub_act.data=act;
Arare 6:ca4fab2957e4 208 pub_action.publish(&pub_act);
Arare 9:1dcd40da31ec 209 led5=0;
Arare 9:1dcd40da31ec 210 //led6=0;
Arare 9:1dcd40da31ec 211 //led7=0;
Arare 6:ca4fab2957e4 212 }
Arare 6:ca4fab2957e4 213 if((act==6)&&(count==0)) {
Arare 6:ca4fab2957e4 214 s_grab=1;
Arare 9:1dcd40da31ec 215 wait(1);
Arare 9:1dcd40da31ec 216 led5=1;
Arare 9:1dcd40da31ec 217 wait(9);
Arare 6:ca4fab2957e4 218 snap=0;
Arare 8:5d8b23cec6d1 219 wait(0.182); //TZ2
Arare 6:ca4fab2957e4 220 grab=0;
Arare 6:ca4fab2957e4 221 ready_action=0;
Arare 6:ca4fab2957e4 222 count=1;
Arare 6:ca4fab2957e4 223 pub_act.data=act;
Arare 6:ca4fab2957e4 224 pub_action.publish(&pub_act);
Arare 9:1dcd40da31ec 225 led5=0;
Arare 6:ca4fab2957e4 226 }
Arare 6:ca4fab2957e4 227 if((act==7)&&(count==0)) {
Arare 6:ca4fab2957e4 228 s_grab=1;
Arare 9:1dcd40da31ec 229 wait(1);
Arare 9:1dcd40da31ec 230 led5=1;
Arare 9:1dcd40da31ec 231 wait(9);
Arare 6:ca4fab2957e4 232 snap=0;
Arare 8:5d8b23cec6d1 233 wait(0.152); //TZ3
Arare 6:ca4fab2957e4 234 grab=0;
Arare 6:ca4fab2957e4 235 ready_action=0;
Arare 6:ca4fab2957e4 236 count=1;
Arare 6:ca4fab2957e4 237 pub_act.data=act;
Arare 6:ca4fab2957e4 238 pub_action.publish(&pub_act);
Arare 9:1dcd40da31ec 239 led5=0;
Arare 6:ca4fab2957e4 240 }
Arare 6:ca4fab2957e4 241 //一連の動作が終了したかを判断する。
Arare 6:ca4fab2957e4 242 if(count>0) {
Arare 6:ca4fab2957e4 243 //count=0;
Arare 6:ca4fab2957e4 244 have_cock=0;
Arare 6:ca4fab2957e4 245 have_action=0;
Arare 6:ca4fab2957e4 246 //have_cock=0;
Arare 6:ca4fab2957e4 247 //ready_action=0;
Arare 6:ca4fab2957e4 248 }
Arare 6:ca4fab2957e4 249 //if(ready_action>0){
Arare 6:ca4fab2957e4 250 // count=0;
Arare 6:ca4fab2957e4 251 // }
Arare 6:ca4fab2957e4 252
Arare 6:ca4fab2957e4 253 //if(pub_act.data!=act)pub_act.data=0;
agari 3:2184f6f5c8e3 254 }
Arare 9:1dcd40da31ec 255 }
agari 0:f6fa58c56955 256 }