main.cpp

00001 //メインプログラム・UARTコマンド受信
00002 #include "mbed.h"
00003 #include <cstdio>
00004 #include <string>
00005 #include "ColorSensor.h"
00006 
00007 #define SirialComSpeed 38400                                                        //シリアル通信のBPS設定
00008 
00009 #define CommandNumber (sizeof CommandList) / (sizeof(CdListTable))                  //コマンドの数（CommandNumber）
00010 
00011 //変数定義
00012 int servoflag = 0;              //サーボホーン位置情報格納用
00013 int *pservoflag;                //ポインター
00014 int off_width = 1720;           //サーボホーン初期位置パルス幅[us]
00015 int *poff_width;                //ポインター
00016 int on_width = 1500;            //サーボホーン終端位置パルス幅[us]
00017 int *pon_width;                 //ポインター
00018 
00019 //シリアル関連
00020 RawSerial SerialCom(SERIAL_TX, SERIAL_RX);                                //シリアル通信
00021 std::string RcvCharStr = "";                                            //シリアル通信受信文字格納用
00022 
00023 //ピン設定
00024 DigitalOut led(LED1);
00025 PwmOut servopwm(PA_0);
00026 InterruptIn det0(PA_4);                       //POWER PKG 設置検知
00027 InterruptIn det1(PC_1);                       //MAIN PKG 設置検知
00028 InterruptIn det2(PC_0);                       //レバー降ろし検知
00029 DigitalOut cmuxen(PA_8);                    //カラーセンサー用マルチプレクサENピン
00030 BusOut cmuxaddr(PC_7,PA_9);                 //カラーセンサー用マルチプレクサADDRバス
00031 DigitalOut rmuxen(PA_10);                   //リレー用マルチプレクサENピン
00032 BusOut rmuxaddr(PB_10,PB_4,PB_5,PB_3);      //リレー用マルチプレクサADDRバス
00033 DigitalInOut relayctl1(PA_11, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
00034 DigitalInOut relayctl2(PB_12, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
00035 DigitalInOut relayctl3(PB_2, PIN_OUTPUT, OpenDrain, 0);               //個別リレー用制御
00036 DigitalInOut relayctl4(PB_1, PIN_OUTPUT, OpenDrain, 0);               //個別リレー用制御
00037 DigitalInOut relayctl5(PB_15, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
00038 DigitalInOut relayctl6(PB_14, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
00039 DigitalInOut relayctl7(PB_13, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
00040 DigitalInOut relayctl8(PC_4, PIN_OUTPUT, OpenDrain, 0);               //個別リレー用制御
00041 
00042 static std::string none;
00043 
00044 //関数宣言
00045 int help_mode(std::string para);
00046 int relay_set(std::string para);            //リレー用制御
00047 void initial();                             //初期化
00048 int led_lum_check(std::string para);        //カラーセンサー用マルチプレクサ制御 ＋ 輝度確認
00049 int led_all_check(std::string para);
00050 int servo_turn(std::string para);
00051 int pkg_detection(std::string para);
00052 void main_uart_receive();                   //コマンド文字列受信処理
00053 void risedet_interrupt();
00054 void falldet_interrupt();
00055 
00056 //クラス名の定義
00057 CsMode CsMode;
00058 
00059 //コマンドの宣言
00060 typedef struct {
00061     std::string Commond;
00062     int (* CommandFunction)(std::string para);
00063 } CdListTable;
00064 
00065 //コマンド一覧（左側がコマンド、右側が飛び先の関数）
00066 CdListTable CommandList[] = {
00067     {"help",   help_mode},
00068     {"led",   led_lum_check},
00069     {"led all",   led_all_check},
00070     {"relay",   relay_set},
00071     {"servo",   servo_turn},
00072     {"det",   pkg_detection},
00073 };
00074 
00075 //ヘルプ用コマンド一覧
00076 std::string HelpStr[] = {
00077     "* Commands List **********************************************************",
00078     "relayALLoff            : relay control",
00079     "relay1,2{on|off}",
00080     "relay3{on|off}",
00081     "relay4{on|off}",
00082     "relay5{on|off}",
00083     "relay21{on|off}",
00084     "relay22,23{on|off}",
00085     "relay24,25{on|off}",
00086     "relay26{on|off}",
00087     "relay26on20ms",
00088     "relay26on73ms",
00089     "relayMUXoff",
00090     "relay6on",
00091     "relay7on",
00092     "relay8on",
00093     "relay9on",
00094     "relay10on",
00095     "relay11on",
00096     "relay12on",
00097     "relay13on",
00098     "relay14on",
00099     "relay15on",
00100     "relay16on",
00101     "relay17on",
00102     "relay18on",
00103     "relay19on",
00104     "relay20on",
00105     "servo{on|off}          : servo control",
00106     "led{1|2|3}             : LED luminance check",
00107     "det                    : pkg,lever detection",
00108     "**************************************************************************",
00109     "",     //helpの終わりを示す
00110 };
00111 
00112 //ヘルプ表示
00113 int help_mode(std::string para)
00114 {
00115     int tmpCount = 0;
00116 
00117     //HelpStrを空文字が出てくるまで繰り返し表示
00118     while(HelpStr[tmpCount] != "") {
00119         SerialCom.printf("%s\r\n",HelpStr[tmpCount].c_str());
00120         tmpCount++;
00121     }
00122     return 0;
00123 }
00124 
00125 //リレー制御
00126 int relay_set(std::string para)
00127 {
00128     //基板とレバーが未検知の時はコマンドを実行しない
00129     int intdet0 = det0;
00130     int intdet1 = det1;
00131     int intdet2 = det2;
00132     if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00133         SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
00134         return 0;
00135     }
00136 
00137     //パラメータが
00138     if(para.substr(0)=="ALLoff") {
00139         relayctl1 = 0;
00140         relayctl2 = 0;
00141         relayctl3 = 0;
00142         relayctl4 = 0;
00143         relayctl5 = 0;
00144         relayctl6 = 0;
00145         relayctl7 = 0;
00146         relayctl8 = 0;
00147         rmuxen = 1;
00148         rmuxaddr.write(0);
00149         SerialCom.printf ("OK\r\n");
00150         return 0;
00151     } else if (para.substr(0)=="1,2on") {
00152         relayctl1 = 1;
00153 
00154         SerialCom.printf ("OK\r\n");
00155         return 0;
00156     } else if (para.substr(0)=="1,2off") {
00157         relayctl1 = 0;
00158         SerialCom.printf ("OK\r\n");
00159         return 0;
00160     } else if (para.substr(0)=="3on") {
00161         relayctl2 = 1;
00162         SerialCom.printf ("OK\r\n");
00163         return 0;
00164     } else if (para.substr(0)=="3off") {
00165         relayctl2 = 0;
00166         SerialCom.printf ("OK\r\n");
00167         return 0;
00168     } else if (para.substr(0)=="4on") {
00169         relayctl3 = 1;
00170         SerialCom.printf ("OK\r\n");
00171         return 0;
00172     } else if (para.substr(0)=="4off") {
00173         relayctl3 = 0;
00174         SerialCom.printf ("OK\r\n");
00175         return 0;
00176     } else if (para.substr(0)=="5on") {
00177         relayctl4 = 1;
00178         SerialCom.printf ("OK\r\n");
00179         return 0;
00180     } else if (para.substr(0)=="5off") {
00181         relayctl4 = 0;
00182         SerialCom.printf ("OK\r\n");
00183         return 0;
00184     } else if (para.substr(0)=="21on") {
00185         relayctl5 = 1;
00186         SerialCom.printf ("OK\r\n");
00187         return 0;
00188     } else if (para.substr(0)=="21off") {
00189         relayctl5 = 0;
00190         SerialCom.printf ("OK\r\n");
00191         return 0;
00192     } else if (para.substr(0)=="22,23on") {
00193         relayctl6 = 1;
00194         SerialCom.printf ("OK\r\n");
00195         return 0;
00196     } else if (para.substr(0)=="22,23off") {
00197         relayctl6 = 0;
00198         SerialCom.printf ("OK\r\n");
00199         return 0;
00200     } else if (para.substr(0)=="24,25on") {
00201         relayctl7 = 1;
00202         SerialCom.printf ("OK\r\n");
00203         return 0;
00204     } else if (para.substr(0)=="24,25off") {
00205         relayctl7 = 0;
00206         SerialCom.printf ("OK\r\n");
00207         return 0;
00208     } else if (para.substr(0)=="26on") {
00209         relayctl8 = 1;
00210         SerialCom.printf ("OK\r\n");
00211         return 0;
00212     } else if (para.substr(0)=="26off") {
00213         relayctl8 = 0;
00214         SerialCom.printf ("OK\r\n");
00215         return 0;
00216     } else if (para.substr(0)=="26on20ms") {
00217         relayctl8 = 1;
00218         wait(0.02);
00219         relayctl8 = 0;
00220         SerialCom.printf ("OK\r\n");
00221         return 0;
00222     } else if (para.substr(0)=="26on73ms") {
00223         relayctl8 = 1;
00224         wait(0.073);
00225         relayctl8 = 0;
00226         SerialCom.printf ("OK\r\n");
00227         return 0;
00228     } else if (para.substr(0)=="MUXoff") {
00229         rmuxen = 1;
00230         wait(0.01);
00231         rmuxaddr.write(0);
00232         SerialCom.printf ("OK\r\n");
00233         return 0;
00234     } else if (para.substr(0)=="6on") {
00235         rmuxen = 1;
00236         wait(0.01);
00237         rmuxaddr.write(1);
00238         wait(0.01);
00239         rmuxen = 0;
00240         SerialCom.printf ("OK\r\n");
00241         return 0;
00242     } else if (para.substr(0)=="7on") {
00243         rmuxen = 1;
00244         wait(0.01);
00245         rmuxaddr.write(2);
00246         wait(0.01);
00247         rmuxen = 0;
00248         SerialCom.printf ("OK\r\n");
00249         return 0;
00250     } else if (para.substr(0)=="8on") {
00251         rmuxen = 1;
00252         wait(0.01);
00253         rmuxaddr.write(3);
00254         wait(0.01);
00255         rmuxen = 0;
00256         SerialCom.printf ("OK\r\n");
00257         return 0;
00258     } else if (para.substr(0)=="9on") {
00259         rmuxen = 1;
00260         wait(0.01);
00261         rmuxaddr.write(4);
00262         wait(0.01);
00263         rmuxen = 0;
00264         SerialCom.printf ("OK\r\n");
00265         return 0;
00266     } else if (para.substr(0)=="10on") {
00267         rmuxen = 1;
00268         wait(0.01);
00269         rmuxaddr.write(5);
00270         wait(0.01);
00271         rmuxen = 0;
00272         SerialCom.printf ("OK\r\n");
00273         return 0;
00274     } else if (para.substr(0)=="11on") {
00275         rmuxen = 1;
00276         wait(0.01);
00277         rmuxaddr.write(6);
00278         wait(0.01);
00279         rmuxen = 0;
00280         SerialCom.printf ("OK\r\n");
00281         return 0;
00282     } else if (para.substr(0)=="12on") {
00283         rmuxen = 1;
00284         wait(0.01);
00285         rmuxaddr.write(7);
00286         wait(0.01);
00287         rmuxen = 0;
00288         SerialCom.printf ("OK\r\n");
00289         return 0;
00290     } else if (para.substr(0)=="13on") {
00291         rmuxen = 1;
00292         wait(0.01);
00293         rmuxaddr.write(8);
00294         wait(0.01);
00295         rmuxen = 0;
00296         SerialCom.printf ("OK\r\n");
00297         return 0;
00298     } else if (para.substr(0)=="14on") {
00299         rmuxen = 1;
00300         wait(0.01);
00301         rmuxaddr.write(9);
00302         wait(0.01);
00303         rmuxen = 0;
00304         SerialCom.printf ("OK\r\n");
00305         return 0;
00306     } else if (para.substr(0)=="15on") {
00307         rmuxen = 1;
00308         wait(0.01);
00309         rmuxaddr.write(10);
00310         wait(0.01);
00311         rmuxen = 0;
00312         SerialCom.printf ("OK\r\n");
00313         return 0;
00314     } else if (para.substr(0)=="16on") {
00315         rmuxen = 1;
00316         wait(0.01);
00317         rmuxaddr.write(11);
00318         wait(0.01);
00319         rmuxen = 0;
00320         SerialCom.printf ("OK\r\n");
00321         return 0;
00322     } else if (para.substr(0)=="17on") {
00323         rmuxen = 1;
00324         wait(0.01);
00325         rmuxaddr.write(12);
00326         wait(0.01);
00327         rmuxen = 0;
00328         SerialCom.printf ("OK\r\n");
00329         return 0;
00330     } else if (para.substr(0)=="18on") {
00331         rmuxen = 1;
00332         wait(0.01);
00333         rmuxaddr.write(13);
00334         wait(0.01);
00335         rmuxen = 0;
00336         SerialCom.printf ("OK\r\n");
00337         return 0;
00338     } else if (para.substr(0)=="19on") {
00339         rmuxen = 1;
00340         wait(0.01);
00341         rmuxaddr.write(14);
00342         wait(0.01);
00343         rmuxen = 0;
00344         SerialCom.printf ("OK\r\n");
00345         return 0;
00346     } else if (para.substr(0)=="20on") {
00347         rmuxen = 1;
00348         wait(0.01);
00349         rmuxaddr.write(15);
00350         wait(0.01);
00351         rmuxen = 0;
00352         SerialCom.printf ("OK\r\n");
00353         return 0;
00354         //パラメータが上記以外の場合
00355     } else {
00356         SerialCom.printf(" ERROR!\r\n");
00357         return 0;
00358     }
00359     //return 0;
00360 }
00361 
00362 //LEDの明るさ取得
00363 int led_lum_check(std::string para)
00364 {
00365     //基板とレバーが未検知の時はコマンドを実行しない
00366     int intdet0 = det0;
00367     int intdet1 = det1;
00368     int intdet2 = det2;
00369     if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00370         SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
00371         return 0;
00372     }
00373 
00374     int tmpInt = atoi(para.c_str());                //パラメータを数字に変換
00375     cmuxen = 1;                                     //マルチプレクサのENピンをHIGHにする
00376     wait(0.01);
00377 
00378     //ADDRバス切替
00379     cmuxaddr.write(tmpInt);
00380     wait(0.01);
00381 
00382     cmuxen = 0;                                     //マルチプレクサのENピンをLOWにする
00383     wait(0.01);
00384 
00385     //選択したLED表示
00386     //SerialCom.printf ("LED%d_", tmpInt);
00387 
00388     //LEDの輝度表示
00389     CsMode.SensorMain();
00390 
00391     SerialCom.printf ("OK\r\n");
00392     return 0;
00393 }
00394 
00395 int pkg_detection(std::string para)
00396 {
00397     int ppkgdet = det0;
00398     int mpkgdet = det1;
00399     int leverdet = det2;
00400 
00401     SerialCom.printf("%d%d%d\r\n", leverdet, mpkgdet, ppkgdet);
00402     SerialCom.printf ("OK\r\n");
00403     return 0;
00404 }
00405 
00406 //コマンド文字列解析
00407 void ReceiveCmdAnalyze()
00408 {
00409     //入力された文字列を1文字ずつ解析（文字列の長さ分まで繰り返す）
00410     for(int y=0; y<=RcvCharStr.size(); y++) {
00411 
00412         //コマンド全部と比較し一致しているのがあれば処理開始(前方一致)
00413         for (int i=0; i<CommandNumber; i++) {
00414             if(RcvCharStr.substr(0,y) == CommandList[i].Commond) {
00415 
00416                 //コマンド以降の文字列を引数にして関数呼び出し
00417                 (* *CommandList[i].CommandFunction)(RcvCharStr.substr(y));
00418 
00419                 //文字列のバッファクリア
00420                 RcvCharStr = "";
00421                 return;
00422             }
00423         }
00424     }
00425     //エラーメッセージ表示
00426     SerialCom.printf (" ERROR!\r\n\r\n");
00427     RcvCharStr = "";
00428     return;
00429 }
00430 
00431 //UART受信
00432 void main_uart_receive()
00433 {
00434     std::string tmpStr = "";                         //シリアル通信受信文字格納(1文字)
00435     tmpStr = SerialCom.getc();                       //受信文字を配列に追加
00436     SerialCom.printf("%s",tmpStr.c_str());           //エコーバック出力
00437 
00438     //①改行文字が来た場合コマンド解析へ
00439     if(tmpStr == "\r") {
00440         SerialCom.printf ("\r\n");                                //TeraTerm改行
00441         ReceiveCmdAnalyze();                                                //コマンド解析
00442         return;
00443 
00444         //②バックスペースが来た場合は文字列の末尾を削除
00445     } else if(tmpStr == "\b") {
00446 
00447         //1文字もない場合は何もしない
00448         if(RcvCharStr.size()>0) {
00449             RcvCharStr.erase(RcvCharStr.length()-1,1);
00450         }
00451 
00452         //③上記以外の場合は受信した文字をRcvCharStrへ格納
00453     } else {
00454         RcvCharStr +=  tmpStr;
00455     }
00456 }
00457 
00458 //サーボモータ制御
00459 int servo_turn(std::string para)
00460 {
00461     //オフ時のパルス幅は上記で定義
00462     //off_width = 1550;
00463     //on_width = 1350;
00464 
00465     //基板とレバーが未検知の時はコマンドを実行しない
00466     int intdet0 = det0;
00467     int intdet1 = det1;
00468     int intdet2 = det2;
00469     if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00470         SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
00471         return 0;
00472     }
00473 
00474     int waitms = 5;       //パルス時間変更後の待ち時間(ms)　(動作速度：0.1秒/60度(サーボモータSG92仕様))
00475 
00476     //パラメータがオフ(off)の時
00477     if(para.substr(0)=="off") {
00478         if(servoflag == 0) {
00479             SerialCom.printf(" ERROR! already OFF\r\n");
00480             return 0;
00481         } else {
00482             /*
00483             int pulseus = on_width;
00484             servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
00485             while (pulseus <= off_width) {
00486                 servopwm.pulsewidth_us(pulseus);
00487                 wait_ms(waitms);
00488                 pulseus = pulseus + 1;
00489             }
00490             */
00491             int pulseus = off_width;
00492             servopwm.period_ms(20);
00493             servopwm.pulsewidth_us(pulseus);
00494             servoflag = 0;              //サーボホーン位置情報
00495         }
00496     }
00497 
00498     //パラメータがオン(on)の時
00499     else if(para.substr(0)=="on") {
00500         //往路
00501         if(servoflag == 1) {
00502             SerialCom.printf(" ERROR! already ON\r\n");
00503             return 0;
00504         } else {
00505             int pulseus = off_width;
00506             servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
00507             while (pulseus >= on_width) {
00508                 servopwm.pulsewidth_us(pulseus);
00509                 wait_ms(waitms);
00510                 pulseus = pulseus - 1;
00511             }
00512             servoflag = 1;              //サーボホーン位置情報
00513         }
00514 
00515         wait(1);      //スイッチを押し切る時間を確保
00516 
00517         //復路
00518         if(servoflag == 0) {
00519             SerialCom.printf(" ERROR! already OFF\r\n");
00520             return 0;
00521         } else {
00522             int pulseus = on_width;
00523             servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
00524             while (pulseus <= off_width) {
00525                 servopwm.pulsewidth_us(pulseus);
00526                 wait_ms(waitms);
00527                 pulseus = pulseus + 1;
00528             }
00529             servoflag = 0;              //サーボホーン位置情報
00530 
00531             SerialCom.printf ("OK\r\n");
00532         }
00533     }
00534     //パラメータがオン時のパルス幅(****us)のとき
00535     else {
00536         int tmpInt = atoi(para.c_str());
00537         int pulseus = off_width;
00538         servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
00539         while (pulseus >= tmpInt) {
00540             servopwm.pulsewidth_us(pulseus);
00541             wait_ms(waitms);
00542             pulseus = pulseus - 1;
00543         }
00544 
00545         wait(1);
00546 
00547         pulseus = tmpInt;
00548         //servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
00549         while (pulseus <= off_width) {
00550             servopwm.pulsewidth_us(pulseus);
00551             wait_ms(waitms);
00552             pulseus = pulseus + 1;
00553         }
00554         SerialCom.printf ("OK\r\n");
00555     }
00556     return 0;
00557 }
00558 
00559 //リレーとマルチプレクサの初期設定
00560 void initial()
00561 {
00562     relayctl1 = 0;
00563     relayctl2 = 0;
00564     relayctl3 = 0;
00565     relayctl4 = 0;
00566     relayctl5 = 0;
00567     relayctl6 = 0;
00568     relayctl7 = 0;
00569     relayctl8 = 0;
00570     rmuxen = 1;
00571     rmuxaddr.write(0);
00572 
00573     //LEDのマルチプレクサをOFFに
00574     cmuxen = 1;
00575 
00576     return;
00577 }
00578 
00579 //LED全チェック
00580 int led_all_check(std::string para)
00581 {
00582     //基板とレバーが未検知の時はコマンドを実行しない
00583     int intdet0 = det0;
00584     int intdet1 = det1;
00585     int intdet2 = det2;
00586     if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00587         SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
00588         return 0;
00589     }
00590 
00591     char buf[2];
00592 
00593     //led_lum_checkをLED1-24まで実施
00594     for (int x = 1; x <= 3; x++) {
00595         snprintf(buf, 12, "%d", x);
00596         std::string str(buf, 2);
00597         led_lum_check(str);
00598     }
00599     return 0;
00600 }
00601 
00602 void risedet_interrupt()
00603 {
00604     wait(0.001);
00605     led = 0;
00606     int intdet0 = det0;
00607     int intdet1 = det1;
00608     int intdet2 = det2;
00609     if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00610         SerialCom.printf("\r\n Error! PKG, lever undetected! initializing...\r\n");
00611 
00612         //初期設定
00613         initial();
00614 
00615         //サーボがonの場合はoffへ
00616         if(servoflag == 1) {
00617             servo_turn("off");
00618         }
00619 
00620         //PKGとレバーが検知されるのを待つ
00621         SerialCom.printf ("\r\n Waiting for PKG installation...\r\n");
00622         intdet0 = det0;
00623         intdet1 = det1;
00624         intdet2 = det2;
00625         while(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00626             intdet0 = det0;
00627             intdet1 = det1;
00628             intdet2 = det2;
00629             if (SerialCom.readable()) {       //読み込む文字があるかどうか判定
00630                 main_uart_receive();
00631             }
00632         }
00633         SerialCom.printf("\r\n PKG, lever detected!\r\n\r\n restart!\r\n\r\n");
00634     }
00635     led = 1;
00636 }
00637 
00638 int main()
00639 {
00640     led = 0;
00641 
00642     //UART ボーレート設定
00643     SerialCom.baud(SirialComSpeed);
00644 
00645     //起動表示
00646     SerialCom.printf ("PCU-1L Unitchecker Initializing...\r\n");
00647 
00648     /* 基板上にプルアップを入れたのでコメントアウト
00649     //ピン変化割込みポートモード設定
00650     det0.mode(PullUp);
00651     det1.mode(PullUp);
00652     det2.mode(PullUp);
00653     */
00654 
00655     //初期設定
00656     initial();
00657 
00658     //LEDチェック
00659     led_all_check(none);
00660 
00661     //サーボモータ第一初期化
00662     pservoflag = &servoflag;        //ポインター(サーボホーン位置情報格納用)
00663     poff_width = &off_width;
00664     pon_width = &on_width;
00665     servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
00666     servopwm.pulsewidth_us(off_width);   //角度が決まる
00667 
00668     //PKGとレバーが検知されるのを待つ
00669     int intdet0 = det0;
00670     int intdet1 = det1;
00671     int intdet2 = det2;
00672     if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00673         SerialCom.printf ("\r\n Waiting for PKG installation...\r\n");
00674         while(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
00675             //wait(1);
00676             intdet0 = det0;
00677             intdet1 = det1;
00678             intdet2 = det2;
00679             if (SerialCom.readable()) {       //読み込む文字があるかどうか判定
00680                 main_uart_receive();
00681             }
00682         }
00683         SerialCom.printf("\r\n PKG, lever detected!\r\n\r\n");
00684     }
00685 
00686     led = 1;
00687 
00688     //PKG,レバー未検知割込みを起動
00689     det0.rise(&risedet_interrupt);
00690     det1.rise(&risedet_interrupt);
00691     det2.rise(&risedet_interrupt);
00692 
00693     SerialCom.printf (" ready!\r\n\r\n");
00694     //メインループ
00695     while(1) {
00696         if (SerialCom.readable()) {       //読み込む文字があるかどうか判定
00697             main_uart_receive();
00698         }
00699     }
00700 }