a
Dependencies: mbed KondoServoLibrary ros_lib_kinetic
main.cpp
00001 #include "mbed.h" 00002 #include "KondoServo.h" 00003 00004 #include <ros.h> 00005 #include <geometry_msgs/Point.h> 00006 #include <geometry_msgs/Pose.h> 00007 #include <std_msgs/String.h> 00008 00009 ros::NodeHandle nh; 00010 geometry_msgs::Point arm_num; 00011 00012 ros::Publisher pub("/arm_rt",&arm_num); 00013 00014 00015 #define RESOLUTION 2048//分解能 00016 00017 //#define NHK2020_TEST_MODE 00018 //#define NHK2020_MAIN_MODE 00019 #define NHK2020_MAIN_MODE2 00020 //#define NHK2020_MAIN_MODE3 00021 //CAN can1(p30,p29); 00022 00023 Ticker can_ticker; //can用ticker 00024 00025 //Ec4multi EC_backdrop(p15,p16,RESOLUTION); 00026 //SpeedControl backdrop(p22,p21,50,EC_backdrop); 00027 ///////////Serial pc(USBTX,USBRX); 00028 DigitalOut snatch(p8);//sponge 00029 DigitalOut pass1(p27); 00030 DigitalOut pass2(p28); 00031 DigitalOut tkeep(p11); 00032 DigitalOut kick(p12); 00033 KondoServo servo(p13, p14, 1, 115200); //ピン宣言 (TX,RX,不明,通信の周期?) 00034 00035 00036 00037 int ID=0; //ICSマネージャーを使って変更可能(このプログラムそのまま使って動かなかったらIDが違う説濃厚) 00038 //Ticker motor_tick; //角速度計算用ticker 00039 00040 char t2[1]= {0}; //動作番号(送信する値(char型)) 00041 int t2_r=0, T2=0; //動作番号(受け取った値、送信する値(int型)) 00042 00043 double turn=0; 00044 int k = 0; 00045 int kkk = 0; 00046 00047 double gb1, gb2, gb3; 00048 void messageCallback(const geometry_msgs::Quaternion &arm_get) 00049 { 00050 t2_r = (int)arm_get.x; 00051 gb1 = (double)arm_get.y; 00052 gb2 = (double)arm_get.z; 00053 gb3 = (double)arm_get.w; 00054 } 00055 00056 ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_arm", &messageCallback); 00057 00058 void tsukami() 00059 { 00060 servo.set_degree(ID, 80); //サーボを動かす(ID,動かしたい角度) 00061 } 00062 void put() 00063 { 00064 servo.set_degree(ID, 100); //サーボを動かす(ID,動かしたい角度) 00065 } 00066 void top() 00067 { 00068 servo.set_degree(ID, 90); //サーボを動かす(ID,動かしたい角度) 00069 } 00070 00071 00072 int q = 0; 00073 00074 void can_read() 00075 { 00076 00077 00078 //CANMessage msg; 00079 00080 arm_num.x = T2; 00081 arm_num.y = (float)kkk; 00082 arm_num.z = 12; 00083 00084 pub.publish(&arm_num); 00085 00086 if(t2_r > T2) { 00087 T2 = t2_r; 00088 } 00089 00090 00091 } 00092 00093 00094 int main() 00095 { 00096 //pc.printf("setting please"); 00097 00098 nh.getHardware()->setBaud(115200); 00099 nh.initNode(); 00100 nh.advertise(pub); 00101 nh.subscribe(sub); 00102 // snatch=0,pass1=0,pass2=0,ttsukami=0,kick=0; 00103 // servo.init(); //servo初期化? 決まり文句 00104 00105 #ifdef NHK2020_TEST_MODE 00106 while(1) { 00107 printf("T2 = %d\n\r",T2); 00108 //printf("%lf",EC_backdrop.getRad()); 00109 /* if(sel=='z') { 00110 pc.printf("z\r\n"); 00111 tsukami(); 00112 } else if(sel=='x') { 00113 pc.printf("x\r\n"); 00114 put(); 00115 } else if(sel=='c') { 00116 pc.printf("c\r\n"); 00117 top(); 00118 } 00119 if(sel=='q') { 00120 printf("\r\n"); 00121 if(denjiben==0)denjiben=1; 00122 else denjiben=0; 00123 } 00124 if(sel=='1') { 00125 snatch=0; 00126 printf("snatch_off\r\n"); 00127 } 00128 if(sel=='2') { 00129 snatch=1; 00130 printf("snatch_on\r\n"); 00131 } 00132 if(sel=='3') { 00133 pass1=0; 00134 printf("pass1_off\r\n"); 00135 } 00136 if(sel=='4') { 00137 pass1=1; 00138 printf("pass1_on\r\n"); 00139 } 00140 if(sel=='5') { 00141 pass2=0; 00142 printf("pass2_off\r\n"); 00143 } 00144 if(sel=='6') { 00145 pass2=1; 00146 printf("pass2_on\r\n"); 00147 }*/ 00148 00149 00150 } 00151 #endif 00152 #ifdef NHK2020_MAIN_MODE 00153 can_ticker.attach(&can_read,0.01); 00154 printf("wait_mode\r\n"); 00155 00156 00157 00158 //while(1){ 00159 // if()brake; 00160 // } 00161 //ボタン操作待ち(セミオートセット 00162 00163 00164 //snatch=1; 00165 pass1=1; 00166 wait(1); 00167 pass2=1; 00168 wait(5); 00169 while(1) { 00170 printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); 00171 nh.spinOnce(); 00172 wait(0.01); 00173 if(T2 == 1) { 00174 break; 00175 } 00176 } 00177 //T2 = 1; 00178 if(T2 == 1) { //スタート位置からボール前まで移動(アーム待機) 00179 //T2=1; 00180 wait(1); 00181 pass2=0;//動き出した瞬間に展開始めるやつ 00182 while(1) { 00183 printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); 00184 nh.spinOnce(); 00185 wait(0.01); 00186 if(T2 == 2) { 00187 break; 00188 } 00189 } 00190 } 00191 00192 if(T2 == 2) { //ボール掴んで投げる 00193 //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); 00194 tsukami(); 00195 wait(3); 00196 snatch=1; 00197 wait(1); 00198 put(); 00199 wait(3); 00200 snatch=0; 00201 wait(1); 00202 top(); 00203 wait(5); 00204 pass1=0; 00205 wait(5); 00206 //wait(5); 00207 00208 T2++; 00209 } 00210 if(T2 == 3) { //スタートゾーンに戻る(アーム待機) 00211 while(1) { 00212 printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); 00213 nh.spinOnce(); 00214 wait(0.01); 00215 if(T2 == 4) { 00216 break; 00217 } 00218 } 00219 } 00220 printf("end\n\r"); 00221 00222 #endif 00223 #ifdef NHK2020_MAIN_MODE2 00224 //can1.frequency(1000000); 00225 can_ticker.attach(&can_read,0.01); 00226 //printf("T2 = %d\n\r",T2); 00227 if(T2 == 0) { 00228 //T2=1; 00229 /* while(1) { 00230 char sel=pc.getc(); 00231 if(sel=='1') { 00232 snatch=0; 00233 printf("snatch_off\r\n"); 00234 } 00235 if(sel=='2') { 00236 snatch=1; 00237 printf("snatch_on\r\n"); 00238 } 00239 if(sel=='3') { 00240 pass1=0; 00241 printf("pass1_off\r\n"); 00242 } 00243 if(sel=='4') { 00244 pass1=1; 00245 printf("pass1_on\r\n"); 00246 } 00247 if(sel=='5') { 00248 pass2=0; 00249 printf("pass2_off\r\n"); 00250 } 00251 if(sel=='6') { 00252 pass2=1; 00253 printf("pass2_on\r\n"); 00254 } 00255 if(sel=='q') { 00256 printf("end_wait_mode\r\n"); 00257 T2=1; 00258 break; 00259 00260 }*/ 00261 00262 snatch=1; 00263 nh.spinOnce(); 00264 wait(5); 00265 //T2=1; 00266 while(1) { 00267 nh.spinOnce(); 00268 wait(0.01); 00269 printf("t2=1"); 00270 if(T2 == 1) { 00271 break; 00272 } 00273 } 00274 } 00275 if(T2 == 1) { //idou 00276 //T2=2; 00277 pass2=0; 00278 printf("t2=1"); 00279 while(1) { 00280 nh.spinOnce(); 00281 wait(0.01); 00282 printf("t2=1"); 00283 if(T2 == 2) { 00284 break; 00285 } 00286 } 00287 } 00288 00289 if(T2 == 2) { //idou 00290 //T2=3; 00291 00292 while(1) { 00293 kkk++; 00294 nh.spinOnce(); 00295 wait(0.01); 00296 printf("t2=2"); 00297 wait(5); 00298 T2 = 3; 00299 if(T2 == 3) { 00300 break; 00301 } 00302 00303 } 00304 } 00305 if(T2 == 3) { //idou 00306 //T2=4; 00307 while(1) { 00308 nh.spinOnce(); 00309 wait(0.01); 00310 printf("t2=3"); 00311 if(T2 == 4) { 00312 break; 00313 } 00314 } 00315 } 00316 if(T2 == 4) { //kikk 00317 //T2=5; 00318 wait(1); 00319 tkeep=1; 00320 wait(1); 00321 snatch=0; 00322 wait(1); 00323 kick=1; 00324 T2++; 00325 00326 while(1) { 00327 nh.spinOnce(); 00328 wait(0.01); 00329 printf("t2=4"); 00330 if(T2 == 5) { 00331 break; 00332 } 00333 } 00334 } 00335 00336 00337 if(T2 == 5) { //idou 00338 pass1=1; 00339 pass2=1; 00340 wait(2); 00341 pass2=0; 00342 while(1) { 00343 nh.spinOnce(); 00344 wait(0.01); 00345 printf("t2=5"); 00346 if(T2 == 6) { 00347 break; 00348 } 00349 } 00350 } 00351 if(T2 == 6) { //ボール掴んで投げる 00352 snatch=1; 00353 tsukami(); 00354 printf("tsukami"); 00355 wait(3); 00356 snatch=0; 00357 wait(1); 00358 put(); 00359 printf("put"); 00360 wait(3); 00361 snatch=1; 00362 wait(1); 00363 top(); 00364 printf("top"); 00365 wait(5); 00366 pass1=0; 00367 wait(5); 00368 00369 pass1=1; 00370 pass2=1; 00371 wait(5); 00372 T2++; 00373 //T2=7; 00374 //while(1) { 00375 // printf("t2=6"); 00376 // if(T2 == 7) { 00377 // break; 00378 // } 00379 // } 00380 } 00381 if(T2 == 7) { //移動 00382 //T2=8; 00383 00384 pass2=0; 00385 wait(1); 00386 while(1) { 00387 nh.spinOnce(); 00388 wait(0.01); 00389 printf("t2=7"); 00390 if(T2 == 8) { 00391 break; 00392 } 00393 } 00394 } 00395 00396 if(T2 == 8) { //ボール掴んで投げる 00397 snatch=1; 00398 tsukami(); 00399 printf("tsukami"); 00400 wait(3); 00401 snatch=0; 00402 wait(1); 00403 put(); 00404 printf("put"); 00405 wait(3); 00406 snatch=1; 00407 wait(1); 00408 top(); 00409 printf("top"); 00410 wait(5); 00411 pass1=0; 00412 wait(5); 00413 //wait(5); 00414 00415 pass1=1; 00416 pass2=1; 00417 wait(5); 00418 00419 T2++; 00420 //T2=9; 00421 //while(1) { 00422 // printf("t2=8"); 00423 // if(T2 == 9) { 00424 // break; 00425 // } 00426 // } 00427 } 00428 if(T2 == 9) { //idou 00429 pass2=0; 00430 wait(1); 00431 //T2=10; 00432 while(1) { 00433 nh.spinOnce(); 00434 wait(0.01); 00435 printf("t2=9"); 00436 if(T2 == 10) { 00437 break; 00438 } 00439 } 00440 } 00441 if(T2 == 10) { //ボール掴んで投げる 00442 snatch=1; 00443 tsukami(); 00444 printf("tsukami"); 00445 wait(3); 00446 snatch=0; 00447 wait(1); 00448 put(); 00449 printf("put"); 00450 wait(3); 00451 snatch=1; 00452 wait(1); 00453 top(); 00454 printf("top"); 00455 wait(5); 00456 pass1=0; 00457 wait(5); 00458 //wait(5); 00459 00460 T2++; 00461 //T2=9; 00462 //while(1) { 00463 // printf("t2=10"); 00464 // if(T2 == 11) { 00465 // break; 00466 // } 00467 // } 00468 } 00469 if(T2 == 11) { //スタートゾーンに戻る(アーム待機) 00470 //T2=10; 00471 while(1) { 00472 nh.spinOnce(); 00473 wait(0.01); 00474 printf("t2=11"); 00475 if(T2 == 12) { 00476 break; 00477 } 00478 } 00479 } 00480 #endif 00481 #ifdef NHK2020_MAIN_MODE3 00482 can1.frequency(1000000); 00483 can_ticker.attach(&can_read,0.01); 00484 //printf("T2 = %d\n\r",T2); 00485 if(T2 == 0) { 00486 //T2=1; 00487 snatch=1; 00488 while(1) { 00489 nh.spinOnce(); 00490 wait(0.01); 00491 if(T2 == 1) { 00492 break; 00493 } 00494 } 00495 } 00496 if(T2 == 1) { //スタート位置からkick前まで移動(アーム待機) 00497 //T2=2; 00498 while(1) { 00499 nh.spinOnce(); 00500 wait(0.01); 00501 if(T2 == 2) { 00502 break; 00503 } 00504 } 00505 } 00506 00507 if(T2 == 2) { //idou 00508 //T2=3; 00509 while(1) { 00510 nh.spinOnce(); 00511 wait(0.01); 00512 if(T2 == 3) { 00513 break; 00514 } 00515 } 00516 } 00517 if(T2 == 3) { //kick 00518 //T2=4; 00519 wait(1); 00520 tkeep=1; 00521 wait(1); 00522 snatch=0; 00523 wait(1); 00524 kick=1; 00525 while(1) { 00526 nh.spinOnce(); 00527 wait(0.01); 00528 if(T2 == 4) { 00529 break; 00530 } 00531 } 00532 } 00533 if(T2 == 4) { //kitaku 00534 //T2=5; 00535 while(1) { 00536 nh.spinOnce(); 00537 wait(0.01); 00538 if(T2 == 5) { 00539 break; 00540 } 00541 } 00542 } 00543 00544 00545 #endif 00546 00547 00548 00549 00550 }
Generated on Tue Aug 2 2022 04:17:20 by
1.7.2