main.cpp

00001 #include "mbed.h"
00002 //#include "USBHostMSD.h"
00003 //#include "USBHostXpad.h"
00004 //#include "SDFileSystem.h"
00005 
00006 /******************************入出力設定******************************/
00007 DigitalOut led1(LED1);      //mbed上LED出力4つ
00008 DigitalOut led2(LED2);
00009 DigitalOut led3(LED3);
00010 DigitalOut led4(LED4);
00011 
00012 //モータのPWM出力
00013 PwmOut pwm1(p21);       //一番上
00014 PwmOut pwm2(p22);       //上から二番目
00015 PwmOut pwm3(p23);       //上から三番目
00016 PwmOut pwm4(p24);       //一番下
00017 
00018 //モータドライバのDIRへの信号
00019 DigitalOut dir1(p30);
00020 DigitalOut dir2(p29);
00021 DigitalOut dir3(p28);
00022 DigitalOut dir4(p27);
00023 
00024 //スイッチ類入力
00025 DigitalIn startsw(p11);     //スタートスイッチ、押した後離してプログラム開始
00026 DigitalIn dip1(p12);        //DIPスイッチ、dip1は一番左(DIPスイッチの1)
00027 DigitalIn dip2(p13);        //2
00028 DigitalIn dip3(p14);        //3
00029 DigitalIn dip4(p15);        //4
00030 DigitalIn forward(p16);     //FRスイッチのF
00031 DigitalIn reverse(p17);     //FRスイッチのR
00032 AnalogIn potentiometer(p19);        //ポテンショメータ
00033 AnalogIn lrsw(p20);
00034 
00035 //ブザー
00036 DigitalOut buzzer(p18);      //電子ブザー用アナorデジ出力
00037 
00038 //USBフラッシュメモリ
00039 //USBHostMSD msc("msc");
00040 //USBでXbox360ワイヤレスコントローラを使うには、USBHostXpadを使う
00041 
00042 //SDカード
00043 //SDFileSystem sd(p5, p6, p7, p8, "sd");
00044 
00045 //予備のデジタル入出力:p16,p17,p19,p20
00046 
00047 /******************************プロトタイプ宣言******************************/
00048 int Init();     //初期化関数
00049 int DipLed();       //DIPスイッチの状態によってledを光らせる関数
00050 int GetDipValue();      //DIPスイッチの値を取得し，値を返す関数
00051 int SW();        //スタートスイッチ用関数，押して離したらスタート
00052 int Buzzer(int buzvar = 1);     //電子ブザーを鳴らす関数
00053 int GraduallyStop();        //ゆるやかにモータを停止させるための関数
00054 int GraduallyStop2();
00055 int GraduallyStart(float pot, int lr, int fr);      //ゆるやかにモータを始動させる関数
00056 int GraduallyStart2(float a, float b, float c, float d, float v);
00057 int MotorsPwm(double a = 0, double b = 0, double c = 0, double d = 0, double v = 0);       //4つのモータに速度を与える関数(デフォルトで停止)
00058 int MotorsPwm2(double pot = 0, int lr = 0, int fr = -1);        //4つのモータに速度を与える関数その2(デフォルトで停止)
00059 int MotorsDir(int a);       //4つのモータの回転方向を決定する関数)
00060 float RoundDemical1(float a);       //小数点以下第一桁で四捨五入
00061 float RoundDemicaln(float a, int n);        //小数点以下第n桁で四捨五入
00062 int SwitchState();      //2つのスイッチからの入力を返す
00063 
00064 /******************************main文******************************/
00065 int main() {
00066     Init();
00067     Buzzer(2);
00068     int flag = 0;       //動作モード切り替え用変数
00069     int mfrstate = 1;     //モータの状態を保存する変数(正転:0,逆転:1)        ←違うふうに使っている場合あり
00070     int mlrstate = 0;       //モータの状態を保存する変数(左:0,直進:1,右:2)
00071     float pot = 0;     //ポテンショメータからの入力を格納
00072     float lrvalue = 0;     //LRスイッチからの入力を格納
00073     int frvalue = 0;        //FRスイッチの状態を格納
00074     int lrstate = 0;        //進み方の状態を格納(0,1,2)
00075     int swstate = 0;       //スイッチの入力状態を格納
00076     int dir[4];
00077     
00078     while(1){
00079         flag = 0;
00080         mfrstate = 1;
00081         mlrstate = 1;
00082         pot = 0;
00083         lrvalue = 0;
00084         frvalue = 0;
00085         swstate = 0;
00086         for(int i = 0; i < 4; i++){
00087             dir[i] = 0;
00088         }
00089         
00090         Init();
00091         wait(1);        //1秒待つ
00092         SW();       //スイッチ押下でスタート
00093         wait(1);
00094         flag = GetDipValue();
00095         DipLed();
00096         
00097         switch(flag){
00098             case 0:     //DIPスイッチ0でなにもなし
00099                 Buzzer(4);
00100                 break;
00101             
00102             ////////////////////////////////////////////////////////////////////////////////////////////////
00103             case 1:     //テストモード1
00104                 Buzzer(1);
00105                 
00106                     dir1 = 0;
00107                     dir2 = 0;
00108                     dir3 = 0;
00109                     dir4 = 0;
00110                     pwm1 = 1;
00111                     pwm2 = 1;
00112                     pwm3 = 1;
00113                     pwm4 = 1;
00114                     
00115                     SW();       //スイッチの入力で停止
00116                     Buzzer(5);
00117                     GraduallyStop();
00118                 
00119                 pwm1 = 0;
00120                 pwm2 = 0;
00121                 pwm3 = 0;
00122                 pwm4 = 0;
00123                 break;
00124             
00125             ////////////////////////////////////////////////////////////////////////////////////////////////
00126             case 2:     //テストモード2(1の反対回転)
00127                 Buzzer(2);
00128                     dir1 = 1;
00129                     dir2 = 1;
00130                     dir3 = 1;
00131                     dir4 = 1;
00132                     pwm1 = 1;
00133                     pwm2 = 1;
00134                     pwm3 = 1;
00135                     pwm4 = 1;
00136                     
00137                     SW();
00138                     Buzzer(5);
00139                     GraduallyStop();
00140                     
00141                 pwm1 = 0;
00142                 pwm2 = 0;
00143                 pwm3 = 0;
00144                 pwm4 = 0;
00145                 break;
00146             
00147             ////////////////////////////////////////////////////////////////////////////////////////////////
00148             case 3:     //前後回転の切り替えと、ポテンショメータで回転速度を制御
00149                 Buzzer(3);
00150                 while(startsw == 0){
00151                     pot = potentiometer;
00152                     
00153                     if(((forward == 1 && mfrstate == 1) || (reverse == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
00154                         GraduallyStop();
00155                     }else if(forward == 1){     //正転
00156                         pwm1 = pot;
00157                         pwm2 = pot;
00158                         pwm3 = pot;
00159                         pwm4 = pot;
00160                         dir1 = 0;
00161                         dir2 = 0;
00162                         dir3 = 0;
00163                         dir4 = 0;
00164                         mfrstate = 0;
00165                     }else if(reverse == 1){        //逆転
00166                         pwm1 = pot;
00167                         pwm2 = pot;
00168                         pwm3 = pot;
00169                         pwm4 = pot;
00170                         dir1 = 1;
00171                         dir2 = 1;
00172                         dir3 = 1;
00173                         dir4 = 1;
00174                         mfrstate = 1;
00175                     }
00176                     
00177                     wait_ms(100);
00178                 }
00179                 GraduallyStop();
00180                 pwm1 = 0;
00181                 pwm2 = 0;
00182                 pwm3 = 0;
00183                 pwm4 = 0;
00184                 mfrstate = 0;
00185                 Buzzer(5);
00186                 break;
00187             
00188             ////////////////////////////////////////////////////////////////////////////////////////////////
00189             case 4:     //左右移動の切り替え
00190                 Buzzer(3);
00191                 while(startsw == 0){
00192                     pot = potentiometer;
00193                     lrvalue = lrsw;
00194                     if(lrvalue < 0.33){                                 //LRスイッチ中央
00195                         MotorsPwm(0.75, 0.75, 0.75, 0.75, pot);
00196                     }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
00197                         MotorsPwm(1,0.5,1,0.5,pot);
00198                     }else{                                              //LRスイッチ左or右
00199                         MotorsPwm(0.5,1,0.5,1,pot);
00200                     }
00201                     wait_ms(100);
00202                 }
00203                 GraduallyStop();
00204                 MotorsPwm();
00205                 Buzzer(5);
00206                 break;
00207             
00208             ////////////////////////////////////////////////////////////////////////////////////////////////
00209             case 5:     //正転・逆転と左右旋回
00210                 Buzzer(3);
00211                 while(startsw == 0){
00212                     pot = potentiometer;
00213                     lrvalue = lrsw;
00214                     
00215                     if(forward == 1){
00216                         frvalue = 0;        //正転
00217                     }else if(reverse == 1){
00218                         frvalue = 1;        //逆転
00219                     }
00220                     if(((frvalue == 0 && mfrstate == 1) || (frvalue == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
00221                         GraduallyStop();
00222                     }
00223                     MotorsDir(frvalue);
00224                     mfrstate = frvalue;
00225                     
00226                     
00227                     
00228                     if(lrvalue < 0.33){                                 //LRスイッチ中央
00229                         MotorsPwm(0.5, 0.5, 0.5, 0.5, RoundDemical1(pot));
00230                     }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
00231                         MotorsPwm(1,0.33,1,0.33,RoundDemical1(pot));
00232                     }else{                                              //LRスイッチ左or右
00233                         MotorsPwm(0.33,1,0.33,1,RoundDemical1(pot));
00234                     }
00235                     
00236                     wait_ms(100);
00237                 }
00238                 GraduallyStop();
00239                 MotorsPwm();
00240                 Buzzer(5);
00241                 break;
00242                 
00243             ////////////////////////////////////////////////////////////////////////////////////////////////
00244             case 6:     //正転・逆転と左右旋回
00245                 Buzzer(3);
00246                 while(startsw == 0){
00247                     pot = potentiometer;
00248                     lrvalue = lrsw;
00249                     
00250                     if(forward == 1){
00251                         frvalue = 0;        //正転
00252                     }else if(reverse == 1){
00253                         frvalue = 1;        //逆転
00254                     }else{
00255                         frvalue = -1;       //停止
00256                     }
00257                     if(lrvalue < 0.33){                                 //LRスイッチ中央
00258                         lrstate = 0;
00259                     }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
00260                         lrstate = 1;
00261                     }else{                                              //LRスイッチ左or右
00262                         lrstate = 2;
00263                     }
00264                     
00265                     if(((frvalue == 0 && mfrstate == 1) || (frvalue == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
00266                         GraduallyStop();
00267                         wait(0.5);
00268                         MotorsDir(frvalue);
00269                         GraduallyStart(pot, lrstate, frvalue);
00270                     }else if((frvalue == -1) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
00271                         GraduallyStop();
00272                     }
00273                     MotorsDir(frvalue);
00274                     MotorsPwm2(pot, lrstate, frvalue);
00275                     mfrstate = frvalue;
00276                     
00277                     wait_ms(100);
00278                 }
00279                 GraduallyStop();
00280                 MotorsPwm();
00281                 Buzzer(5);
00282                 break;
00283                 
00284             ////////////////////////////////////////////////////////////////////////////////////////////////
00285             case 7:     //1台向けモード
00286                 Buzzer(1);
00287                 while(startsw == 0){
00288                     lrvalue = lrsw;
00289                     if(forward == 1){
00290                         frvalue = 0;        //正転
00291                     }else if(reverse == 1){
00292                         frvalue = 1;        //逆転
00293                     }else{
00294                         frvalue = -1;       //停止
00295                     }
00296                     if(lrvalue < 0.33){                                 //LRスイッチ中央
00297                         lrstate = 0;
00298                     }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
00299                         lrstate = 1;
00300                     }else{                                              //LRスイッチ左or右
00301                         lrstate = 2;
00302                     }
00303                     
00304                     /*if(((mfrstate != frvalue) || (mlrstate != lrstate)) && (pwm1.read() != 0) || (pwm2.read() != 0)){      //この辺作業中
00305                         GraduallyStop();
00306                     }*/
00307                     
00308                     
00309                     if((mfrstate != frvalue) || (mlrstate != lrstate)){
00310                         GraduallyStop();
00311                         wait(0.5);
00312                     }
00313                     
00314                     if(lrstate == 0){
00315                         if(frvalue == 0){
00316                             dir1 = 0;
00317                             dir2 = 0;
00318                         }else if(frvalue == 1){
00319                             dir1 = 1;
00320                             dir2 = 1;
00321                         }else if((pwm1.read() != 0) || (pwm2.read() != 0)){
00322                             GraduallyStop();
00323                         }
00324                     }else if(lrstate == 1){
00325                         dir1 = 0;
00326                         dir2 = 1;
00327                     }else if(lrstate == 2){
00328                         dir1 = 1;
00329                         dir2 = 0;
00330                     }
00331                     if(((mfrstate != frvalue) || (mlrstate != lrstate)) && (frvalue >= 0)){
00332                         pot = potentiometer;
00333                         for(int i = 0; i <= 100; i++){
00334                             pwm1 = RoundDemical1(pot) * (double)i / 100;
00335                             pwm2 = RoundDemical1(pot) * (double)i / 100;
00336                             wait_ms(2);
00337                         }
00338                     }
00339                         
00340                     
00341                     /*if(lrstate == 0){       //lrスイッチが中央で
00342                         if(frvalue == 0){       //frスイッチが前で前進
00343                             if(dir1.read() != 0 || dir2.read() != 0){
00344                                 //GraduallyStop();
00345                                 dir1 = 0;
00346                                 dir2 = 0;
00347                                 for(int i = 0; i <= 100; i++){
00348                                     pwm1 = RoundDemical1(pot) * (double)i / 100;
00349                                     pwm2 = RoundDemical1(pot) * (double)i / 100;
00350                                     wait_ms(5);
00351                                 }
00352                             }
00353                             dir1 = 0;
00354                             dir2 = 0;
00355                         }else if(frvalue == 1){
00356                             if(dir1.read() != 1 || dir2.read() != 1){
00357                                 //GraduallyStop();
00358                                 dir1 = 1;
00359                                 dir2 = 1;
00360                                 for(int i = 0; i <= 100; i++){
00361                                     pwm1 = RoundDemical1(pot) * (double)i / 100;
00362                                     pwm2 = RoundDemical1(pot) * (double)i / 100;
00363                                     wait_ms(5);
00364                                 }
00365                             }
00366                             dir1 = 1;
00367                             dir2 = 1;
00368                         }else if((pwm1.read() != 0) || (pwm2.read() != 0)){
00369                             GraduallyStop();
00370                         }
00371                     }else if(lrstate == 1){
00372                         if(dir1.read() != 0 || dir2.read() != 1){
00373                                 //GraduallyStop();
00374                                 dir1 = 0;
00375                                 dir2 = 1;
00376                                 for(int i = 0; i <= 100; i++){
00377                                     pwm1 = RoundDemical1(pot) * (double)i / 100;
00378                                     pwm2 = RoundDemical1(pot) * (double)i / 100;
00379                                     wait_ms(5);
00380                                 }
00381                             }
00382                         dir1 = 0;
00383                         dir2 = 1;
00384                     }else if(lrstate == 2){
00385                         if(dir1.read() != 1 || dir2.read() != 0){
00386                                 //GraduallyStop();
00387                                 dir1 = 1;
00388                                 dir2 = 0;
00389                                 for(int i = 0; i <= 100; i++){
00390                                     pwm1 = RoundDemical1(pot) * (double)i / 100;
00391                                     pwm2 = RoundDemical1(pot) * (double)i / 100;
00392                                     wait_ms(5);
00393                                 }
00394                             }
00395                         dir1 = 1;
00396                         dir2 = 0;
00397                     }*/
00398                     
00399                     pot = potentiometer;
00400                     if(frvalue >= 0){
00401                         pwm1 = RoundDemical1(pot);
00402                         pwm2 = RoundDemical1(pot);
00403                     }
00404                     mfrstate = frvalue;
00405                     mlrstate = lrstate;
00406                     
00407                     wait_ms(100);
00408                 }
00409                 
00410                 Buzzer(5);
00411                 break;
00412             
00413             case 8:     //case7 のブラッシュアップ版(1台モード)
00414                 Buzzer(1);
00415                 while(startsw == 0){
00416                     swstate = SwitchState();        //スイッチの入力を見る
00417                     if(1 <= swstate && swstate <= 3){       //1<= swstate<=3で前進
00418                         dir[0] = 0;
00419                         dir[1] = 0;
00420                     }else if(swstate == 4){     //どっちも中点で停止
00421                     }else if(swstate == 5){     //右に信地旋回
00422                         dir[0] = 0;
00423                         dir[1] = 1;
00424                     }else if(swstate == 6){     //左に信地旋回
00425                         dir[0] = 1;
00426                         dir[1] = 0;
00427                     }else if(swstate <= 9){     //7<=swstate<=9で後退
00428                         dir[0] = 1;
00429                         dir[1] = 1;
00430                     }
00431                     
00432                     if((dir1.read() != dir[0] || dir2.read() != dir[1] || swstate == 4) && (pwm1.read() != 0 || pwm2.read() != 0)){
00433                         GraduallyStop2();
00434                         mfrstate = 1;       //フラグ
00435                     }
00436                     dir1 = dir[0];      //dirに入力
00437                     dir2 = dir[1];
00438                     
00439                     pot = potentiometer;        //ポテンショメータの入力を見る
00440                     if(swstate % 3 == 1){       //ここ以下は走行するスピードを決定
00441                         if(swstate == 4){
00442                             pwm1 = 0;
00443                             pwm2 = 0;
00444                         }else{
00445                             if(mfrstate == 1){
00446                                 GraduallyStart2(1,1,0,0,RoundDemical1(pot));
00447                                 mfrstate = 0;
00448                             }
00449                             pwm1 = RoundDemical1(pot);
00450                             pwm2 = RoundDemical1(pot);
00451                         }
00452                     }else if(swstate % 3 == 2){     //右
00453                         if(swstate == 5){
00454                             if(mfrstate == 1){
00455                                 GraduallyStart2(1,1,0,0,RoundDemical1(pot));
00456                                 mfrstate = 0;
00457                             }
00458                             pwm1 = RoundDemical1(pot);
00459                             pwm2 = RoundDemical1(pot);
00460                         }else{
00461                             if(mfrstate == 1){
00462                                 GraduallyStart2(1,0.5,0,0,RoundDemical1(pot));
00463                                 mfrstate = 0;
00464                             }
00465                             pwm1 = RoundDemical1(pot);
00466                             pwm2 = 0.5 * RoundDemical1(pot);
00467                         }
00468                     }else{                              //左
00469                         if(swstate == 6){
00470                             if(mfrstate == 1){
00471                                 GraduallyStart2(1,1,0,0,RoundDemical1(pot));
00472                                 mfrstate = 0;
00473                             }
00474                             pwm1 = RoundDemical1(pot);
00475                             pwm2 = RoundDemical1(pot);
00476                         }else{
00477                             if(mfrstate == 1){
00478                                 GraduallyStart2(0.5,1,0,0,RoundDemical1(pot));
00479                                 mfrstate = 0;
00480                             }
00481                             pwm1 = 0.5 * RoundDemical1(pot);
00482                             pwm2 = RoundDemical1(pot);
00483                         }
00484                     }
00485                     wait_ms(100);
00486                 }
00487                 Buzzer(5);
00488                 break;
00489             
00490             /////////////////////////////////////////////////////////////////////////////////////
00491             case 9:     //2台モード
00492                 Buzzer(1);
00493                 while(startsw == 0){
00494                     swstate = SwitchState();
00495                     if(1 <= swstate && swstate <= 3){       //1<= swstate<=3で前進
00496                         for(int i = 0; i < 4; i++){
00497                             dir[i] = 0;
00498                         }
00499                     }else if(swstate <= 4){     //中点で停止
00500                     }else if(swstate <= 5){     //中点，右
00501                         dir[0] = 1;     //右に姿勢変化
00502                         dir[1] = 0;
00503                         dir[2] = 0;
00504                         dir[3] = 1;
00505                     }else if(swstate <= 6){     //中点，左
00506                         dir[0] = 0;     //左に姿勢変化
00507                         dir[1] = 1;
00508                         dir[2] = 1;
00509                         dir[3] = 0;
00510                     }else if(swstate <= 9){     //7<=swstate<=9で後退
00511                         for(int i = 0; i < 4; i++){
00512                             dir[i] = 1;
00513                         }
00514                     }
00515                     
00516                     if((dir1.read() != dir[0] || dir2.read() != dir[1] || dir3.read() != dir[2] || dir4.read() != dir[3] || (swstate == 4))
00517                     && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
00518                         GraduallyStop2();
00519                         mfrstate = 1;
00520                     }
00521                     dir1 = dir[0];
00522                     dir2 = dir[1];
00523                     dir3 = dir[2];
00524                     dir4 = dir[3];
00525                     
00526                     pot = potentiometer;
00527                     
00528                     if(4 <= swstate && swstate <= 6){       //ここ変える←変えた
00529                         if(swstate == 4){       //中点+中点で停止
00530                             pwm1 = 0;
00531                             pwm2 = 0;
00532                             pwm3 = 0;
00533                             pwm4 = 0;
00534                         }else{                  //中点+左右でそれぞれ信地旋回
00535                             if(mfrstate == 1){
00536                                 GraduallyStart2(1,1,1,1,RoundDemical1(pot));
00537                                 mfrstate = 0;
00538                             }
00539                             pwm1 = RoundDemical1(pot);
00540                             pwm2 = RoundDemical1(pot);
00541                             pwm3 = RoundDemical1(pot);
00542                             pwm4 = RoundDemical1(pot);
00543                         }
00544                     }else if(swstate % 3 == 1){
00545                         if(mfrstate == 1){
00546                             GraduallyStart2(1,1,1,1,RoundDemical1(pot));
00547                             mfrstate = 0;
00548                         }
00549                         pwm1 = RoundDemical1(pot);
00550                         pwm2 = RoundDemical1(pot);
00551                         pwm3 = RoundDemical1(pot);
00552                         pwm4 = RoundDemical1(pot);
00553                     }else if(swstate % 3 == 2){     //右
00554                         if(mfrstate == 1){
00555                             GraduallyStart2(1,0.5,1,0.5,RoundDemical1(pot));
00556                             mfrstate = 0;
00557                         }
00558                         pwm1 = RoundDemical1(pot);
00559                         pwm2 = 0.5 * RoundDemical1(pot);
00560                         pwm3 = RoundDemical1(pot);
00561                         pwm4 = 0.5 * RoundDemical1(pot);
00562                     }else{                          //左
00563                         if(mfrstate == 1){
00564                             GraduallyStart2(0.5,1,0.5,1,RoundDemical1(pot));
00565                             mfrstate = 0;
00566                         }
00567                         pwm1 = 0.5 * RoundDemical1(pot);
00568                         pwm2 = RoundDemical1(pot);
00569                         pwm3 = 0.5 * RoundDemical1(pot);
00570                         pwm4 = RoundDemical1(pot);
00571                     }
00572                     wait_ms(100);
00573                 }
00574                 Buzzer(5);
00575                 break;
00576             
00577             default:        //上記以外で何もなし
00578                 Buzzer(-3);
00579                 break;
00580             
00581         }
00582     }
00583     return 0;
00584 }
00585 
00586 
00587 /******************************ここから関数******************************/
00588 int Init(){     //初期化関数
00589     pwm1 = 0;
00590     pwm2 = 0;
00591     pwm3 = 0;
00592     pwm4 = 0;
00593     dir1 = 0;
00594     dir2 = 0;
00595     dir3 = 0;
00596     dir4 = 0;
00597     buzzer = 0;
00598     pwm1.period_us(25);     //Pololu 18v15のpwm周波数40kHzに合わせる
00599     pwm2.period_us(25);
00600     pwm3.period_us(25);
00601     pwm4.period_us(25);
00602     DipLed();
00603     return 0;
00604 }
00605 
00606 int DipLed(){       //DIPスイッチの状態によってledを光らせる関数
00607     if(dip1 == 1) led1 = 1; else led1 = 0;     //DIPスイッチの1がHならLEDを光らせる,Lなら光らせない
00608     if(dip2 == 1) led2 = 1; else led2 = 0;
00609     if(dip3 == 1) led3 = 1; else led3 = 0;
00610     if(dip4 == 1) led4 = 1; else led4 = 0;
00611     return 0;
00612 }
00613 
00614 int GetDipValue(){      //DIPスイッチの値を取得し，値を返す関数
00615     int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4; //DIPスイッチの値を取得
00616     if(Dip1 == 0){
00617         if(Dip2 == 0){
00618             if(Dip3 == 0){
00619                 if(Dip4 == 0) return 0; else return 1;      //DIPスイッチが0000の場合0を返す．0001なら1
00620             }else{
00621                 if(Dip4 == 0) return 2; else return 3;
00622             }
00623         }else{
00624             if(Dip3 == 0){
00625                 if(Dip4 == 0) return 4; else return 5;
00626             }else{
00627                 if(Dip4 == 0) return 6; else return 7;
00628             }
00629         }
00630     }else{
00631         if(Dip2 == 0){
00632             if(Dip3 == 0){
00633                 if(Dip4 == 0) return 8; else return 9;
00634             }else{
00635                 if(Dip4 == 0) return 10; else return 11;
00636             }
00637         }else{
00638             if(Dip3 == 0){
00639                 if(Dip4 == 0) return 12; else return 13;
00640             }else{
00641                 if(Dip4 == 0) return 14; else return 15;
00642             }
00643         }
00644     }
00645 }
00646 
00647 int SW(){        //スタートスイッチ用関数，押して離したらスタート
00648     int i = 0, j = 0;
00649     while(i < 3){       //チャタリング除去，15msにわたってスタートスイッチが押されていればbreak
00650         if(startsw == 1) i++;
00651         else i = 0;
00652         DipLed();
00653         wait_ms(5);
00654     }
00655     while(j < 3){       //上に同じ，スタートスイッチが離されたことを検知
00656         if(startsw == 0) j++;
00657         else j = 0;
00658         DipLed();
00659         wait_ms(5);
00660     }
00661     return 0;
00662 }
00663 
00664 int Buzzer(int buzvar){     //電子ブザーを鳴らす関数
00665     switch (buzvar){
00666         /**************エラーを知らせるbeep**************/
00667         case -3:        //error * - -
00668             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
00669             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00670             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00671             break;
00672         case -2:        //error * - - -
00673             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
00674             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00675             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00676             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00677             break;
00678         case -1:        //error * - - - -
00679             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
00680             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00681             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00682             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00683             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00684             break;
00685         /**************エラーここまで**************/
00686         case 0:         //サウンドなし
00687             buzzer = 0;
00688             break;
00689         /**************状態を知らせるためのbeep**************/
00690         case 1:         // *(短)
00691             buzzer = 1; wait(0.1); buzzer = 0;
00692             break;
00693         case 2:         // * *
00694             buzzer = 1; wait(0.1); buzzer = 0; wait(0.05);
00695             buzzer = 1; wait(0.1); buzzer = 0;
00696             break;
00697         case 3:         // -(長)
00698             buzzer = 1; wait(0.3); buzzer = 0;
00699             break;
00700         case 4:         // - -
00701             buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
00702             buzzer = 1; wait(0.3); buzzer = 0;
00703             break;
00704         case 5:         // ---
00705             buzzer = 1; wait(0.9); buzzer = 0;
00706             break;
00707         case 6:         // * * *  * * *  * * *  *
00708             for(int i = 0; i < 3; i++){
00709                 for(int j = 0; j < 3; j++){
00710                     buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
00711                 }
00712                 wait(0.2);
00713             }
00714             buzzer = 1; wait(0.1); buzzer = 0;
00715             break;
00716         case 7:         // **-* ** -* ** *** **** "finish"
00717             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00718             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00719             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
00720             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00721             wait(0.2);
00722             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00723             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00724             wait(0.2);
00725             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
00726             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00727             wait(0.2);
00728             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00729             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00730             wait(0.2);
00731             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00732             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00733             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00734             wait(0.2);
00735             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00736             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00737             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00738             buzzer = 1; wait(0.1); buzzer = 0;
00739             break;
00740         case 8:         // *-*** -*- --* "オワリ"
00741             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
00742             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
00743             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00744             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00745             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00746             wait(0.2);
00747             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
00748             buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
00749             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
00750             wait(0.2);
00751             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
00752             buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
00753             buzzer = 1; wait(0.1); buzzer = 0;
00754             break;
00755         case 9:        //ケータイの着信音的な
00756             for(int i = 0; i < 20; i++){
00757                 buzzer = 1; wait(0.03);
00758                 buzzer = 0; wait(0.03);
00759             }
00760             break;
00761         case 10:        //9の短いバージョン
00762             for(int i = 0; i < 20; i++){
00763                 buzzer = 1; wait(0.02);
00764                 buzzer = 0; wait(0.02);
00765             }
00766             break;
00767         /**************状態を知らせるためのbeepここまで**************/
00768         default:        //no sound
00769             buzzer = 0;
00770             break;
00771     }
00772     return 0;
00773 }
00774 
00775 int GraduallyStop(){        //ゆるやかにモータを停止させるための関数
00776     double pwmn1 = pwm1.read();
00777     double pwmn2 = pwm2.read();
00778     double pwmn3 = pwm3.read();
00779     double pwmn4 = pwm4.read();
00780     const int loop = 100;
00781     
00782     for(int i = 0; i < loop; i++){
00783         pwm1 = pwm1.read() - pwmn1 / loop;
00784         pwm2 = pwm2.read() - pwmn2 / loop;
00785         pwm3 = pwm3.read() - pwmn3 / loop;
00786         pwm4 = pwm4.read() - pwmn4 / loop;
00787         wait_ms(10);
00788     }
00789     
00790     pwm1 = 0;
00791     pwm2 = 0;
00792     pwm3 = 0;
00793     pwm4 = 0;
00794     
00795     return 0;
00796 }
00797 
00798 int GraduallyStop2(){
00799     double pwmn1 = pwm1.read();
00800     double pwmn2 = pwm2.read();
00801     double pwmn3 = pwm3.read();
00802     double pwmn4 = pwm4.read();
00803     const int loop = 100;
00804     
00805     for(int i = 0; i < loop; i++){
00806         pwm1 = pwm1.read() - pwmn1 / loop;
00807         pwm2 = pwm2.read() - pwmn2 / loop;
00808         pwm3 = pwm3.read() - pwmn3 / loop;
00809         pwm4 = pwm4.read() - pwmn4 / loop;
00810         if(i > 40){
00811             wait_ms(5);
00812             if(i > 60){
00813                 wait_ms(5);
00814             }
00815         }
00816         wait_ms(5);
00817     }
00818     
00819     pwm1 = 0;
00820     pwm2 = 0;
00821     pwm3 = 0;
00822     pwm4 = 0;
00823     
00824     return 0;
00825 }
00826     
00827     
00828 
00829 int GraduallyStart(float pot, int lr, int fr){      //ゆるやかにモータを始動させる関数
00830     const int loop = 100;
00831     for(int i = 0; i < loop; i++){
00832         MotorsPwm2(pot * (double)i/(double)loop, lr, fr);
00833         wait_ms(10);
00834     }
00835     MotorsPwm2(pot, lr);
00836     return 0;
00837 }
00838 
00839 int GraduallyStart2(float a, float b, float c, float d, float v){       //vはポテンショメータ
00840     float threshold = 0.3;
00841     if(v <= threshold){
00842         pwm1 = a * v;
00843         pwm2 = b * v;
00844         pwm3 = c * v;
00845         pwm4 = d * v;
00846     }else{
00847         for(float i = 0; i < v; i += 0.1){
00848             pwm1 = a * i;
00849             pwm2 = b * i;
00850             pwm3 = c * i;
00851             pwm4 = d * i;
00852             wait(0.05);
00853         }
00854     }
00855     return 0;
00856 }
00857     
00858 
00859 int MotorsPwm(double a, double b, double c, double d, double v){       //4つのモータに速度を与える関数(デフォルトで停止)
00860     pwm1 = a * v;
00861     pwm2 = b * v;
00862     pwm3 = c * v;
00863     pwm4 = d * v;
00864     return 0;
00865 }
00866 
00867 int MotorsPwm2(double pot, int lr, int fr){        //4つのモータに速度を与える関数その2(デフォルトで停止)
00868     float H = 1.0;
00869     float M = 0.75;
00870     float L = 0.5;
00871     if(fr >= 0){
00872         switch (lr){
00873             case 0:     //前進
00874                 MotorsPwm(M, M, M, M, RoundDemical1(pot));
00875                 break;
00876             case 1:     //右or左旋回
00877                 MotorsPwm(H, L, H, L, RoundDemical1(pot));
00878                 break;
00879             case 2:     //左or右旋回(1と逆)
00880                 MotorsPwm(L, H, L, H, RoundDemical1(pot));
00881                 break;
00882             default:
00883                 GraduallyStop();
00884                 break;
00885         }
00886     }else{
00887         MotorsPwm();
00888     }
00889     return 0;
00890 }
00891 
00892 int MotorsDir(int a){       //4つのモータの回転方向を決定する関数)
00893     if(a >= 0){
00894         dir1 = a;
00895         dir2 = a;
00896         dir3 = a;
00897         dir4 = a;
00898     }
00899     
00900     return 0;
00901 }
00902 
00903 float RoundDemical1(float a){       //小数点以下第一桁で四捨五入
00904     float b;
00905     a *= 10;
00906     b = (float)(int)(a+0.5);
00907     b /= 10;
00908     return b;
00909 }
00910 /*
00911 float RoundDemicaln(float a, int n){        //小数点以下第n桁で四捨五入
00912     float b, c;
00913     b = pow(a, n);
00914     c = (float)(int)(b+0.5);
00915     b = pow(c, 1.0/(double)n);
00916     return b;
00917 }
00918 */
00919 
00920 int SwitchState(){      //2つのスイッチからの入力を返す
00921     /*********************************************
00922     fr・lrで
00923     前・中点：1  前・右：2   前・左：3
00924     中点・中点：4 中点・右：5  中点・左：6
00925     後・中点：7  後・右：8   後・左：9
00926     を返す
00927     *********************************************/
00928     int state = 0;
00929     float lr = lrsw;
00930     
00931     if(forward == 1){        //frスイッチ前
00932         state = 1;
00933     }else if(reverse == 1){     //frスイッチ後ろ
00934         state = 7;
00935     }else{                  //frスイッチ中点
00936         state = 4;
00937     }
00938     if(lr < 0.33){      //lrスイッチ中点
00939         state += 0;
00940     }else if(lr < 0.67){        //lrスイッチ左
00941         state += 2;
00942     }else{              //lrスイッチ右
00943         state += 1;
00944     }
00945     
00946     return state;
00947 }
00948         
00949