a

Dependencies:   mbed KondoServoLibrary ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }