ハヤト タジリ / Mbed 2 deprecated Robot_for_Saga_univ_de_lab

佐賀大deラボ機

Dependencies:   mbed MAIDROBO BNO055_fusion Ping

main.cpp@1:0d7cc59d485e, 2020-07-28 (annotated)

Committer:
tajiri1999
Date:
Tue Jul 28 17:11:21 2020 +0000
Revision:
1:0d7cc59d485e
Parent:
0:6c2e6a485519 
commit ; 20200729;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tajiri1999 0:6c2e6a485519 1 #include "mbed.h"
tajiri1999 0:6c2e6a485519 2 #include "BNO055.h"
tajiri1999 0:6c2e6a485519 3 #include "Ping.h"
tajiri1999 0:6c2e6a485519 4 #include "Motor.h"
tajiri1999 0:6c2e6a485519 5
tajiri1999 1:0d7cc59d485e 6 //constant
tajiri1999 1:0d7cc59d485e 7 #define GK 1
tajiri1999 1:0d7cc59d485e 8 #define FW 0
tajiri1999 1:0d7cc59d485e 9 #define PI 3.1415926
tajiri1999 0:6c2e6a485519 10
tajiri1999 1:0d7cc59d485e 11 /*調整するときに読んでね*/
tajiri1999 1:0d7cc59d485e 12 /*ロボットを調整する項目は3つあります
tajiri1999 1:0d7cc59d485e 13 * 以下の調整は試合前に必ずに行ってください
tajiri1999 1:0d7cc59d485e 14 *
tajiri1999 1:0d7cc59d485e 15 * 一つ目はラインセンサの調整です
tajiri1999 1:0d7cc59d485e 16 *　ロボットを起動し、USBケーブルを挿しこんだ後ターミナルを起動して'd'を押してください
tajiri1999 1:0d7cc59d485e 17 *　各種センサ値が表示されます
tajiri1999 1:0d7cc59d485e 18 *　コートの緑のマット上でLine_front ~ Line_rightの数値が
tajiri1999 1:0d7cc59d485e 19 *　75前後　になるように、
tajiri1999 1:0d7cc59d485e 20 *　ロボットの底面についている可変抵抗(青いやつ)を小さいドライバーで回して調整してください
tajiri1999 1:0d7cc59d485e 21 *　可変抵抗を時計回りに回すと数値が上がります
tajiri1999 1:0d7cc59d485e 22 *　キーボード'r'でセンサの値の取得を一時停止することができます
tajiri1999 1:0d7cc59d485e 23 *
tajiri1999 1:0d7cc59d485e 24 *　２つ目は回り込み半径の調整です
tajiri1999 1:0d7cc59d485e 25 * Rを調整することでボールに対してどれくらいの半径を描いて回り込むか調整することができます
tajiri1999 1:0d7cc59d485e 26 * 上記と同様にターミナルソフトを起動してキーボード'd'を押すと
tajiri1999 1:0d7cc59d485e 27 * 現在のセンサの数値を見ることができますのでBall distance:
tajiri1999 1:0d7cc59d485e 28 * の数値を参考にして最適なRを決定してください(distanceの数値=Rにする)
tajiri1999 1:0d7cc59d485e 29 * キーボード'r'でセンサの値の取得を一時停止することができます
tajiri1999 1:0d7cc59d485e 30 *
tajiri1999 1:0d7cc59d485e 31 * ３つ目はスピードとPIDゲイン値の調整です
tajiri1999 1:0d7cc59d485e 32 * まずラインから出ないようにspeedを調整します
tajiri1999 1:0d7cc59d485e 33 * そのあとロボットが常に前に向くように(傾いても元の方向にすぐに戻るように)
tajiri1999 1:0d7cc59d485e 34 * tp ti tdを調整してください
tajiri1999 1:0d7cc59d485e 35 * P→D→I(またはP→I→D)の順番に調整すると良いかもしれません
tajiri1999 1:0d7cc59d485e 36 *
tajiri1999 1:0d7cc59d485e 37 *　以上のことが完了してからプログラムの作成を開始してください
tajiri1999 1:0d7cc59d485e 38 */
tajiri1999 0:6c2e6a485519 39
tajiri1999 1:0d7cc59d485e 40 /*BNO055モードについて
tajiri1999 1:0d7cc59d485e 41 * ジャイロセンサーには様々なモードがありますが
tajiri1999 1:0d7cc59d485e 42 * 基本的にNDOFモードを使って下さい
tajiri1999 1:0d7cc59d485e 43 * 会場の磁場環境がひどい場合はIMUモードを使ってください
tajiri1999 1:0d7cc59d485e 44 *　ロボットの電源を起動した直後にキャリブレーションを行ってください
tajiri1999 1:0d7cc59d485e 45 *　キャリブレーションをすることでオウンゴールを防ぐことができます。
tajiri1999 1:0d7cc59d485e 46 */
tajiri1999 1:0d7cc59d485e 47 /*change Mode IMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF*/
tajiri1999 0:6c2e6a485519 48
tajiri1999 1:0d7cc59d485e 49 //パラメータ調整項目
tajiri1999 1:0d7cc59d485e 50 #define MODE MODE_NDOF//ジャイロのモード
tajiri1999 1:0d7cc59d485e 51 const int mode_robot = FW;//FW:その場で停止 GK:ゴール前までもどる
tajiri1999 1:0d7cc59d485e 52 const int R = 100; //ロボット回り込み半径(0~255
tajiri1999 1:0d7cc59d485e 53 const int speed = 100; //(0~100)の間で調整
tajiri1999 1:0d7cc59d485e 54 const double tp = 1.5; //比例ゲイン
tajiri1999 1:0d7cc59d485e 55 const double ti = 1.5; //積分ゲイン
tajiri1999 1:0d7cc59d485e 56 const double td = 0.25; //微分ゲイン
tajiri1999 0:6c2e6a485519 57
tajiri1999 1:0d7cc59d485e 58 //TerminalDisplay(TeraTerm)
tajiri1999 1:0d7cc59d485e 59 Serial pc(SERIAL_TX, SERIAL_RX);
tajiri1999 0:6c2e6a485519 60
tajiri1999 1:0d7cc59d485e 61 //voltage
tajiri1999 1:0d7cc59d485e 62 AnalogIn voltage(PC_4);
tajiri1999 0:6c2e6a485519 63
tajiri1999 1:0d7cc59d485e 64 //UltraSonicSensor
tajiri1999 1:0d7cc59d485e 65 DigitalIn change(PA_6);
tajiri1999 1:0d7cc59d485e 66 Ping uss_left(PB_3);
tajiri1999 1:0d7cc59d485e 67 Ping uss_right(PA_7);
tajiri1999 1:0d7cc59d485e 68 Ping uss_back(PB_6);
tajiri1999 1:0d7cc59d485e 69 Timer timer_USS;
tajiri1999 1:0d7cc59d485e 70 Timer timer_volt;
tajiri1999 0:6c2e6a485519 71
tajiri1999 1:0d7cc59d485e 72 //BallSensor&Linesensor
tajiri1999 1:0d7cc59d485e 73 AnalogIn ball_degree(PA_4);
tajiri1999 1:0d7cc59d485e 74 AnalogIn ball_distance(PB_0);
tajiri1999 1:0d7cc59d485e 75 Serial sensor(PC_10, PC_11, 115200);
tajiri1999 0:6c2e6a485519 76
tajiri1999 1:0d7cc59d485e 77 //Motor
tajiri1999 1:0d7cc59d485e 78 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);
tajiri1999 1:0d7cc59d485e 79
tajiri1999 1:0d7cc59d485e 80 //Timer
tajiri1999 1:0d7cc59d485e 81 Timer timer_PID;
tajiri1999 0:6c2e6a485519 82
tajiri1999 1:0d7cc59d485e 83 //GyroSensor
tajiri1999 1:0d7cc59d485e 84 I2C i2c(D14, D15);
tajiri1999 1:0d7cc59d485e 85 BNO055 imu(i2c, PC_8);
tajiri1999 0:6c2e6a485519 86 BNO055_ID_INF_TypeDef bno055_id_inf;
tajiri1999 1:0d7cc59d485e 87 BNO055_EULER_TypeDef euler_angles;
tajiri1999 0:6c2e6a485519 88
tajiri1999 1:0d7cc59d485e 89 //HoldCheckSensor
tajiri1999 1:0d7cc59d485e 90 DigitalIn hold_check(PC_3);
tajiri1999 0:6c2e6a485519 91
tajiri1999 1:0d7cc59d485e 92 //kicker
tajiri1999 1:0d7cc59d485e 93 DigitalOut kick(PA_13);
tajiri1999 1:0d7cc59d485e 94
tajiri1999 1:0d7cc59d485e 95 //ToggleSwitch
tajiri1999 1:0d7cc59d485e 96 DigitalIn sw_start(PD_2); //program start switch
tajiri1999 1:0d7cc59d485e 97 DigitalIn sw_reset(PC_12); //gyro sensor reset switch
tajiri1999 1:0d7cc59d485e 98 DigitalIn sw_kick(USER_BUTTON);
tajiri1999 0:6c2e6a485519 99
tajiri1999 1:0d7cc59d485e 100 //declear prototype (function list)
tajiri1999 1:0d7cc59d485e 101 void uss_send_and_read(); //超音波センサ読み込み
tajiri1999 1:0d7cc59d485e 102 int PID(double kp, double ki, double kd, int target, int degree, int reset = 0); //姿勢制御のPIDで計算する
tajiri1999 1:0d7cc59d485e 103 int get_ball_degree(int A = 0); //ボールの角度を-180~180(ボールなし999)で取得する
tajiri1999 1:0d7cc59d485e 104 int get_ball_distance(int A = 0); //ボールの距離を0~255で取得する
tajiri1999 1:0d7cc59d485e 105 int get_line_degree(); //反応したラインの方向(角度)を0~315(反応なし999)で取得する
tajiri1999 1:0d7cc59d485e 106 int turn(int degree, int distance, int target = 0, int angle = 0); //回り込みの進行方向を計算する
tajiri1999 1:0d7cc59d485e 107 int get_uss_range(char c); //超音波センサの距離を取得する　引数('l','r','b')を代入するとその方向の値が得られる
tajiri1999 1:0d7cc59d485e 108 bool check_voltage(); //電圧をチェックする
tajiri1999 0:6c2e6a485519 109
tajiri1999 1:0d7cc59d485e 110 /*****************************************************************/
tajiri1999 1:0d7cc59d485e 111 /**********************main function******************************/
tajiri1999 1:0d7cc59d485e 112 /*****************************************************************/
tajiri1999 0:6c2e6a485519 113
tajiri1999 0:6c2e6a485519 114 int main()
tajiri1999 0:6c2e6a485519 115 {
tajiri1999 0:6c2e6a485519 116
tajiri1999 1:0d7cc59d485e 117 //while(imu.chip_ready() == 0);//imu set
tajiri1999 1:0d7cc59d485e 118 //**************************************************************//
tajiri1999 1:0d7cc59d485e 119 ////////////////////////initialize setting////////////////////////
tajiri1999 1:0d7cc59d485e 120 //**************************************************************//
tajiri1999 1:0d7cc59d485e 121 wait_ms(50);
tajiri1999 1:0d7cc59d485e 122 kick = 0; //キックを解除する
tajiri1999 0:6c2e6a485519 123
tajiri1999 1:0d7cc59d485e 124 /*ultra sonic sensor set speed*/
tajiri1999 1:0d7cc59d485e 125 uss_right.Set_Speed_of_Sound(32); //(cm/ms)
tajiri1999 1:0d7cc59d485e 126 uss_left.Set_Speed_of_Sound(32); //(cm/ms)
tajiri1999 1:0d7cc59d485e 127 uss_back.Set_Speed_of_Sound(32); //(cm/ms)
tajiri1999 1:0d7cc59d485e 128
tajiri1999 1:0d7cc59d485e 129 /*motor pwm frequency set*/
tajiri1999 1:0d7cc59d485e 130 motor.setPwmPeriod(0.00005);
tajiri1999 0:6c2e6a485519 131
tajiri1999 1:0d7cc59d485e 132 /*Variable*/
tajiri1999 1:0d7cc59d485e 133 int rotation, move, distance, degree, direction;
tajiri1999 1:0d7cc59d485e 134 uint8_t config_profile[24];
tajiri1999 1:0d7cc59d485e 135
tajiri1999 1:0d7cc59d485e 136 motor.omniWheels(0, 0, 0);
tajiri1999 1:0d7cc59d485e 137
tajiri1999 1:0d7cc59d485e 138 /*gyro sensor */
tajiri1999 0:6c2e6a485519 139 imu.reset();
tajiri1999 1:0d7cc59d485e 140 wait_ms(100);
tajiri1999 1:0d7cc59d485e 141 imu.change_fusion_mode(MODE);
tajiri1999 1:0d7cc59d485e 142 wait_ms(200);
tajiri1999 0:6c2e6a485519 143 imu.get_Euler_Angles(&euler_angles);
tajiri1999 1:0d7cc59d485e 144 int init_degree = (int) euler_angles.h;
tajiri1999 0:6c2e6a485519 145
tajiri1999 1:0d7cc59d485e 146 while (1) {
tajiri1999 1:0d7cc59d485e 147 //***************************************************************//
tajiri1999 1:0d7cc59d485e 148 ////////////////Play mode////////////
tajiri1999 1:0d7cc59d485e 149 //***************************************************************//
tajiri1999 1:0d7cc59d485e 150 timer_USS.reset();
tajiri1999 1:0d7cc59d485e 151 timer_USS.start();
tajiri1999 1:0d7cc59d485e 152 timer_PID.reset();
tajiri1999 1:0d7cc59d485e 153 timer_PID.start();
tajiri1999 1:0d7cc59d485e 154 timer_volt.reset();
tajiri1999 1:0d7cc59d485e 155 timer_volt.start();
tajiri1999 1:0d7cc59d485e 156 PID(tp, ti, td, init_degree, init_degree, 1); //PIDの積分値をリセットする
tajiri1999 0:6c2e6a485519 157
tajiri1999 1:0d7cc59d485e 158 while (sw_start == 0 && check_voltage() == 1) {
tajiri1999 1:0d7cc59d485e 159 imu.get_Euler_Angles(&euler_angles); //ジャイロの値をしゅとくする
tajiri1999 1:0d7cc59d485e 160 rotation = PID(tp, ti, td, init_degree, (int) euler_angles.h); //PIDを計算する
tajiri1999 1:0d7cc59d485e 161 ///////////////////////
tajiri1999 1:0d7cc59d485e 162 ////*ラインの制御*///////
tajiri1999 1:0d7cc59d485e 163 ////////////////////
tajiri1999 1:0d7cc59d485e 164 direction = get_line_degree();
tajiri1999 1:0d7cc59d485e 165 if (direction != 999) {
tajiri1999 1:0d7cc59d485e 166 int tmp = direction;
tajiri1999 1:0d7cc59d485e 167
tajiri1999 1:0d7cc59d485e 168 /*ラインが反応したときコートの中に戻る制御プログラム*/
tajiri1999 1:0d7cc59d485e 169 while (direction != 999 && sw_start == 0 && check_voltage() == 1) {
tajiri1999 1:0d7cc59d485e 170
tajiri1999 1:0d7cc59d485e 171 imu.get_Euler_Angles(&euler_angles);
tajiri1999 1:0d7cc59d485e 172 rotation = PID(tp, ti, td, init_degree,
tajiri1999 1:0d7cc59d485e 173 (int) euler_angles.h, 0);
tajiri1999 1:0d7cc59d485e 174 tmp = direction; //踏んだラインの方向を保存
tajiri1999 1:0d7cc59d485e 175 if (direction == 90) {
tajiri1999 1:0d7cc59d485e 176 if (get_uss_range('b') >= 50) {
tajiri1999 1:0d7cc59d485e 177 direction = 240;
tajiri1999 1:0d7cc59d485e 178 }
tajiri1999 1:0d7cc59d485e 179 } else if (direction == 270) {
tajiri1999 1:0d7cc59d485e 180 if (get_uss_range('b') >= 50) {
tajiri1999 1:0d7cc59d485e 181 direction = 120;
tajiri1999 1:0d7cc59d485e 182 }
tajiri1999 1:0d7cc59d485e 183 } else if (direction <= 179) {
tajiri1999 1:0d7cc59d485e 184 direction = direction + 180;
tajiri1999 1:0d7cc59d485e 185 } else if (direction >= 180) {
tajiri1999 1:0d7cc59d485e 186 direction = direction - 180;
tajiri1999 0:6c2e6a485519 187 }
tajiri1999 1:0d7cc59d485e 188 motor.omniWheels(direction, speed, rotation);
tajiri1999 1:0d7cc59d485e 189 direction = get_line_degree();
tajiri1999 0:6c2e6a485519 190 }
tajiri1999 1:0d7cc59d485e 191
tajiri1999 1:0d7cc59d485e 192 /*コートの中に戻った後、ボールが移動するまでその場で待機する*/
tajiri1999 1:0d7cc59d485e 193 if (tmp >= 180) {
tajiri1999 1:0d7cc59d485e 194 tmp = tmp - 360;
tajiri1999 0:6c2e6a485519 195 }
tajiri1999 1:0d7cc59d485e 196 degree = get_ball_degree();
tajiri1999 1:0d7cc59d485e 197 while ((degree > (tmp - 45)) && (degree < (tmp + 45))
tajiri1999 1:0d7cc59d485e 198 && sw_start == 0) {
tajiri1999 1:0d7cc59d485e 199 imu.get_Euler_Angles(&euler_angles);
tajiri1999 1:0d7cc59d485e 200 rotation = PID(tp, ti, td, init_degree,
tajiri1999 1:0d7cc59d485e 201 (int) euler_angles.h);
tajiri1999 1:0d7cc59d485e 202 motor.omniWheels(0, 0, rotation);
tajiri1999 1:0d7cc59d485e 203 degree = get_ball_degree();
tajiri1999 0:6c2e6a485519 204 }
tajiri1999 0:6c2e6a485519 205 }
tajiri1999 1:0d7cc59d485e 206 ////////////////////////////////////
tajiri1999 1:0d7cc59d485e 207 /*ボールを追いかけるときの制御プログラム*///////
tajiri1999 1:0d7cc59d485e 208 ///////////////////////////////////
tajiri1999 1:0d7cc59d485e 209 else {
tajiri1999 1:0d7cc59d485e 210 int P = (int) euler_angles.h - init_degree; //proportional
tajiri1999 1:0d7cc59d485e 211 if (P <= -180) { //convert to -180 ~ 180
tajiri1999 1:0d7cc59d485e 212 P = P + 360;
tajiri1999 1:0d7cc59d485e 213 }
tajiri1999 1:0d7cc59d485e 214 if (P >= 180) {
tajiri1999 1:0d7cc59d485e 215 P = P - 360;
tajiri1999 1:0d7cc59d485e 216 }
tajiri1999 1:0d7cc59d485e 217 /*ロボット向き修正*/
tajiri1999 1:0d7cc59d485e 218 if (P <= -35) {
tajiri1999 1:0d7cc59d485e 219 motor.setPower(60, 60, 60,60);
tajiri1999 1:0d7cc59d485e 220 } else if (P >= 35) {
tajiri1999 1:0d7cc59d485e 221 motor.setPower(-60, -60, -60,-60);
tajiri1999 1:0d7cc59d485e 222 } else {
tajiri1999 1:0d7cc59d485e 223 /*ボールの角度と距離を取得する*/
tajiri1999 1:0d7cc59d485e 224 degree = get_ball_degree();
tajiri1999 1:0d7cc59d485e 225 distance = get_ball_distance();
tajiri1999 0:6c2e6a485519 226
tajiri1999 1:0d7cc59d485e 227 ////////////////////
tajiri1999 1:0d7cc59d485e 228 /*ボールが見つからないとき*/
tajiri1999 1:0d7cc59d485e 229 ////////////////////
tajiri1999 1:0d7cc59d485e 230 if (distance >= 240) {
tajiri1999 1:0d7cc59d485e 231 /*
tajiri1999 1:0d7cc59d485e 232 * GKモードの場合、ゴール前まで戻ってくる
tajiri1999 1:0d7cc59d485e 233 * FWモードの場合、その場で停止する
tajiri1999 1:0d7cc59d485e 234 *
tajiri1999 1:0d7cc59d485e 235 */
tajiri1999 1:0d7cc59d485e 236 switch (mode_robot) {
tajiri1999 0:6c2e6a485519 237
tajiri1999 1:0d7cc59d485e 238 case GK: { //GKモード
tajiri1999 1:0d7cc59d485e 239 int uss_l = get_uss_range('l'), uss_r = get_uss_range('r'); //超音波センサの値を取得する
tajiri1999 1:0d7cc59d485e 240 static float Vx = 0, Vy = 0;
tajiri1999 0:6c2e6a485519 241
tajiri1999 1:0d7cc59d485e 242 Vy = get_uss_range('b') - 45; //y座標を取る
tajiri1999 1:0d7cc59d485e 243 if ((uss_l + uss_r) > 120) { //ロボットが他のロボットと干渉していないときx座標を取る
tajiri1999 1:0d7cc59d485e 244 Vx = 182 * (float) uss_l
tajiri1999 1:0d7cc59d485e 245 / (float) (uss_l + uss_r) - 91;
tajiri1999 1:0d7cc59d485e 246 } else {
tajiri1999 1:0d7cc59d485e 247 Vx = 0; //x軸上で座標0にいる判定にする
tajiri1999 1:0d7cc59d485e 248 }
tajiri1999 0:6c2e6a485519 249
tajiri1999 1:0d7cc59d485e 250 if (Vy >= 0) { //ゴールの前にいないとき
tajiri1999 1:0d7cc59d485e 251 int direction_of_going = (int) ((180.0 / PI)
tajiri1999 1:0d7cc59d485e 252 * acos(Vy / sqrt(Vx * Vx + Vy * Vy))); //進行方向を計算する
tajiri1999 1:0d7cc59d485e 253 if (Vx >= 20) { //右側にいるとき
tajiri1999 1:0d7cc59d485e 254 direction_of_going = 180
tajiri1999 1:0d7cc59d485e 255 + direction_of_going;
tajiri1999 1:0d7cc59d485e 256 motor.omniWheels(direction_of_going,
tajiri1999 1:0d7cc59d485e 257 speed / 2, rotation); //左後ろに移動
tajiri1999 1:0d7cc59d485e 258 } else if (Vx <= -20) { //左側にいるとき
tajiri1999 1:0d7cc59d485e 259 direction_of_going = 180
tajiri1999 1:0d7cc59d485e 260 - direction_of_going;
tajiri1999 1:0d7cc59d485e 261 motor.omniWheels(direction_of_going,
tajiri1999 1:0d7cc59d485e 262 speed / 2, rotation); //右後ろに移動
tajiri1999 1:0d7cc59d485e 263 } else { //真ん中付近にいるとき
tajiri1999 1:0d7cc59d485e 264 motor.omniWheels(180, speed / 2, rotation); //後ろへ移動
tajiri1999 1:0d7cc59d485e 265 }
tajiri1999 1:0d7cc59d485e 266 } else { //ゴールの前にいるとき
tajiri1999 1:0d7cc59d485e 267 if (Vx > 30) { //右側にいるとき
tajiri1999 1:0d7cc59d485e 268 motor.omniWheels(-90, speed / 2, rotation); //左へ移動
tajiri1999 1:0d7cc59d485e 269 } else if (Vx < -30) { //左側にいるとき
tajiri1999 1:0d7cc59d485e 270 motor.omniWheels(90, speed / 2, rotation); //右へ移動
tajiri1999 1:0d7cc59d485e 271 } else { //真ん中にいるとき
tajiri1999 1:0d7cc59d485e 272 motor.omniWheels(0, 0, rotation); //停止
tajiri1999 1:0d7cc59d485e 273 }
tajiri1999 1:0d7cc59d485e 274 }
tajiri1999 1:0d7cc59d485e 275 break;
tajiri1999 1:0d7cc59d485e 276 }
tajiri1999 0:6c2e6a485519 277
tajiri1999 1:0d7cc59d485e 278 case FW: //FWモード
tajiri1999 1:0d7cc59d485e 279 motor.omniWheels(0, 0, rotation); //ボールがないとき停止する
tajiri1999 1:0d7cc59d485e 280 break;
tajiri1999 1:0d7cc59d485e 281 default:
tajiri1999 1:0d7cc59d485e 282 motor.omniWheels(0, 0, rotation); //ボールがないとき停止する
tajiri1999 1:0d7cc59d485e 283 break;
tajiri1999 1:0d7cc59d485e 284 }
tajiri1999 1:0d7cc59d485e 285 }
tajiri1999 1:0d7cc59d485e 286 /////////////////
tajiri1999 1:0d7cc59d485e 287 /*ボールが見つかった時*/
tajiri1999 1:0d7cc59d485e 288 /////////////////
tajiri1999 1:0d7cc59d485e 289 else {
tajiri1999 1:0d7cc59d485e 290
tajiri1999 1:0d7cc59d485e 291 /*ホールドセンサーが反応したとき*/
tajiri1999 1:0d7cc59d485e 292 static int count = 0; //タイマー代わりのカウンター
tajiri1999 1:0d7cc59d485e 293 move = turn(degree, distance); //回り込み方向を計算する
tajiri1999 1:0d7cc59d485e 294 motor.omniWheels(move, speed, rotation);
tajiri1999 1:0d7cc59d485e 295 if (hold_check.read() == 0) { //0
tajiri1999 1:0d7cc59d485e 296 if (count > 50) { //ホールドしてから0.1秒過ぎたとき時
tajiri1999 1:0d7cc59d485e 297 kick = 1; //キックする
tajiri1999 1:0d7cc59d485e 298 wait_ms(50); //キック待機時間
tajiri1999 1:0d7cc59d485e 299 kick = 0; //キックを解除する
tajiri1999 1:0d7cc59d485e 300 count = 0; //カウンタを0にする
tajiri1999 1:0d7cc59d485e 301 } else {
tajiri1999 1:0d7cc59d485e 302 wait_us(1); //マイクロ秒
tajiri1999 1:0d7cc59d485e 303 count++;
tajiri1999 1:0d7cc59d485e 304 }
tajiri1999 1:0d7cc59d485e 305 } else {
tajiri1999 1:0d7cc59d485e 306 count = 0;
tajiri1999 1:0d7cc59d485e 307 }
tajiri1999 1:0d7cc59d485e 308 }
tajiri1999 1:0d7cc59d485e 309 }
tajiri1999 0:6c2e6a485519 310 }
tajiri1999 0:6c2e6a485519 311 }
tajiri1999 1:0d7cc59d485e 312 timer_PID.stop();
tajiri1999 0:6c2e6a485519 313
tajiri1999 1:0d7cc59d485e 314 //***************************************************************//
tajiri1999 1:0d7cc59d485e 315 ////////////////////////Gyro reset mode////////////////////////////
tajiri1999 1:0d7cc59d485e 316 //***************************************************************//
tajiri1999 1:0d7cc59d485e 317 if (sw_reset == 0) {
tajiri1999 1:0d7cc59d485e 318 imu.get_Euler_Angles(&euler_angles);
tajiri1999 1:0d7cc59d485e 319 init_degree = (int) euler_angles.h;
tajiri1999 1:0d7cc59d485e 320 }
tajiri1999 1:0d7cc59d485e 321 motor.omniWheels(0, 0, 0);
tajiri1999 0:6c2e6a485519 322
tajiri1999 1:0d7cc59d485e 323 //***************************************************************//
tajiri1999 1:0d7cc59d485e 324 //////////////////////////debug mode///////////////////////////////
tajiri1999 1:0d7cc59d485e 325 //***************************************************************//
tajiri1999 1:0d7cc59d485e 326 if (pc.readable() > 0) {
tajiri1999 1:0d7cc59d485e 327 char command = pc.getc();
tajiri1999 1:0d7cc59d485e 328 //if 'c' is pressed,calibration mode will start.
tajiri1999 1:0d7cc59d485e 329 if (command == 'c') {
tajiri1999 1:0d7cc59d485e 330 while (1) {
tajiri1999 1:0d7cc59d485e 331 if (pc.readable() > 0) {
tajiri1999 1:0d7cc59d485e 332 if (pc.getc() == 'r') {
tajiri1999 1:0d7cc59d485e 333 break;
tajiri1999 1:0d7cc59d485e 334 }
tajiri1999 1:0d7cc59d485e 335 }
tajiri1999 1:0d7cc59d485e 336 uint8_t status = 0, temp[3];
tajiri1999 1:0d7cc59d485e 337 status = imu.read_calib_status();
tajiri1999 0:6c2e6a485519 338
tajiri1999 1:0d7cc59d485e 339 temp[0] = status << 2;
tajiri1999 1:0d7cc59d485e 340 temp[1] = status << 4;
tajiri1999 1:0d7cc59d485e 341 temp[2] = status << 6;
tajiri1999 1:0d7cc59d485e 342 //pc.printf("%d\r\n",status);
tajiri1999 1:0d7cc59d485e 343 pc.printf("SYS:%d, GYRO:%d, ACC:%d, MAG:%d \r", status >> 6,
tajiri1999 1:0d7cc59d485e 344 temp[0] >> 6, temp[1] >> 6, temp[2] >> 6);
tajiri1999 0:6c2e6a485519 345
tajiri1999 1:0d7cc59d485e 346 //キャリブレーションプロファイル読みとり
tajiri1999 1:0d7cc59d485e 347 if (temp[2] >> 6 == 3 && temp[0] >> 6 == 3 && status >> 6 == 3) {
tajiri1999 1:0d7cc59d485e 348 imu.change_fusion_mode(CONFIGMODE);
tajiri1999 1:0d7cc59d485e 349 for (int i = 0; i < 22; i++) {
tajiri1999 1:0d7cc59d485e 350 config_profile[i] = imu.read_reg0(i + 0x55);
tajiri1999 1:0d7cc59d485e 351 }
tajiri1999 1:0d7cc59d485e 352 kick = 1;
tajiri1999 1:0d7cc59d485e 353 wait_ms(200);
tajiri1999 1:0d7cc59d485e 354 kick = 0;
tajiri1999 1:0d7cc59d485e 355 break;
tajiri1999 1:0d7cc59d485e 356 }
tajiri1999 1:0d7cc59d485e 357 wait_ms(100);
tajiri1999 1:0d7cc59d485e 358 }
tajiri1999 1:0d7cc59d485e 359 imu.change_fusion_mode(MODE);
tajiri1999 1:0d7cc59d485e 360 }
tajiri1999 1:0d7cc59d485e 361 //if 'd' is pressed,debug mode will start.
tajiri1999 1:0d7cc59d485e 362 if (command == 'd') {
tajiri1999 1:0d7cc59d485e 363 while (1) {
tajiri1999 1:0d7cc59d485e 364 if (pc.readable() > 0) {
tajiri1999 1:0d7cc59d485e 365 if (pc.getc() == 'r') { //if 'r' is pressed,debug mode will be end.
tajiri1999 1:0d7cc59d485e 366 break;
tajiri1999 1:0d7cc59d485e 367 }
tajiri1999 1:0d7cc59d485e 368 }
tajiri1999 1:0d7cc59d485e 369 pc.printf(
tajiri1999 1:0d7cc59d485e 370 "////////////////////////////////////////////\r\n");
tajiri1999 1:0d7cc59d485e 371 pc.printf(
tajiri1999 1:0d7cc59d485e 372 "/*******************debug******************/\r\n");
tajiri1999 1:0d7cc59d485e 373 pc.printf(
tajiri1999 1:0d7cc59d485e 374 "////////////////////////////////////////////\r\n\n");
tajiri1999 1:0d7cc59d485e 375 imu.get_Euler_Angles(&euler_angles);
tajiri1999 1:0d7cc59d485e 376 pc.printf("Gyro degree: %d \r\n\n", (int) euler_angles.h);
tajiri1999 1:0d7cc59d485e 377 pc.printf("Ball degree: %d \r\n", get_ball_degree());
tajiri1999 1:0d7cc59d485e 378 pc.printf("Ball distance: %d \r\n\n", get_ball_distance());
tajiri1999 1:0d7cc59d485e 379 pc.printf("Line sensor: %d \r\n", get_line_degree());
tajiri1999 1:0d7cc59d485e 380 pc.printf("Hold sensor: %d \r\n\n", hold_check.read());
tajiri1999 1:0d7cc59d485e 381 pc.printf("USS left: %d cm\r\n", get_uss_range('l'));
tajiri1999 1:0d7cc59d485e 382 pc.printf("USS right: %d cm\r\n", get_uss_range('r'));
tajiri1999 1:0d7cc59d485e 383 pc.printf("USS back: %d cm\r\n\n", get_uss_range('b'));
tajiri1999 1:0d7cc59d485e 384 sensor.putc('D'); //request Line data
tajiri1999 1:0d7cc59d485e 385 while (sensor.readable() == 0)
tajiri1999 1:0d7cc59d485e 386 ;
tajiri1999 1:0d7cc59d485e 387 int f = sensor.getc();
tajiri1999 1:0d7cc59d485e 388 while (sensor.readable() == 0)
tajiri1999 1:0d7cc59d485e 389 ;
tajiri1999 1:0d7cc59d485e 390 int b = sensor.getc();
tajiri1999 1:0d7cc59d485e 391 while (sensor.readable() == 0)
tajiri1999 1:0d7cc59d485e 392 ;
tajiri1999 1:0d7cc59d485e 393 int r = sensor.getc();
tajiri1999 1:0d7cc59d485e 394 while (sensor.readable() == 0)
tajiri1999 1:0d7cc59d485e 395 ;
tajiri1999 1:0d7cc59d485e 396 int l = sensor.getc();
tajiri1999 1:0d7cc59d485e 397 pc.printf("Line front: %d\r\n", f);
tajiri1999 1:0d7cc59d485e 398 pc.printf("Line back: %d\r\n", b);
tajiri1999 1:0d7cc59d485e 399 pc.printf("Line left: %d\r\n", l);
tajiri1999 1:0d7cc59d485e 400 pc.printf("Line right: %d\r\n\n", r);
tajiri1999 1:0d7cc59d485e 401 pc.printf("batey voltage: %f\r\n", voltage.read() * 10.2);//8.15
tajiri1999 1:0d7cc59d485e 402 pc.printf("\f");
tajiri1999 1:0d7cc59d485e 403 wait_ms(300);
tajiri1999 1:0d7cc59d485e 404 }
tajiri1999 1:0d7cc59d485e 405 }
tajiri1999 0:6c2e6a485519 406 }
tajiri1999 1:0d7cc59d485e 407 //****************************************************************//
tajiri1999 1:0d7cc59d485e 408 //////////////////////calibration mode/////////////////////////////////////
tajiri1999 1:0d7cc59d485e 409 //****************************************************************//
tajiri1999 1:0d7cc59d485e 410 if (sw_kick.read() != 1) {
tajiri1999 1:0d7cc59d485e 411 uint8_t status = 0, temp[3] = { 0, 0, 0 };
tajiri1999 1:0d7cc59d485e 412 /*ジャイロキャリブレーション*/
tajiri1999 1:0d7cc59d485e 413 while (1) {
tajiri1999 1:0d7cc59d485e 414 status = imu.read_calib_status();
tajiri1999 1:0d7cc59d485e 415 temp[0] = status << 2;
tajiri1999 1:0d7cc59d485e 416 if (temp[0] >> 6 == 3) {
tajiri1999 1:0d7cc59d485e 417 kick = 1;
tajiri1999 1:0d7cc59d485e 418 wait_ms(200);
tajiri1999 1:0d7cc59d485e 419 kick = 0;
tajiri1999 1:0d7cc59d485e 420 break;
tajiri1999 1:0d7cc59d485e 421 }
tajiri1999 1:0d7cc59d485e 422 }
tajiri1999 1:0d7cc59d485e 423 /*コンパスキャリブレーション*/
tajiri1999 1:0d7cc59d485e 424 /*(IMUモードの時は必要ないのでコメントアウトして)*/
tajiri1999 1:0d7cc59d485e 425 while (MODE == MODE_NDOF) {
tajiri1999 1:0d7cc59d485e 426 status = imu.read_calib_status();
tajiri1999 1:0d7cc59d485e 427 temp[2] = status << 6;
tajiri1999 1:0d7cc59d485e 428 pc.printf("%d\r",temp[2]>>6);
tajiri1999 1:0d7cc59d485e 429 if (temp[2]>>6 == 3 && status >>6 == 3) {
tajiri1999 1:0d7cc59d485e 430 break;
tajiri1999 1:0d7cc59d485e 431 }
tajiri1999 1:0d7cc59d485e 432 }
tajiri1999 1:0d7cc59d485e 433 /*キャリブレーションプロファイル取得*/
tajiri1999 1:0d7cc59d485e 434 imu.change_fusion_mode(CONFIGMODE);
tajiri1999 1:0d7cc59d485e 435 wait_ms(100);
tajiri1999 1:0d7cc59d485e 436 for (int i = 0; i < 22; i++) {
tajiri1999 1:0d7cc59d485e 437 config_profile[i] = imu.read_reg0(i + 0x55);
tajiri1999 1:0d7cc59d485e 438 }
tajiri1999 1:0d7cc59d485e 439 imu.change_fusion_mode(MODE);
tajiri1999 1:0d7cc59d485e 440 wait_ms(200);
tajiri1999 1:0d7cc59d485e 441 kick = 1;
tajiri1999 1:0d7cc59d485e 442 wait_ms(200);
tajiri1999 1:0d7cc59d485e 443 kick = 0;
tajiri1999 1:0d7cc59d485e 444 }
tajiri1999 1:0d7cc59d485e 445 /**********************end main function**************************/
tajiri1999 0:6c2e6a485519 446 }
tajiri1999 0:6c2e6a485519 447 }
tajiri1999 0:6c2e6a485519 448
tajiri1999 1:0d7cc59d485e 449 /////////////////////////////////////////////////////////
tajiri1999 1:0d7cc59d485e 450 /*PID feedback*/
tajiri1999 1:0d7cc59d485e 451 //////////////////////////////////////////////////////////
tajiri1999 1:0d7cc59d485e 452 int PID(double kp, double ki, double kd, int target, int degree, int reset)
tajiri1999 0:6c2e6a485519 453 {
tajiri1999 1:0d7cc59d485e 454 static double dt, time, pretime;
tajiri1999 1:0d7cc59d485e 455 static double P = 0, OP = 0, I = 0, D = 0, re = 0;
tajiri1999 0:6c2e6a485519 456
tajiri1999 1:0d7cc59d485e 457 if (reset == 1) {
tajiri1999 1:0d7cc59d485e 458 I = 0;
tajiri1999 1:0d7cc59d485e 459 time = 0;
tajiri1999 1:0d7cc59d485e 460 pretime = 0;
tajiri1999 0:6c2e6a485519 461 }
tajiri1999 0:6c2e6a485519 462
tajiri1999 1:0d7cc59d485e 463 time = timer_PID.read();
tajiri1999 1:0d7cc59d485e 464 dt = time - pretime; //time
tajiri1999 1:0d7cc59d485e 465 P = degree - target; //proportional
tajiri1999 1:0d7cc59d485e 466 if (P <= -180) { //convert to -180 ~ 180
tajiri1999 1:0d7cc59d485e 467 P = P + 360;
tajiri1999 1:0d7cc59d485e 468 }
tajiri1999 1:0d7cc59d485e 469 if (P >= 180) {
tajiri1999 1:0d7cc59d485e 470 P = P - 360;
tajiri1999 1:0d7cc59d485e 471 }
tajiri1999 1:0d7cc59d485e 472 D = (P - OP) / dt; //differential
tajiri1999 1:0d7cc59d485e 473 I += (P + OP) * dt / 2; //Integral
tajiri1999 1:0d7cc59d485e 474 pretime = time;
tajiri1999 1:0d7cc59d485e 475 OP = P;
tajiri1999 1:0d7cc59d485e 476 re = -1 * (kp * P + ki * I + kd * D);
tajiri1999 0:6c2e6a485519 477
tajiri1999 1:0d7cc59d485e 478 if (P >= -5 && P <= 5) {
tajiri1999 1:0d7cc59d485e 479 I = 0;
tajiri1999 0:6c2e6a485519 480 }
tajiri1999 1:0d7cc59d485e 481 if (re >= 60) {
tajiri1999 1:0d7cc59d485e 482 re = 60;
tajiri1999 1:0d7cc59d485e 483 } else if (re <= -60) {
tajiri1999 1:0d7cc59d485e 484 re = -60;
tajiri1999 1:0d7cc59d485e 485 }
tajiri1999 1:0d7cc59d485e 486
tajiri1999 1:0d7cc59d485e 487 return re;
tajiri1999 0:6c2e6a485519 488 }
tajiri1999 0:6c2e6a485519 489
tajiri1999 1:0d7cc59d485e 490 /////////////////////////////////////
tajiri1999 1:0d7cc59d485e 491 /*turn control*/
tajiri1999 1:0d7cc59d485e 492 ////////////////////////////////////
tajiri1999 1:0d7cc59d485e 493 int turn(int degree, int distance, int target, int angle)
tajiri1999 0:6c2e6a485519 494 {
tajiri1999 1:0d7cc59d485e 495 int going;
tajiri1999 0:6c2e6a485519 496
tajiri1999 1:0d7cc59d485e 497 angle = angle - target; //proportional
tajiri1999 1:0d7cc59d485e 498 if (angle <= -180) { //convert to -180 ~ 180
tajiri1999 1:0d7cc59d485e 499 angle = angle + 360;
tajiri1999 1:0d7cc59d485e 500 }
tajiri1999 1:0d7cc59d485e 501 if (angle >= 180) {
tajiri1999 1:0d7cc59d485e 502 angle = angle - 360;
tajiri1999 1:0d7cc59d485e 503 }
tajiri1999 0:6c2e6a485519 504
tajiri1999 1:0d7cc59d485e 505 degree = degree + angle; //ジャイロの値を加算
tajiri1999 1:0d7cc59d485e 506 if (degree >= -45 && degree <= 45) { //ボールがまえにあるとき
tajiri1999 1:0d7cc59d485e 507 going = 3 * degree;
tajiri1999 1:0d7cc59d485e 508 } else if (degree <= -160 && degree >= 160) { //ボールが真後ろにある時
tajiri1999 1:0d7cc59d485e 509 if (degree >= 0) {
tajiri1999 1:0d7cc59d485e 510 going = degree + 90;
tajiri1999 1:0d7cc59d485e 511 } else {
tajiri1999 1:0d7cc59d485e 512 going = degree - 90;
tajiri1999 1:0d7cc59d485e 513 }
tajiri1999 1:0d7cc59d485e 514 } else {
tajiri1999 1:0d7cc59d485e 515 if (distance <= R) { //ボールとの距離が近すぎるとき
tajiri1999 1:0d7cc59d485e 516 if (degree >= 0) {
tajiri1999 1:0d7cc59d485e 517 going = degree + 90; //(int)(180.0 - ((180.0/PI)*asin((double)distance / (double)R)));
tajiri1999 1:0d7cc59d485e 518 } else {
tajiri1999 1:0d7cc59d485e 519 going = degree - 90; //(int)(180.0 - ((180.0/PI)*asin((double)distance / (double)R)));
tajiri1999 1:0d7cc59d485e 520 }
tajiri1999 1:0d7cc59d485e 521 } else { //ボールとの距離が離れているとき
tajiri1999 1:0d7cc59d485e 522 if (degree >= 0) {
tajiri1999 1:0d7cc59d485e 523 going = degree
tajiri1999 1:0d7cc59d485e 524 + (int) ((180.0 / PI)
tajiri1999 1:0d7cc59d485e 525 * asin((double) R / (double) distance));
tajiri1999 1:0d7cc59d485e 526 } else {
tajiri1999 1:0d7cc59d485e 527 going = degree
tajiri1999 1:0d7cc59d485e 528 - (int) ((180.0 / PI)
tajiri1999 1:0d7cc59d485e 529 * asin((double) R / (double) distance));
tajiri1999 1:0d7cc59d485e 530 }
tajiri1999 1:0d7cc59d485e 531 }
tajiri1999 1:0d7cc59d485e 532 }
tajiri1999 1:0d7cc59d485e 533 if (get_uss_range('b') <= 60) { //後ろの壁に近いとき
tajiri1999 1:0d7cc59d485e 534 if (going > 90) {
tajiri1999 1:0d7cc59d485e 535 going = 90;
tajiri1999 1:0d7cc59d485e 536 } else if (going < -90) {
tajiri1999 1:0d7cc59d485e 537 going = -90;
tajiri1999 1:0d7cc59d485e 538 }
tajiri1999 1:0d7cc59d485e 539 }
tajiri1999 1:0d7cc59d485e 540 return going - angle;
tajiri1999 1:0d7cc59d485e 541 }
tajiri1999 0:6c2e6a485519 542
tajiri1999 1:0d7cc59d485e 543 //////////////////////////////
tajiri1999 1:0d7cc59d485e 544 /*get ball degree*/
tajiri1999 1:0d7cc59d485e 545 //////////////////////////////
tajiri1999 1:0d7cc59d485e 546 int get_ball_degree(int A)
tajiri1999 1:0d7cc59d485e 547 {
tajiri1999 1:0d7cc59d485e 548 static int value = 0;
tajiri1999 0:6c2e6a485519 549
tajiri1999 1:0d7cc59d485e 550 if (A == 1) { //analog read
tajiri1999 1:0d7cc59d485e 551 if (ball_distance.read() >= (double) 0.95) {
tajiri1999 1:0d7cc59d485e 552 return 999;
tajiri1999 1:0d7cc59d485e 553 } else {
tajiri1999 1:0d7cc59d485e 554 return (int) (ball_degree.read() * 360 - 185);
tajiri1999 1:0d7cc59d485e 555 }
tajiri1999 1:0d7cc59d485e 556 } else { //serial read
tajiri1999 0:6c2e6a485519 557
tajiri1999 1:0d7cc59d485e 558 /*get ball degree*/
tajiri1999 1:0d7cc59d485e 559 int count = 0;
tajiri1999 1:0d7cc59d485e 560 sensor.putc('B');
tajiri1999 1:0d7cc59d485e 561 char buffer[2];
tajiri1999 1:0d7cc59d485e 562 for (int i = 0; i < 2; i++) {
tajiri1999 1:0d7cc59d485e 563 while (sensor.readable() == 0) { //wait
tajiri1999 1:0d7cc59d485e 564 if (count >= 10000) {
tajiri1999 1:0d7cc59d485e 565 return value;
tajiri1999 1:0d7cc59d485e 566 }
tajiri1999 1:0d7cc59d485e 567 wait_us(1);
tajiri1999 1:0d7cc59d485e 568 count++;
tajiri1999 1:0d7cc59d485e 569 }
tajiri1999 1:0d7cc59d485e 570 buffer[i] = sensor.getc(); //get
tajiri1999 1:0d7cc59d485e 571 }
tajiri1999 1:0d7cc59d485e 572 value = ((uint16_t) buffer[0] << 8) + (uint8_t) buffer[1] - 180;
tajiri1999 0:6c2e6a485519 573
tajiri1999 1:0d7cc59d485e 574 return value;
tajiri1999 0:6c2e6a485519 575 }
tajiri1999 0:6c2e6a485519 576 }
tajiri1999 0:6c2e6a485519 577
tajiri1999 1:0d7cc59d485e 578 //////////////////////////////
tajiri1999 1:0d7cc59d485e 579 /*get ball distance*/
tajiri1999 1:0d7cc59d485e 580 //////////////////////////////
tajiri1999 1:0d7cc59d485e 581 int get_ball_distance(int A)
tajiri1999 0:6c2e6a485519 582 {
tajiri1999 1:0d7cc59d485e 583 static int value = 0;
tajiri1999 1:0d7cc59d485e 584 if (A == 1) { //analog read
tajiri1999 1:0d7cc59d485e 585 return (uint8_t) (ball_distance.read() * 255);
tajiri1999 1:0d7cc59d485e 586 } else { //serial read
tajiri1999 1:0d7cc59d485e 587 int count = 0;
tajiri1999 0:6c2e6a485519 588
tajiri1999 1:0d7cc59d485e 589 /*get ball distance*/
tajiri1999 1:0d7cc59d485e 590 sensor.putc('C');
tajiri1999 1:0d7cc59d485e 591 while (sensor.readable() == 0) { //wait
tajiri1999 1:0d7cc59d485e 592 if (count >= 10000) {
tajiri1999 1:0d7cc59d485e 593 return value;
tajiri1999 1:0d7cc59d485e 594 }
tajiri1999 1:0d7cc59d485e 595 wait_us(1);
tajiri1999 1:0d7cc59d485e 596 count++;
tajiri1999 1:0d7cc59d485e 597 }
tajiri1999 1:0d7cc59d485e 598 value = (int) sensor.getc(); //get
tajiri1999 1:0d7cc59d485e 599 return value;
tajiri1999 0:6c2e6a485519 600 }
tajiri1999 0:6c2e6a485519 601 }
tajiri1999 0:6c2e6a485519 602
tajiri1999 1:0d7cc59d485e 603 //////////////////////////////
tajiri1999 1:0d7cc59d485e 604 /*get line degree*/
tajiri1999 1:0d7cc59d485e 605 //////////////////////////////
tajiri1999 1:0d7cc59d485e 606 int get_line_degree()
tajiri1999 0:6c2e6a485519 607 {
tajiri1999 1:0d7cc59d485e 608 static char value = 0;
tajiri1999 1:0d7cc59d485e 609 static int degree = 0;
tajiri1999 1:0d7cc59d485e 610 int count = 0;
tajiri1999 1:0d7cc59d485e 611
tajiri1999 1:0d7cc59d485e 612 /*get line value*/
tajiri1999 1:0d7cc59d485e 613 sensor.putc('A');
tajiri1999 1:0d7cc59d485e 614 while (sensor.readable() == 0) { //wait
tajiri1999 1:0d7cc59d485e 615 if (count >= 10000) {
tajiri1999 1:0d7cc59d485e 616 return degree;
tajiri1999 1:0d7cc59d485e 617 }
tajiri1999 1:0d7cc59d485e 618 wait_us(1);
tajiri1999 1:0d7cc59d485e 619 count++;
tajiri1999 1:0d7cc59d485e 620 }
tajiri1999 1:0d7cc59d485e 621 value = sensor.getc(); //get
tajiri1999 1:0d7cc59d485e 622
tajiri1999 1:0d7cc59d485e 623 switch (value) {
tajiri1999 1:0d7cc59d485e 624 case 'N':
tajiri1999 1:0d7cc59d485e 625 degree = 999;
tajiri1999 1:0d7cc59d485e 626 return 999;
tajiri1999 1:0d7cc59d485e 627 //break;
tajiri1999 1:0d7cc59d485e 628 case '0':
tajiri1999 1:0d7cc59d485e 629 degree = 0;
tajiri1999 1:0d7cc59d485e 630 return 0;
tajiri1999 1:0d7cc59d485e 631 //break;
tajiri1999 1:0d7cc59d485e 632 case '1':
tajiri1999 1:0d7cc59d485e 633 degree = 45;
tajiri1999 1:0d7cc59d485e 634 return 45;
tajiri1999 1:0d7cc59d485e 635 //break;
tajiri1999 1:0d7cc59d485e 636 case '2':
tajiri1999 1:0d7cc59d485e 637 degree = 90;
tajiri1999 1:0d7cc59d485e 638 return 90;
tajiri1999 1:0d7cc59d485e 639 //break;
tajiri1999 1:0d7cc59d485e 640 case '3':
tajiri1999 1:0d7cc59d485e 641 degree = 135;
tajiri1999 1:0d7cc59d485e 642 return 135;
tajiri1999 1:0d7cc59d485e 643 //break;
tajiri1999 1:0d7cc59d485e 644 case '4':
tajiri1999 1:0d7cc59d485e 645 degree = 180;
tajiri1999 1:0d7cc59d485e 646 return 180;
tajiri1999 1:0d7cc59d485e 647 //break;
tajiri1999 1:0d7cc59d485e 648 case '5':
tajiri1999 1:0d7cc59d485e 649 degree = 225;
tajiri1999 1:0d7cc59d485e 650 return 225;
tajiri1999 1:0d7cc59d485e 651 //break;
tajiri1999 1:0d7cc59d485e 652 case '6':
tajiri1999 1:0d7cc59d485e 653 degree = 270;
tajiri1999 1:0d7cc59d485e 654 return 270;
tajiri1999 1:0d7cc59d485e 655 //break;
tajiri1999 1:0d7cc59d485e 656 case '7':
tajiri1999 1:0d7cc59d485e 657 degree = 315;
tajiri1999 1:0d7cc59d485e 658 return 315;
tajiri1999 1:0d7cc59d485e 659 //break;
tajiri1999 1:0d7cc59d485e 660 default:
tajiri1999 1:0d7cc59d485e 661 degree = 999;
tajiri1999 1:0d7cc59d485e 662 return 999;
tajiri1999 1:0d7cc59d485e 663 //break;
tajiri1999 1:0d7cc59d485e 664 }
tajiri1999 1:0d7cc59d485e 665 }
tajiri1999 1:0d7cc59d485e 666
tajiri1999 1:0d7cc59d485e 667 //////////////////////////////////////
tajiri1999 1:0d7cc59d485e 668 /*USS reading*/
tajiri1999 1:0d7cc59d485e 669 //////////////////////////////////////
tajiri1999 1:0d7cc59d485e 670 /////////////////////////////
tajiri1999 1:0d7cc59d485e 671 /*get uss range cm*/
tajiri1999 1:0d7cc59d485e 672 ////////////////////////////
tajiri1999 1:0d7cc59d485e 673 int get_uss_range(char c)
tajiri1999 1:0d7cc59d485e 674 {
tajiri1999 1:0d7cc59d485e 675 static int count = 0;
tajiri1999 1:0d7cc59d485e 676 static int l = 0, r = 0, b = 0;
tajiri1999 1:0d7cc59d485e 677
tajiri1999 1:0d7cc59d485e 678 if (timer_USS.read() >= 0.04) {
tajiri1999 1:0d7cc59d485e 679 l = uss_left.Read_cm();
tajiri1999 1:0d7cc59d485e 680 r = uss_right.Read_cm();
tajiri1999 1:0d7cc59d485e 681 b = uss_back.Read_cm();
tajiri1999 1:0d7cc59d485e 682 uss_left.Send();
tajiri1999 1:0d7cc59d485e 683 uss_right.Send();
tajiri1999 1:0d7cc59d485e 684 uss_back.Send();
tajiri1999 1:0d7cc59d485e 685 timer_USS.reset();
tajiri1999 1:0d7cc59d485e 686 }
tajiri1999 1:0d7cc59d485e 687
tajiri1999 1:0d7cc59d485e 688 if (c == 'l' || c == 'L') {
tajiri1999 1:0d7cc59d485e 689 return l;
tajiri1999 1:0d7cc59d485e 690 } else if (c == 'r' || c == 'R') {
tajiri1999 1:0d7cc59d485e 691 return r;
tajiri1999 1:0d7cc59d485e 692 } else if (c == 'b' || c == 'B') {
tajiri1999 1:0d7cc59d485e 693 return b;
tajiri1999 1:0d7cc59d485e 694 } else {
tajiri1999 1:0d7cc59d485e 695 return 999;
tajiri1999 1:0d7cc59d485e 696 }
tajiri1999 1:0d7cc59d485e 697 }
tajiri1999 1:0d7cc59d485e 698
tajiri1999 1:0d7cc59d485e 699 //////////////////////////////////////
tajiri1999 1:0d7cc59d485e 700 /*Battery check voltage*/
tajiri1999 1:0d7cc59d485e 701 //////////////////////////////////////
tajiri1999 1:0d7cc59d485e 702 bool check_voltage()
tajiri1999 1:0d7cc59d485e 703 {
tajiri1999 1:0d7cc59d485e 704 static float sum[5] = { 8, 8, 8, 8, 8 };
tajiri1999 1:0d7cc59d485e 705 static float Ave = 8;
tajiri1999 1:0d7cc59d485e 706 static bool S = 1;
tajiri1999 1:0d7cc59d485e 707 if (timer_volt.read() > 0.2) {
tajiri1999 1:0d7cc59d485e 708 for (int i = 4; i > 0; i--) {
tajiri1999 1:0d7cc59d485e 709 sum[i] = sum[i - 1];
tajiri1999 1:0d7cc59d485e 710 }
tajiri1999 1:0d7cc59d485e 711 sum[0] = voltage.read() * 9.9;
tajiri1999 1:0d7cc59d485e 712 Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5;
tajiri1999 1:0d7cc59d485e 713 if (Ave < 7.0) { //7.0V以下で自動遮断
tajiri1999 1:0d7cc59d485e 714 S = 0;
tajiri1999 1:0d7cc59d485e 715 } else {
tajiri1999 1:0d7cc59d485e 716 S = 1;
tajiri1999 1:0d7cc59d485e 717
tajiri1999 1:0d7cc59d485e 718 }
tajiri1999 1:0d7cc59d485e 719 timer_volt.reset();
tajiri1999 1:0d7cc59d485e 720 }
tajiri1999 1:0d7cc59d485e 721 return S;
tajiri1999 1:0d7cc59d485e 722 }
tajiri1999 1:0d7cc59d485e 723