佐賀大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?
User | Revision | Line number | New 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 | * 2つ目は回り込み半径の調整です |
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 | * 3つ目はスピードと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 |