main.cpp

00001 #include "mbed.h"
00002 #include "EC.h" //Encoderライブラリをインクルード
00003 #include "SpeedController.h"
00004 #define RESOLUTION 2048//分解能
00005 
00006 //#define NHK2020_TEST_MODE
00007 #define NHK2020_MAIN_MODE
00008 //#define NHK2020_MAIN_MODE2
00009 CAN can1(p30,p29);
00010 
00011 Ticker can_ticker;  //can用ticker
00012 
00013 Ec4multi EC_backdrop(p15,p16,RESOLUTION);
00014 SpeedControl backdrop(p21,p22,50,EC_backdrop);
00015 Serial pc(USBTX,USBRX);
00016 DigitalOut snatch(p8);
00017 DigitalOut pass1(p27);
00018 DigitalOut pass2(p28);
00019 DigitalOut pakapaka1(p14);
00020 //Ticker motor_tick;  //角速度計算用ticker
00021 
00022 char t2[1]= {0}; //動作番号（送信する値（char型））
00023 int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））
00024 
00025 double a=0;//now
00026 double b=0;//target
00027 double turn=0;
00028 int k = 0;
00029 void tsukami()
00030 {
00031     b=1.3;
00032 }
00033 void put()
00034 {
00035     b=-0.7;
00036 }
00037 void top()
00038 {
00039     b=0;
00040 }
00041 void move_motor()
00042 {
00043     while(1) {
00044         double old_turn=turn;
00045         a=EC_backdrop.getRad();
00046         if(a-b>=0.1) {
00047             turn=0.1;
00048             pc.printf("F");
00049         } else if (a-b>=0.05) {
00050             turn=10*(a-b)*(a-b);
00051             pc.printf("f");
00052         } else if (b-a>=0.1) {
00053             turn=-0.1;
00054             pc.printf("B");
00055         } else if (b-a>=0.05) {
00056             turn=-10*(a-b)*(a-b);
00057             pc.printf("b");
00058         } else {
00059             backdrop.stop();
00060             backdrop.turn(0);
00061             turn=0;
00062             pc.printf("s");
00063         }
00064         if(turn*old_turn<0)turn=0;
00065         backdrop.turn(turn);
00066         pc.printf("%lf",EC_backdrop.getRad());
00067         if(b-a<0.05&&a-b<0.05)break;
00068     }
00069 }
00070 
00071 int q = 0;
00072 
00073 void can_read()
00074 {
00075 
00076 
00077     CANMessage msg;
00078 
00079     if(can1.read(msg)) {
00080 
00081         if(msg.id == 1) {
00082             t2_r = msg.data[0];
00083             //printf("arm T2 = %d  t2_r = %d\n\r",T2,t2_r);
00084         } else {
00085             if(q > 100) {
00086                 //printf("id1 fale\n\r");
00087                 q = 0;
00088             }
00089             q++;
00090         }
00091 
00092     } else {
00093         if(k > 100) {
00094             //printf("arm fale\n\r");
00095             k = 0;
00096         }
00097         k++;
00098     }
00099 
00100     t2[0] = T2;
00101 
00102     if(can1.write(CANMessage(2,t2,1))) {
00103     }
00104 
00105     if(t2_r > T2) {
00106         T2 = t2_r;
00107     }
00108 
00109 
00110 }
00111 
00112 
00113 int main()
00114 {
00115     pc.printf("setting please");
00116     move_motor();
00117     while(1) {
00118 
00119 
00120 
00121 
00122 #ifdef NHK2020_TEST_MODE
00123         printf("T2 = %d\n\r",T2);
00124         pc.printf("%lf",EC_backdrop.getRad());
00125         if(pc.readable()) {
00126             char sel=pc.getc();
00127             if(sel=='z') {
00128                 pc.printf("z\r\n");
00129                 tsukami();
00130             } else if(sel=='x') {
00131                 pc.printf("x\r\n");
00132                 put();
00133             } else if(sel=='c') {
00134                 pc.printf("c\r\n");
00135                 top();
00136             }
00137             /*            if(sel=='q') {
00138                             printf("\r\n");
00139                             if(denjiben==0)denjiben=1;
00140                             else denjiben=0;
00141                         }*/
00142             if(sel=='1') {
00143                 snatch=0;
00144                 printf("snatch_off\r\n");
00145             }
00146             if(sel=='2') {
00147                 snatch=1;
00148                 printf("snatch_on\r\n");
00149             }
00150             if(sel=='3') {
00151                 pass1=0;
00152                 printf("pass1_off\r\n");
00153             }
00154             if(sel=='4') {
00155                 pass1=1;
00156                 printf("pass1_on\r\n");
00157             }
00158             if(sel=='5') {
00159                 pass2=0;
00160                 printf("pass2_off\r\n");
00161             }
00162             if(sel=='6') {
00163                 pass2=1;
00164                 printf("pass2_on\r\n");
00165             }
00166             //     if(sel=='a') {
00167             //         //pc.printf("x\r\n");
00168             //         //put();
00169             //     }
00170         }
00171     }
00172 #endif
00173 #ifdef NHK2020_MAIN_MODE
00174     can1.frequency(1000000);
00175     can_ticker.attach(&can_read,0.01);
00176     printf("wait_mode\r\n");
00177     //printf("T2 = %d\n\r",T2);
00178     if(T2 == 0) {
00179         while(1) {
00180             char sel=pc.getc();
00181             if(sel=='1') {
00182                 snatch=0;
00183                 printf("snatch_off\r\n");
00184             }
00185             if(sel=='2') {
00186                 snatch=1;
00187                 printf("snatch_on\r\n");
00188             }
00189             if(sel=='3') {
00190                 pass1=0;
00191                 printf("pass1_off\r\n");
00192             }
00193             if(sel=='4') {
00194                 pass1=1;
00195                 printf("pass1_on\r\n");
00196             }
00197             if(sel=='5') {
00198                 pass2=0;
00199                 printf("pass2_off\r\n");
00200             }
00201             if(sel=='6') {
00202                 pass2=1;
00203                 printf("pass2_on\r\n");
00204             }
00205             if(sel=='q') {
00206                 printf("end_wait_mode\r\n");
00207                 T2=1;
00208                 break;
00209 
00210             }
00211         }
00212     }
00213     if(T2 == 1) {  //スタート位置からボール前まで移動（アーム待機）
00214         //T2=1;
00215         while(1) {
00216             printf("a-T = 0  T2 = %d  t2_r = %d\n\r",T2,t2_r);
00217             wait(0.1);
00218             if(T2 == 2) {
00219                 break;
00220             }
00221         }
00222     }
00223 
00224     if(T2 == 2) {  //ボール掴んで投げる
00225         //printf("a-T = 1  T2 = %d  t2_r = %d\n\r",T2,t2_r);
00226         snatch=1;
00227         tsukami();
00228         printf("tsukami");
00229         move_motor();
00230         wait(3);
00231         snatch=0;
00232         wait(1);
00233         put();
00234         printf("put");
00235         move_motor();
00236         wait(3);
00237         snatch=1;
00238         wait(1);
00239         top();
00240         printf("top");
00241         move_motor();
00242         wait(5);
00243         pass1=0;
00244         wait(5);
00245         //wait(5);
00246 
00247         T2++;
00248     }
00249     if(T2 == 3) {  //スタートゾーンに戻る（アーム待機）
00250         while(1) {
00251             printf("a-T = 2  T2 = %d  t2_r = %d\n\r",T2,t2_r);
00252             wait(0.1);
00253             if(T2 == 4) {
00254                 break;
00255             }
00256         }
00257     }
00258     printf("end\n\r");
00259 }
00260 #endif
00261 #ifdef NHK2020_MAIN_MODE2
00262 can1.frequency(1000000);
00263 can_ticker.attach(&can_read,0.01);
00264 //printf("T2 = %d\n\r",T2);
00265 if(T2 == 0)
00266 {
00267     //T2=1;
00268     while(1) {
00269         if(T2 == 1) {
00270             break;
00271         }
00272     }
00273 }
00274 if(T2 == 1)    //スタート位置からボール前まで移動（アーム待機）
00275 {
00276     //T2=2;
00277     while(1) {
00278         if(T2 == 2) {
00279             break;
00280         }
00281     }
00282 }
00283 
00284 if(T2 == 2)    //ボール掴んで投げる
00285 {
00286     pakapaka=1;
00287     //T2=3;
00288     while(1) {
00289         if(T2 == 3) {
00290             break;
00291         }
00292     }
00293 }
00294 if(T2 == 3)    //スタートゾーンに戻る（アーム待機）
00295 {
00296     //T2=4;
00297     while(1) {
00298         if(T2 == 4) {
00299             break;
00300         }
00301     }
00302 }
00303 if(T2 == 4)    //スタート位置からボール前まで移動（アーム待機）
00304 {
00305     pakapaka=0;
00306     //T2=5;
00307     while(1) {
00308         if(T2 == 5) {
00309             break;
00310         }
00311     }
00312 }
00313 
00314 if(T2 == 5)    //ボール掴んで投げる
00315 {
00316     //T2=6;
00317     while(1) {
00318         if(T2 == 6) {
00319             break;
00320         }
00321     }
00322 }
00323 if(T2 == 6)    //スタートゾーンに戻る（アーム待機）
00324 {
00325     //T2=7;
00326     while(1) {
00327         if(T2 == 7) {
00328             break;
00329         }
00330     }
00331 }
00332 if(T2 == 7)    //スタート位置からボール前まで移動（アーム待機）
00333 {
00334     //T2=8;
00335     while(1) {
00336         if(T2 == 8) {
00337             break;
00338         }
00339     }
00340 }
00341 
00342 if(T2 == 8)    //ボール掴んで投げる
00343 {
00344     //T2=9;
00345     while(1) {
00346         if(T2 == 9) {
00347             break;
00348         }
00349     }
00350 }
00351 if(T2 == 9)    //スタートゾーンに戻る（アーム待機）
00352 {
00353     //T2=10;
00354     while(1) {
00355         if(T2 == 10) {
00356             break;
00357         }
00358     }
00359 }
00360 
00361 #endif
00362 #ifdef NHK2020_MAIN_MODE3
00363 can1.frequency(1000000);
00364 can_ticker.attach(&can_read,0.01);
00365 //printf("T2 = %d\n\r",T2);
00366 if(T2 == 0)
00367 {
00368     //T2=1;
00369     while(1) {
00370         if(T2 == 1) {
00371             break;
00372         }
00373     }
00374 }
00375 if(T2 == 1)    //スタート位置からボール前まで移動（アーム待機）
00376 {
00377     //T2=2;
00378     while(1) {
00379         if(T2 == 2) {
00380             break;
00381         }
00382     }
00383 }
00384 
00385 if(T2 == 2)    //ボール掴んで投げる
00386 {
00387     pakapaka=1;
00388     //T2=3;
00389     while(1) {
00390         if(T2 == 3) {
00391             break;
00392         }
00393     }
00394 }
00395 if(T2 == 3)    //スタートゾーンに戻る（アーム待機）
00396 {
00397     //T2=4;
00398     while(1) {
00399         if(T2 == 4) {
00400             break;
00401         }
00402     }
00403 }
00404 if(T2 == 4)    //スタート位置からボール前まで移動（アーム待機）
00405 {
00406     pakapaka=0;
00407     //T2=5;
00408     while(1) {
00409         if(T2 == 5) {
00410             break;
00411         }
00412     }
00413 }
00414 
00415 
00416 #endif
00417 
00418 
00419 
00420 
00421 }