main.cpp

00001 #include "mbed.h"
00002 #include "BNO055.h"
00003 #include "Ping.h"
00004 #include "Motor.h"
00005 
00006 //constant
00007 #define GK  1
00008 #define FW  0
00009 #define PI  3.1415926
00010 
00011 /*調整するときに読んでね*/
00012 /*ロボットを調整する項目は3つあります
00013  * 以下の調整は試合前に必ずに行ってください
00014  *
00015  * 一つ目はラインセンサの調整です
00016  *　ロボットを起動し、USBケーブルを挿しこんだ後ターミナルを起動して'd'を押してください
00017  *　各種センサ値が表示されます
00018  *　コートの緑のマット上でLine_front ~ Line_rightの数値が
00019  *　75前後　になるように、
00020  *　ロボットの底面についている可変抵抗(青いやつ)を小さいドライバーで回して調整してください
00021  *　可変抵抗を時計回りに回すと数値が上がります
00022  *　キーボード'r'でセンサの値の取得を一時停止することができます
00023  *
00024  *　２つ目は回り込み半径の調整です
00025  * Rを調整することでボールに対してどれくらいの半径を描いて回り込むか調整することができます
00026  * 上記と同様にターミナルソフトを起動してキーボード'd'を押すと
00027  * 現在のセンサの数値を見ることができますのでBall distance:
00028  * の数値を参考にして最適なRを決定してください(distanceの数値=Rにする)
00029  * キーボード'r'でセンサの値の取得を一時停止することができます
00030  *
00031  * ３つ目はスピードとPIDゲイン値の調整です
00032  * まずラインから出ないようにspeedを調整します
00033  * そのあとロボットが常に前に向くように(傾いても元の方向にすぐに戻るように)
00034  * tp ti tdを調整してください
00035  * P→D→I(またはP→I→D)の順番に調整すると良いかもしれません
00036  *
00037  *　以上のことが完了してからプログラムの作成を開始してください
00038  */
00039 
00040 /*BNO055モードについて
00041  * ジャイロセンサーには様々なモードがありますが
00042  * 基本的にNDOFモードを使って下さい
00043  * 会場の磁場環境がひどい場合はIMUモードを使ってください
00044  *　ロボットの電源を起動した直後にキャリブレーションを行ってください
00045  *　キャリブレーションをすることでオウンゴールを防ぐことができます。
00046  */
00047 /*change Mode IMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF*/
00048 
00049 //パラメータ調整項目
00050 #define MODE MODE_NDOF//ジャイロのモード
00051 const int mode_robot = FW;//FW:その場で停止 GK:ゴール前までもどる
00052 const int R = 100; //ロボット回り込み半径(0~255
00053 const int speed = 100; //(0~100)の間で調整
00054 const double tp = 1.5; //比例ゲイン
00055 const double ti = 1.5; //積分ゲイン
00056 const double td = 0.25; //微分ゲイン
00057 
00058 //TerminalDisplay(TeraTerm)
00059 Serial pc(SERIAL_TX, SERIAL_RX);
00060 
00061 //voltage
00062 AnalogIn voltage(PC_4);
00063 
00064 //UltraSonicSensor
00065 DigitalIn change(PA_6);
00066 Ping uss_left(PB_3);
00067 Ping uss_right(PA_7);
00068 Ping uss_back(PB_6);
00069 Timer timer_USS;
00070 Timer timer_volt;
00071 
00072 //BallSensor&Linesensor
00073 AnalogIn ball_degree(PA_4);
00074 AnalogIn ball_distance(PB_0);
00075 Serial sensor(PC_10, PC_11, 115200);
00076 
00077 //Motor
00078 Motor motor(PA_8,PB_4,PB_10, PB_15,PB_14,PB_13, PB_2,PB_1,PA_9, PA_12,PA_11,PA_5);
00079 
00080 //Timer
00081 Timer timer_PID;
00082 
00083 //GyroSensor
00084 I2C i2c(D14, D15);
00085 BNO055 imu(i2c, PC_8);
00086 BNO055_ID_INF_TypeDef bno055_id_inf;
00087 BNO055_EULER_TypeDef euler_angles;
00088 
00089 //HoldCheckSensor
00090 DigitalIn hold_check(PC_3);
00091 
00092 //kicker
00093 DigitalOut kick(PA_13);
00094 
00095 //ToggleSwitch
00096 DigitalIn sw_start(PD_2); //program start switch
00097 DigitalIn sw_reset(PC_12); //gyro sensor reset switch
00098 DigitalIn sw_kick(USER_BUTTON);
00099 
00100 //declear prototype (function list)
00101 void uss_send_and_read(); //超音波センサ読み込み
00102 int PID(double kp, double ki, double kd, int target, int degree, int reset = 0); //姿勢制御のPIDで計算する
00103 int get_ball_degree(int A = 0); //ボールの角度を-180~180(ボールなし999)で取得する
00104 int get_ball_distance(int A = 0); //ボールの距離を0~255で取得する
00105 int get_line_degree(); //反応したラインの方向(角度)を0~315(反応なし999)で取得する
00106 int turn(int degree, int distance, int target = 0, int angle = 0); //回り込みの進行方向を計算する
00107 int get_uss_range(char c); //超音波センサの距離を取得する　引数('l','r','b')を代入するとその方向の値が得られる
00108 bool check_voltage(); //電圧をチェックする
00109 
00110 /*****************************************************************/
00111 /**********************main function******************************/
00112 /*****************************************************************/
00113 
00114 int main()
00115 {
00116 
00117     //while(imu.chip_ready() == 0);//imu set
00118 //**************************************************************//
00119 ////////////////////////initialize setting////////////////////////
00120 //**************************************************************//
00121     wait_ms(50);
00122     kick = 0; //キックを解除する
00123 
00124     /*ultra sonic sensor set speed*/
00125     uss_right.Set_Speed_of_Sound(32); //(cm/ms)
00126     uss_left.Set_Speed_of_Sound(32); //(cm/ms)
00127     uss_back.Set_Speed_of_Sound(32); //(cm/ms)
00128 
00129     /*motor pwm frequency set*/
00130     motor.setPwmPeriod(0.00005);
00131 
00132     /*Variable*/
00133     int rotation, move, distance, degree, direction;
00134     uint8_t config_profile[24];
00135 
00136     motor.omniWheels(0, 0, 0);
00137 
00138     /*gyro sensor */
00139     imu.reset();
00140     wait_ms(100);
00141     imu.change_fusion_mode(MODE);
00142     wait_ms(200);
00143     imu.get_Euler_Angles(&euler_angles);
00144     int init_degree = (int) euler_angles.h;
00145 
00146     while (1) {
00147 //***************************************************************//
00148 ////////////////Play mode////////////
00149 //***************************************************************//
00150         timer_USS.reset();
00151         timer_USS.start();
00152         timer_PID.reset();
00153         timer_PID.start();
00154         timer_volt.reset();
00155         timer_volt.start();
00156         PID(tp, ti, td, init_degree, init_degree, 1); //PIDの積分値をリセットする
00157 
00158         while (sw_start == 0 && check_voltage() == 1) {
00159             imu.get_Euler_Angles(&euler_angles); //ジャイロの値をしゅとくする
00160             rotation = PID(tp, ti, td, init_degree, (int) euler_angles.h); //PIDを計算する
00161             ///////////////////////
00162             ////*ラインの制御*///////
00163             ////////////////////
00164             direction = get_line_degree();
00165             if (direction != 999) {
00166                 int tmp = direction;
00167 
00168                 /*ラインが反応したときコートの中に戻る制御プログラム*/
00169                 while (direction != 999 && sw_start == 0 && check_voltage() == 1) {
00170 
00171                     imu.get_Euler_Angles(&euler_angles);
00172                     rotation = PID(tp, ti, td, init_degree,
00173                                    (int) euler_angles.h, 0);
00174                     tmp = direction;            //踏んだラインの方向を保存
00175                     if (direction == 90) {
00176                         if (get_uss_range('b') >= 50) {
00177                             direction = 240;
00178                         }
00179                     } else if (direction == 270) {
00180                         if (get_uss_range('b') >= 50) {
00181                             direction = 120;
00182                         }
00183                     } else if (direction <= 179) {
00184                         direction = direction + 180;
00185                     } else if (direction >= 180) {
00186                         direction = direction - 180;
00187                     }
00188                     motor.omniWheels(direction, speed, rotation);
00189                     direction = get_line_degree();
00190                 }
00191 
00192                 /*コートの中に戻った後、ボールが移動するまでその場で待機する*/
00193                 if (tmp >= 180) {
00194                     tmp = tmp - 360;
00195                 }
00196                 degree = get_ball_degree();
00197                 while ((degree > (tmp - 45)) && (degree < (tmp + 45))
00198                         && sw_start == 0) {
00199                     imu.get_Euler_Angles(&euler_angles);
00200                     rotation = PID(tp, ti, td, init_degree,
00201                                    (int) euler_angles.h);
00202                     motor.omniWheels(0, 0, rotation);
00203                     degree = get_ball_degree();
00204                 }
00205             }
00206             ////////////////////////////////////
00207             /*ボールを追いかけるときの制御プログラム*///////
00208             ///////////////////////////////////
00209             else {
00210                 int P = (int) euler_angles.h - init_degree; //proportional
00211                 if (P <= -180) { //convert to -180 ~ 180
00212                     P = P + 360;
00213                 }
00214                 if (P >= 180) {
00215                     P = P - 360;
00216                 }
00217                 /*ロボット向き修正*/
00218                 if (P <= -35) {
00219                     motor.setPower(60, 60, 60,60);
00220                 } else if (P >= 35) {
00221                     motor.setPower(-60, -60, -60,-60);
00222                 } else {
00223                     /*ボールの角度と距離を取得する*/
00224                     degree = get_ball_degree();
00225                     distance = get_ball_distance();
00226 
00227                     ////////////////////
00228                     /*ボールが見つからないとき*/
00229                     ////////////////////
00230                     if (distance >= 240) {
00231                         /*
00232                          * GKモードの場合、ゴール前まで戻ってくる
00233                          * FWモードの場合、その場で停止する
00234                          *
00235                          */
00236                         switch (mode_robot) {
00237 
00238                             case GK: { //GKモード
00239                                 int uss_l = get_uss_range('l'), uss_r = get_uss_range('r'); //超音波センサの値を取得する
00240                                 static float Vx = 0, Vy = 0;
00241 
00242                                 Vy = get_uss_range('b') - 45; //y座標を取る
00243                                 if ((uss_l + uss_r) > 120) { //ロボットが他のロボットと干渉していないときx座標を取る
00244                                     Vx = 182 * (float) uss_l
00245                                          / (float) (uss_l + uss_r) - 91;
00246                                 } else {
00247                                     Vx = 0; //x軸上で座標0にいる判定にする
00248                                 }
00249 
00250                                 if (Vy >= 0) { //ゴールの前にいないとき
00251                                     int direction_of_going = (int) ((180.0 / PI)
00252                                                                     * acos(Vy / sqrt(Vx * Vx + Vy * Vy))); //進行方向を計算する
00253                                     if (Vx >= 20) { //右側にいるとき
00254                                         direction_of_going = 180
00255                                                              + direction_of_going;
00256                                         motor.omniWheels(direction_of_going,
00257                                                          speed / 2, rotation); //左後ろに移動
00258                                     } else if (Vx <= -20) { //左側にいるとき
00259                                         direction_of_going = 180
00260                                                              - direction_of_going;
00261                                         motor.omniWheels(direction_of_going,
00262                                                          speed / 2, rotation); //右後ろに移動
00263                                     } else { //真ん中付近にいるとき
00264                                         motor.omniWheels(180, speed / 2, rotation); //後ろへ移動
00265                                     }
00266                                 } else { //ゴールの前にいるとき
00267                                     if (Vx > 30) { //右側にいるとき
00268                                         motor.omniWheels(-90, speed / 2, rotation); //左へ移動
00269                                     } else if (Vx < -30) { //左側にいるとき
00270                                         motor.omniWheels(90, speed / 2, rotation); //右へ移動
00271                                     } else { //真ん中にいるとき
00272                                         motor.omniWheels(0, 0, rotation); //停止
00273                                     }
00274                                 }
00275                                 break;
00276                             }
00277 
00278                             case FW: //FWモード
00279                                 motor.omniWheels(0, 0, rotation); //ボールがないとき停止する
00280                                 break;
00281                             default:
00282                                 motor.omniWheels(0, 0, rotation); //ボールがないとき停止する
00283                                 break;
00284                         }
00285                     }
00286                     /////////////////
00287                     /*ボールが見つかった時*/
00288                     /////////////////
00289                     else {
00290 
00291                         /*ホールドセンサーが反応したとき*/
00292                         static int count = 0; //タイマー代わりのカウンター
00293                         move = turn(degree, distance); //回り込み方向を計算する
00294                         motor.omniWheels(move, speed, rotation);
00295                         if (hold_check.read() == 0) { //0
00296                             if (count > 50) { //ホールドしてから0.1秒過ぎたとき時
00297                                 kick = 1; //キックする
00298                                 wait_ms(50); //キック待機時間
00299                                 kick = 0; //キックを解除する
00300                                 count = 0; //カウンタを0にする
00301                             } else {
00302                                 wait_us(1); //マイクロ秒
00303                                 count++;
00304                             }
00305                         } else {
00306                             count = 0;
00307                         }
00308                     }
00309                 }
00310             }
00311         }
00312         timer_PID.stop();
00313 
00314 //***************************************************************//
00315 ////////////////////////Gyro reset mode////////////////////////////
00316 //***************************************************************//
00317         if (sw_reset == 0) {
00318             imu.get_Euler_Angles(&euler_angles);
00319             init_degree = (int) euler_angles.h;
00320         }
00321         motor.omniWheels(0, 0, 0);
00322 
00323 //***************************************************************//
00324 //////////////////////////debug mode///////////////////////////////
00325 //***************************************************************//
00326         if (pc.readable() > 0) {
00327             char command = pc.getc();
00328             //if 'c' is pressed,calibration mode will start.
00329             if (command == 'c') {
00330                 while (1) {
00331                     if (pc.readable() > 0) {
00332                         if (pc.getc() == 'r') {
00333                             break;
00334                         }
00335                     }
00336                     uint8_t status = 0, temp[3];
00337                     status = imu.read_calib_status();
00338 
00339                     temp[0] = status << 2;
00340                     temp[1] = status << 4;
00341                     temp[2] = status << 6;
00342                     //pc.printf("%d\r\n",status);
00343                     pc.printf("SYS:%d, GYRO:%d, ACC:%d, MAG:%d \r", status >> 6,
00344                               temp[0] >> 6, temp[1] >> 6, temp[2] >> 6);
00345 
00346                     //キャリブレーションプロファイル読みとり
00347                     if (temp[2] >> 6 == 3 && temp[0] >> 6 == 3 && status >> 6 == 3) {
00348                         imu.change_fusion_mode(CONFIGMODE);
00349                         for (int i = 0; i < 22; i++) {
00350                             config_profile[i] = imu.read_reg0(i + 0x55);
00351                         }
00352                         kick = 1;
00353                         wait_ms(200);
00354                         kick = 0;
00355                         break;
00356                     }
00357                     wait_ms(100);
00358                 }
00359                 imu.change_fusion_mode(MODE);
00360             }
00361             //if 'd' is pressed,debug mode will start.
00362             if (command == 'd') {
00363                 while (1) {
00364                     if (pc.readable() > 0) {
00365                         if (pc.getc() == 'r') { //if 'r' is pressed,debug mode will be end.
00366                             break;
00367                         }
00368                     }
00369                     pc.printf(
00370                         "////////////////////////////////////////////\r\n");
00371                     pc.printf(
00372                         "/*******************debug******************/\r\n");
00373                     pc.printf(
00374                         "////////////////////////////////////////////\r\n\n");
00375                     imu.get_Euler_Angles(&euler_angles);
00376                     pc.printf("Gyro   degree: %d \r\n\n", (int) euler_angles.h);
00377                     pc.printf("Ball   degree: %d \r\n", get_ball_degree());
00378                     pc.printf("Ball distance: %d \r\n\n", get_ball_distance());
00379                     pc.printf("Line   sensor: %d \r\n", get_line_degree());
00380                     pc.printf("Hold   sensor: %d \r\n\n", hold_check.read());
00381                     pc.printf("USS      left: %d cm\r\n", get_uss_range('l'));
00382                     pc.printf("USS     right: %d cm\r\n", get_uss_range('r'));
00383                     pc.printf("USS      back: %d cm\r\n\n", get_uss_range('b'));
00384                     sensor.putc('D'); //request Line data
00385                     while (sensor.readable() == 0)
00386                         ;
00387                     int f = sensor.getc();
00388                     while (sensor.readable() == 0)
00389                         ;
00390                     int b = sensor.getc();
00391                     while (sensor.readable() == 0)
00392                         ;
00393                     int r = sensor.getc();
00394                     while (sensor.readable() == 0)
00395                         ;
00396                     int l = sensor.getc();
00397                     pc.printf("Line    front: %d\r\n", f);
00398                     pc.printf("Line     back: %d\r\n", b);
00399                     pc.printf("Line     left: %d\r\n", l);
00400                     pc.printf("Line    right: %d\r\n\n", r);
00401                     pc.printf("batey voltage: %f\r\n", voltage.read() * 10.2);//8.15
00402                     pc.printf("\f");
00403                     wait_ms(300);
00404                 }
00405             }
00406         }
00407 //****************************************************************//
00408 //////////////////////calibration mode/////////////////////////////////////
00409 //****************************************************************//
00410         if (sw_kick.read() != 1) {
00411             uint8_t status = 0, temp[3] = { 0, 0, 0 };
00412             /*ジャイロキャリブレーション*/
00413             while (1) {
00414                 status = imu.read_calib_status();
00415                 temp[0] = status << 2;
00416                 if (temp[0] >> 6 == 3) {
00417                     kick = 1;
00418                     wait_ms(200);
00419                     kick = 0;
00420                     break;
00421                 }
00422             }
00423             /*コンパスキャリブレーション*/
00424             /*(IMUモードの時は必要ないのでコメントアウトして)*/
00425             while (MODE == MODE_NDOF) {
00426                 status = imu.read_calib_status();
00427                 temp[2] = status << 6;
00428                 pc.printf("%d\r",temp[2]>>6);
00429                 if (temp[2]>>6 == 3 && status >>6 == 3) {
00430                     break;
00431                 }
00432             }
00433             /*キャリブレーションプロファイル取得*/
00434             imu.change_fusion_mode(CONFIGMODE);
00435             wait_ms(100);
00436             for (int i = 0; i < 22; i++) {
00437                 config_profile[i] = imu.read_reg0(i + 0x55);
00438             }
00439             imu.change_fusion_mode(MODE);
00440             wait_ms(200);
00441             kick = 1;
00442             wait_ms(200);
00443             kick = 0;
00444         }
00445         /**********************end main function**************************/
00446     }
00447 }
00448 
00449 /////////////////////////////////////////////////////////
00450 /*PID feedback*/
00451 //////////////////////////////////////////////////////////
00452 int PID(double kp, double ki, double kd, int target, int degree, int reset)
00453 {
00454     static double dt, time, pretime;
00455     static double P = 0, OP = 0, I = 0, D = 0, re = 0;
00456 
00457     if (reset == 1) {
00458         I = 0;
00459         time = 0;
00460         pretime = 0;
00461     }
00462 
00463     time = timer_PID.read();
00464     dt = time - pretime; //time
00465     P = degree - target; //proportional
00466     if (P <= -180) { //convert to -180 ~ 180
00467         P = P + 360;
00468     }
00469     if (P >= 180) {
00470         P = P - 360;
00471     }
00472     D = (P - OP) / dt; //differential
00473     I += (P + OP) * dt / 2; //Integral
00474     pretime = time;
00475     OP = P;
00476     re = -1 * (kp * P + ki * I + kd * D);
00477 
00478     if (P >= -5 && P <= 5) {
00479         I = 0;
00480     }
00481     if (re >= 60) {
00482         re = 60;
00483     } else if (re <= -60) {
00484         re = -60;
00485     }
00486 
00487     return re;
00488 }
00489 
00490 /////////////////////////////////////
00491 /*turn control*/
00492 ////////////////////////////////////
00493 int turn(int degree, int distance, int target, int angle)
00494 {
00495     int going;
00496 
00497     angle = angle - target; //proportional
00498     if (angle <= -180) { //convert to -180 ~ 180
00499         angle = angle + 360;
00500     }
00501     if (angle >= 180) {
00502         angle = angle - 360;
00503     }
00504 
00505     degree = degree + angle; //ジャイロの値を加算
00506     if (degree >= -45 && degree <= 45) { //ボールがまえにあるとき
00507         going = 3 * degree;
00508     } else if (degree <= -160 && degree >= 160) { //ボールが真後ろにある時
00509         if (degree >= 0) {
00510             going = degree + 90;
00511         } else {
00512             going = degree - 90;
00513         }
00514     } else {
00515         if (distance <= R) { //ボールとの距離が近すぎるとき
00516             if (degree >= 0) {
00517                 going = degree + 90; //(int)(180.0 - ((180.0/PI)*asin((double)distance / (double)R)));
00518             } else {
00519                 going = degree - 90; //(int)(180.0 - ((180.0/PI)*asin((double)distance / (double)R)));
00520             }
00521         } else { //ボールとの距離が離れているとき
00522             if (degree >= 0) {
00523                 going = degree
00524                         + (int) ((180.0 / PI)
00525                                  * asin((double) R / (double) distance));
00526             } else {
00527                 going = degree
00528                         - (int) ((180.0 / PI)
00529                                  * asin((double) R / (double) distance));
00530             }
00531         }
00532     }
00533     if (get_uss_range('b') <= 60) { //後ろの壁に近いとき
00534         if (going > 90) {
00535             going = 90;
00536         } else if (going < -90) {
00537             going = -90;
00538         }
00539     }
00540     return going - angle;
00541 }
00542 
00543 //////////////////////////////
00544 /*get ball degree*/
00545 //////////////////////////////
00546 int get_ball_degree(int A)
00547 {
00548     static int value = 0;
00549 
00550     if (A == 1) { //analog read
00551         if (ball_distance.read() >= (double) 0.95) {
00552             return 999;
00553         } else {
00554             return (int) (ball_degree.read() * 360 - 185);
00555         }
00556     } else { //serial read
00557 
00558         /*get ball degree*/
00559         int count = 0;
00560         sensor.putc('B');
00561         char buffer[2];
00562         for (int i = 0; i < 2; i++) {
00563             while (sensor.readable() == 0) { //wait
00564                 if (count >= 10000) {
00565                     return value;
00566                 }
00567                 wait_us(1);
00568                 count++;
00569             }
00570             buffer[i] = sensor.getc(); //get
00571         }
00572         value = ((uint16_t) buffer[0] << 8) + (uint8_t) buffer[1] - 180;
00573 
00574         return value;
00575     }
00576 }
00577 
00578 //////////////////////////////
00579 /*get ball distance*/
00580 //////////////////////////////
00581 int get_ball_distance(int A)
00582 {
00583     static int value = 0;
00584     if (A == 1) { //analog read
00585         return (uint8_t) (ball_distance.read() * 255);
00586     } else { //serial read
00587         int count = 0;
00588 
00589         /*get ball distance*/
00590         sensor.putc('C');
00591         while (sensor.readable() == 0) { //wait
00592             if (count >= 10000) {
00593                 return value;
00594             }
00595             wait_us(1);
00596             count++;
00597         }
00598         value = (int) sensor.getc(); //get
00599         return value;
00600     }
00601 }
00602 
00603 //////////////////////////////
00604 /*get line degree*/
00605 //////////////////////////////
00606 int get_line_degree()
00607 {
00608     static char value = 0;
00609     static int degree = 0;
00610     int count = 0;
00611 
00612     /*get line value*/
00613     sensor.putc('A');
00614     while (sensor.readable() == 0) { //wait
00615         if (count >= 10000) {
00616             return degree;
00617         }
00618         wait_us(1);
00619         count++;
00620     }
00621     value = sensor.getc(); //get
00622 
00623     switch (value) {
00624         case 'N':
00625             degree = 999;
00626             return 999;
00627         //break;
00628         case '0':
00629             degree = 0;
00630             return 0;
00631         //break;
00632         case '1':
00633             degree = 45;
00634             return 45;
00635         //break;
00636         case '2':
00637             degree = 90;
00638             return 90;
00639         //break;
00640         case '3':
00641             degree = 135;
00642             return 135;
00643         //break;
00644         case '4':
00645             degree = 180;
00646             return 180;
00647         //break;
00648         case '5':
00649             degree = 225;
00650             return 225;
00651         //break;
00652         case '6':
00653             degree = 270;
00654             return 270;
00655         //break;
00656         case '7':
00657             degree = 315;
00658             return 315;
00659         //break;
00660         default:
00661             degree = 999;
00662             return 999;
00663             //break;
00664     }
00665 }
00666 
00667 //////////////////////////////////////
00668 /*USS reading*/
00669 //////////////////////////////////////
00670 /////////////////////////////
00671 /*get uss range cm*/
00672 ////////////////////////////
00673 int get_uss_range(char c)
00674 {
00675     static int count = 0;
00676     static int l = 0, r = 0, b = 0;
00677 
00678     if (timer_USS.read() >= 0.04) {
00679         l = uss_left.Read_cm();
00680         r = uss_right.Read_cm();
00681         b = uss_back.Read_cm();
00682         uss_left.Send();
00683         uss_right.Send();
00684         uss_back.Send();
00685         timer_USS.reset();
00686     }
00687 
00688     if (c == 'l' || c == 'L') {
00689         return l;
00690     } else if (c == 'r' || c == 'R') {
00691         return r;
00692     } else if (c == 'b' || c == 'B') {
00693         return b;
00694     } else {
00695         return 999;
00696     }
00697 }
00698 
00699 //////////////////////////////////////
00700 /*Battery check voltage*/
00701 //////////////////////////////////////
00702 bool check_voltage()
00703 {
00704     static float sum[5] = { 8, 8, 8, 8, 8 };
00705     static float Ave = 8;
00706     static bool S = 1;
00707     if (timer_volt.read() > 0.2) {
00708         for (int i = 4; i > 0; i--) {
00709             sum[i] = sum[i - 1];
00710         }
00711         sum[0] = voltage.read() * 9.9;
00712         Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5;
00713         if (Ave < 7.0) { //7.0V以下で自動遮断
00714             S = 0;
00715         } else {
00716             S = 1;
00717 
00718         }
00719         timer_volt.reset();
00720     }
00721     return S;
00722 }
00723