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 
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 //Ticker motor_tick;  //角速度計算用ticker
00020 
00021 char t2[1]= {0}; //動作番号（送信する値（char型））
00022 int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））
00023 
00024 double a=0;//now
00025 double b=0;//target
00026 double turn=0;
00027 int k = 0;
00028 void tsukami()
00029 {
00030     b=1.3;
00031 }
00032 void put()
00033 {
00034     b=-0.7;
00035 }
00036 void top()
00037 {
00038     b=0;
00039 }
00040 void move_motor()
00041 {
00042     while(1) {
00043         double old_turn=turn;
00044         a=EC_backdrop.getRad();
00045         if(a-b>=0.1) {
00046             turn=0.1;
00047             pc.printf("F");
00048         } else if (a-b>=0.05) {
00049             turn=10*(a-b)*(a-b);
00050             pc.printf("f");
00051         } else if (b-a>=0.1) {
00052             turn=-0.1;
00053             pc.printf("B");
00054         } else if (b-a>=0.05) {
00055             turn=-10*(a-b)*(a-b);
00056             pc.printf("b");
00057         } else {
00058             backdrop.stop();
00059             backdrop.turn(0);
00060             turn=0;
00061             pc.printf("s");
00062         }
00063         if(turn*old_turn<0)turn=0;
00064         backdrop.turn(turn);
00065         pc.printf("%lf",EC_backdrop.getRad());
00066         if(b-a<0.05&&a-b<0.05)break;
00067     }
00068 }
00069 
00070 int q = 0;
00071 
00072 void can_read()
00073 {
00074 
00075 
00076     CANMessage msg;
00077 
00078     if(can1.read(msg)) {
00079 
00080         if(msg.id == 1) {
00081             t2_r = msg.data[0];
00082             //printf("arm T2 = %d  t2_r = %d\n\r",T2,t2_r);
00083         } else {
00084             if(q > 100) {
00085                 //printf("id1 fale\n\r");
00086                 q = 0;
00087             }
00088             q++;
00089         }
00090 
00091     } else {
00092         if(k > 100) {
00093             //printf("arm fale\n\r");
00094             k = 0;
00095         }
00096         k++;
00097     }
00098 
00099     t2[0] = T2;
00100 
00101     if(can1.write(CANMessage(2,t2,1))) {
00102     }
00103 
00104     if(t2_r > T2) {
00105         T2 = t2_r;
00106     }
00107 
00108 
00109 }
00110 
00111 
00112 int main()
00113 {
00114     pc.printf("setting please");
00115     move_motor();
00116     while(1) {
00117 
00118 
00119 
00120 
00121 #ifdef NHK2020_TEST_MODE
00122         printf("T2 = %d\n\r",T2);
00123         pc.printf("%lf",EC_backdrop.getRad());
00124         if(pc.readable()) {
00125             char sel=pc.getc();
00126             if(sel=='z') {
00127                 pc.printf("z\r\n");
00128                 tsukami();
00129             } else if(sel=='x') {
00130                 pc.printf("x\r\n");
00131                 put();
00132             } else if(sel=='c') {
00133                 pc.printf("c\r\n");
00134                 top();
00135             }
00136             /*            if(sel=='q') {
00137                             printf("\r\n");
00138                             if(denjiben==0)denjiben=1;
00139                             else denjiben=0;
00140                         }*/
00141             if(sel=='1') {
00142                 snatch=0;
00143                 printf("snatch_off\r\n");
00144             }
00145             if(sel=='2') {
00146                 snatch=1;
00147                 printf("snatch_on\r\n");
00148             }
00149             if(sel=='3') {
00150                 pass1=0;
00151                 printf("pass1_off\r\n");
00152             }
00153             if(sel=='4') {
00154                 pass1=1;
00155                 printf("pass1_on\r\n");
00156             }
00157             if(sel=='5') {
00158                 pass2=0;
00159                 printf("pass2_off\r\n");
00160             }
00161             if(sel=='6') {
00162                 pass2=1;
00163                 printf("pass2_on\r\n");
00164             }
00165             //     if(sel=='a') {
00166             //         //pc.printf("x\r\n");
00167             //         //put();
00168             //     }
00169         }
00170     }
00171 #endif
00172 #ifdef NHK2020_MAIN_MODE
00173     can1.frequency(1000000);
00174     can_ticker.attach(&can_read,0.01);
00175     printf("wait_mode\r\n");
00176     //printf("T2 = %d\n\r",T2);
00177     if(T2 == 0) {
00178         while(1) {
00179             char sel=pc.getc();
00180             if(sel=='1') {
00181                 snatch=0;
00182                 printf("snatch_off\r\n");
00183             }
00184             if(sel=='2') {
00185                 snatch=1;
00186                 printf("snatch_on\r\n");
00187             }
00188             if(sel=='3') {
00189                 pass1=0;
00190                 printf("pass1_off\r\n");
00191             }
00192             if(sel=='4') {
00193                 pass1=1;
00194                 printf("pass1_on\r\n");
00195             }
00196             if(sel=='5') {
00197                 pass2=0;
00198                 printf("pass2_off\r\n");
00199             }
00200             if(sel=='6') {
00201                 pass2=1;
00202                 printf("pass2_on\r\n");
00203             }
00204             if(sel=='q') {
00205                 printf("end_wait_mode\r\n");
00206                 T2=1;
00207                 break;
00208 
00209             }
00210         }
00211     }
00212     if(T2 == 1) {  //スタート位置からボール前まで移動（アーム待機）
00213         //T2=1;
00214         while(1) {
00215             printf("a-T = 0  T2 = %d  t2_r = %d\n\r",T2,t2_r);
00216             wait(0.1);
00217             if(T2 == 2) {
00218                 break;
00219             }
00220         }
00221     }
00222 
00223     if(T2 == 2) {  //ボール掴んで投げる
00224         //printf("a-T = 1  T2 = %d  t2_r = %d\n\r",T2,t2_r);
00225         snatch=1;
00226         tsukami();
00227         printf("tsukami");
00228         move_motor();
00229         wait(3);
00230         snatch=0;
00231         wait(1);
00232         put();
00233         printf("put");
00234         move_motor();
00235         wait(3);
00236         snatch=1;
00237         wait(1);
00238         top();
00239         printf("top");
00240         move_motor();
00241         wait(5);
00242         pass1=0;
00243         wait(5);
00244         //wait(5);
00245 
00246         T2++;
00247     }
00248     if(T2 == 3) {  //スタートゾーンに戻る（アーム待機）
00249         while(1) {
00250             printf("a-T = 2  T2 = %d  t2_r = %d\n\r",T2,t2_r);
00251             wait(0.1);
00252             if(T2 == 4) {
00253                 break;
00254             }
00255         }
00256     }
00257     printf("end\n\r");
00258 }
00259 #endif
00260 }