NITICRobotClub 2018Team-B / Mbed 2 deprecated UNKO_FINAL_PlanA

Bteam tactics plan A _ move table ver

Dependencies:   HCSR04 PID QEI mbed

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

main.cpp@8:3df97287c825, 2018-10-08 (annotated)

Committer:
yuron
Date:
Mon Oct 08 07:21:45 2018 +0000
Revision:
8:3df97287c825
Parent:
7:7f16fb8b0192
Child:
9:1359f0c813b1
auto robo 10/8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuron 4:df334779a69e 1 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 2 /* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
yuron 4:df334779a69e 3 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 4 /* このプログラムは上記のロボット専用の制御プログラムである。 */
yuron 4:df334779a69e 5 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 6 /* 対応機種: NUCLEO-F446RE */
yuron 4:df334779a69e 7 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 8 /* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */
yuron 4:df334779a69e 9 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 10 /* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
yuron 4:df334779a69e 11 /* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
yuron 4:df334779a69e 12 /* ------------------------------------------------------------------- */
yuron 5:167327a82430 13
yuron 0:f73c1b076ae4 14 #include "mbed.h"
yuron 0:f73c1b076ae4 15 #include "math.h"
yuron 0:f73c1b076ae4 16 #include "QEI.h"
yuron 0:f73c1b076ae4 17 #include "PID.h"
yuron 4:df334779a69e 18 #include "hcsr04.h"
yuron 5:167327a82430 19
yuron 5:167327a82430 20 //Pi
yuron 0:f73c1b076ae4 21 #define PI 3.14159265359
yuron 5:167327a82430 22 //PIDGain of wheels(fast_mode)
yuron 4:df334779a69e 23 #define Kp 20.0
yuron 4:df334779a69e 24 #define Ki 0.02
yuron 4:df334779a69e 25 #define Kd 0.0
yuron 5:167327a82430 26 //PIDGain of rollers
yuron 6:5a051a378e42 27 #define front_roller_Kp 1.3
yuron 6:5a051a378e42 28 #define front_roller_Ki 0.19
yuron 6:5a051a378e42 29 #define front_roller_Kd 0.0
yuron 6:5a051a378e42 30 #define back_roller_Kp 1.3
yuron 6:5a051a378e42 31 #define back_roller_Ki 0.1
yuron 6:5a051a378e42 32 #define back_roller_Kd 0.0
yuron 5:167327a82430 33 //PIDGain of wheels(slow_mode)
yuron 5:167327a82430 34 #define Kp_slow 0.6
yuron 5:167327a82430 35 #define Ki_slow 0.03
yuron 5:167327a82430 36 #define Kd_slow 0.0
yuron 5:167327a82430 37
yuron 7:7f16fb8b0192 38 //搭載ボトル数
yuron 8:3df97287c825 39 //1弾倉当たり9発、残り3発になったら弾倉切り替えしよう
yuron 8:3df97287c825 40 #define bottles 14
yuron 7:7f16fb8b0192 41
yuron 5:167327a82430 42 //停止
yuron 5:167327a82430 43 #define stop 0
yuron 5:167327a82430 44 //低速右移動
yuron 5:167327a82430 45 #define right_slow 1
yuron 5:167327a82430 46 //低速左移動
yuron 5:167327a82430 47 #define left_slow 2
yuron 5:167327a82430 48 //超低速右移動
yuron 5:167327a82430 49 #define right_super_slow 3
yuron 5:167327a82430 50 //超低速左移動
yuron 5:167327a82430 51 #define left_super_slow 4
yuron 5:167327a82430 52 //右旋回
yuron 5:167327a82430 53 #define turn_right 5
yuron 5:167327a82430 54 //左旋回
yuron 5:167327a82430 55 #define turn_left 6
yuron 5:167327a82430 56 //前進
yuron 5:167327a82430 57 #define front_trace 7
yuron 5:167327a82430 58 //後進
yuron 5:167327a82430 59 #define back_trace 8
yuron 5:167327a82430 60 //前前進後ろ右旋回
yuron 5:167327a82430 61 #define front_front_back_right 9
yuron 5:167327a82430 62 //前前進後ろ左旋回
yuron 5:167327a82430 63 #define front_front_back_left 10
yuron 5:167327a82430 64 //前右旋回後ろ前進
yuron 5:167327a82430 65 #define front_right_back_front 11
yuron 5:167327a82430 66 //前左旋回後ろ前進
yuron 5:167327a82430 67 #define front_left_back_front 12
yuron 5:167327a82430 68
yuron 8:3df97287c825 69 #define right_fast 13
yuron 8:3df97287c825 70 #define left_fast 14
yuron 8:3df97287c825 71
yuron 5:167327a82430 72 //赤ゾーン
yuron 5:167327a82430 73 #define red 1
yuron 5:167327a82430 74 //青ゾーン
yuron 5:167327a82430 75 #define blue 0
yuron 4:df334779a69e 76
yuron 4:df334779a69e 77 //右前オムニ
yuron 4:df334779a69e 78 PID migimae(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 79 //右後オムニ
yuron 4:df334779a69e 80 PID migiusiro(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 81 //左前オムニ
yuron 4:df334779a69e 82 PID hidarimae(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 83 //左後オムニ
yuron 4:df334779a69e 84 PID hidariusiro(Kp, Ki, Kd, 0.001);
yuron 5:167327a82430 85
yuron 5:167327a82430 86 //右前オムニ(スローモード)
yuron 5:167327a82430 87 PID migimae_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron 5:167327a82430 88 //右後オムニ(スローモード)
yuron 5:167327a82430 89 PID migiusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron 5:167327a82430 90 //左前オムニ(スローモード)
yuron 5:167327a82430 91 PID hidarimae_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron 5:167327a82430 92 //左後オムニ(スローモード)
yuron 5:167327a82430 93 PID hidariusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron 5:167327a82430 94
yuron 4:df334779a69e 95 //前ローラー
yuron 6:5a051a378e42 96 PID front_roller(front_roller_Kp, front_roller_Ki, front_roller_Kd, 0.001);
yuron 4:df334779a69e 97 //後ローラー
yuron 6:5a051a378e42 98 PID back_roller (back_roller_Kp, back_roller_Ki, back_roller_Kd, 0.001);
yuron 0:f73c1b076ae4 99
yuron 4:df334779a69e 100 //MDとの通信ポート
yuron 4:df334779a69e 101 I2C i2c(PB_9, PB_8); //SDA, SCL
yuron 4:df334779a69e 102 //PCとの通信ポート
yuron 4:df334779a69e 103 Serial pc(USBTX, USBRX); //TX, RX
yuron 4:df334779a69e 104 //フォトインタラプタ制御基板からの受信ポート
yuron 4:df334779a69e 105 Serial photo(NC, PC_11); //TX, RX
yuron 4:df334779a69e 106 //TWE-Liteからの受信ポート
yuron 4:df334779a69e 107 Serial twe(PC_12, PD_2); //TX, RX
yuron 4:df334779a69e 108
yuron 4:df334779a69e 109 //超音波センサ1
yuron 4:df334779a69e 110 HCSR04 sonic(PB_3, PB_10); //Trig, Echo
yuron 4:df334779a69e 111 //超音波センサ2
yuron 4:df334779a69e 112 //HCSR04 sonic2(PC_13, PA_15); //Trig, Echo
yuron 4:df334779a69e 113 //超音波センサ3
yuron 4:df334779a69e 114 //HCSR04 sonic3(PC_15, PC_14); //Trig, Echo
yuron 4:df334779a69e 115 //超音波センサ4
yuron 4:df334779a69e 116 //HCSR04 sonic4(PH_1 , PH_0 ); //Trig, Echo
yuron 4:df334779a69e 117
yuron 4:df334779a69e 118 //赤、青ゾーン切り替え用スイッチ
yuron 4:df334779a69e 119 DigitalIn side(PC_1);
yuron 4:df334779a69e 120 //スタートボタン
yuron 4:df334779a69e 121 DigitalIn start_button(PC_4);
yuron 5:167327a82430 122 //フェンス検知用リミットスイッチ
yuron 5:167327a82430 123 DigitalIn limit(PH_1);
yuron 4:df334779a69e 124 //スイッチ1
yuron 4:df334779a69e 125 DigitalIn user_switch1(PB_0);
yuron 4:df334779a69e 126 //スイッチ2
yuron 4:df334779a69e 127 DigitalIn user_switch2(PA_4);
yuron 4:df334779a69e 128 //スイッチ3
yuron 4:df334779a69e 129 DigitalIn user_switch3(PA_3);
yuron 4:df334779a69e 130 //スイッチ4
yuron 4:df334779a69e 131 //以下の文を入れるとロリコンが読めなくなる
yuron 4:df334779a69e 132 //DigitalIn user_switch4(PA_2);
yuron 4:df334779a69e 133 //スイッチ5
yuron 4:df334779a69e 134 DigitalIn user_switch5(PA_10);
yuron 4:df334779a69e 135
yuron 4:df334779a69e 136 //フォトインタラプタ
yuron 4:df334779a69e 137 DigitalIn photo_interrupter(PA_15);
yuron 4:df334779a69e 138
yuron 4:df334779a69e 139 //12V停止信号ピン
yuron 0:f73c1b076ae4 140 DigitalOut emergency(PA_13);
yuron 4:df334779a69e 141
yuron 4:df334779a69e 142 //赤ゾーンLED
yuron 4:df334779a69e 143 DigitalOut blue_side(PC_0);
yuron 4:df334779a69e 144 //青ゾーンLED
yuron 4:df334779a69e 145 DigitalOut red_side(PC_3);
yuron 4:df334779a69e 146 //スタートLED
yuron 6:5a051a378e42 147 DigitalOut start_LED(PC_13);
yuron 6:5a051a378e42 148 //DigitalOut start_LED(PA_8);
yuron 8:3df97287c825 149
yuron 4:df334779a69e 150 //LED1
yuron 4:df334779a69e 151 DigitalOut myled1(PC_6);
yuron 4:df334779a69e 152 //LED2
yuron 4:df334779a69e 153 DigitalOut myled2(PC_5);
yuron 4:df334779a69e 154 //LED3
yuron 4:df334779a69e 155 DigitalOut myled3(PA_12);
yuron 4:df334779a69e 156 //LED4
yuron 4:df334779a69e 157 DigitalOut myled4(PB_1);
yuron 4:df334779a69e 158 //LED5
yuron 4:df334779a69e 159 DigitalOut myled5(PA_5);
yuron 0:f73c1b076ae4 160
yuron 4:df334779a69e 161 //前ローラー
yuron 4:df334779a69e 162 QEI front_roller_wheel(PA_0, PA_1, NC, 624);
yuron 4:df334779a69e 163 //後ローラー
yuron 4:df334779a69e 164 QEI back_roller_wheel(PB_5, PB_4, NC, 624);
yuron 4:df334779a69e 165 //計測オムニ1
yuron 5:167327a82430 166 QEI measure_wheel(PC_9, PC_8, NC, 624);
yuron 4:df334779a69e 167 //計測オムニ2(使用不可)
yuron 4:df334779a69e 168 //QEI measure2_wheel(PB_3, PB_10, NC, 624);
yuron 4:df334779a69e 169 //右前オムニ
yuron 4:df334779a69e 170 QEI migimae_wheel(PB_6 , PA_7 , NC, 624);
yuron 4:df334779a69e 171 //右後オムニ
yuron 4:df334779a69e 172 QEI migiusiro_wheel(PA_11, PB_12, NC, 624);
yuron 4:df334779a69e 173 //左前オムニ
yuron 4:df334779a69e 174 QEI hidarimae_wheel(PB_2, PB_15, NC, 624);
yuron 4:df334779a69e 175 //左後オムニ
yuron 4:df334779a69e 176 QEI hidariusiro_wheel(PB_14, PB_13, NC, 624);
yuron 0:f73c1b076ae4 177
yuron 6:5a051a378e42 178 Ticker get_rpm;
yuron 1:62b321f6c9c3 179 Timer timer;
yuron 5:167327a82430 180 Timer back_timer1;
yuron 5:167327a82430 181 Timer back_timer2;
yuron 5:167327a82430 182 Timer back_timer3;
yuron 8:3df97287c825 183 Timer start_timer;
yuron 0:f73c1b076ae4 184
yuron 7:7f16fb8b0192 185 bool roller_flag = 0;
yuron 7:7f16fb8b0192 186 bool start_flag = 0;
yuron 8:3df97287c825 187
yuron 8:3df97287c825 188 //2段下段乗ったかなフラグ
yuron 8:3df97287c825 189 bool low_grade_flag = 0;
yuron 8:3df97287c825 190 //2段上段乗ったかなフラグ
yuron 8:3df97287c825 191 bool high_grade_flag = 0;
yuron 8:3df97287c825 192 //移動(低)乗ったかなフラグ
yuron 8:3df97287c825 193 bool low_table_flag = 0;
yuron 8:3df97287c825 194 //移動(中)乗ったかなフラグ
yuron 8:3df97287c825 195 bool medium_table_flag = 0;
yuron 8:3df97287c825 196 //移動(高)乗ったかなフラグ
yuron 8:3df97287c825 197 bool high_table_flag = 0;
yuron 8:3df97287c825 198
yuron 8:3df97287c825 199
yuron 8:3df97287c825 200 static int mouted_bottles = 0;
yuron 7:7f16fb8b0192 201 int loading_state = 0;
yuron 7:7f16fb8b0192 202 int line_state = 0;
yuron 5:167327a82430 203 int front_line_state = 0;
yuron 5:167327a82430 204 int back_line_state = 0;
yuron 7:7f16fb8b0192 205 int line_state_pettern = 0;
yuron 5:167327a82430 206 unsigned int distance;
yuron 5:167327a82430 207 int trace_direction = 0;
yuron 5:167327a82430 208 static int i = 0;
yuron 6:5a051a378e42 209 int lateral_frag = 0;
yuron 0:f73c1b076ae4 210
yuron 4:df334779a69e 211 int migimae_rpm;
yuron 4:df334779a69e 212 int migiusiro_rpm;
yuron 4:df334779a69e 213 int hidarimae_rpm;
yuron 4:df334779a69e 214 int hidariusiro_rpm;
yuron 5:167327a82430 215 //int measure_rpm;
yuron 4:df334779a69e 216 int front_roller_rpm;
yuron 4:df334779a69e 217 int back_roller_rpm;
yuron 0:f73c1b076ae4 218
yuron 5:167327a82430 219 //int migimae_pulse;
yuron 5:167327a82430 220 //int migiusiro_pulse;
yuron 5:167327a82430 221 //int hidarimae_pulse;
yuron 5:167327a82430 222 //int hidariusiro_pulse;
yuron 5:167327a82430 223 int measure_pulse;
yuron 5:167327a82430 224 //int front_roller_pulse;
yuron 5:167327a82430 225 //int back_roller_pulse;
yuron 5:167327a82430 226
yuron 5:167327a82430 227 int migimae_pulse[10];
yuron 5:167327a82430 228 int migiusiro_pulse[10];
yuron 5:167327a82430 229 int hidarimae_pulse[10];
yuron 5:167327a82430 230 int hidariusiro_pulse[10];
yuron 5:167327a82430 231 //int measure_pulse[10];
yuron 5:167327a82430 232 int front_roller_pulse[10];
yuron 5:167327a82430 233 int back_roller_pulse[10];
yuron 0:f73c1b076ae4 234
yuron 5:167327a82430 235 int sum_migimae_pulse;
yuron 5:167327a82430 236 int sum_migiusiro_pulse;
yuron 5:167327a82430 237 int sum_hidarimae_pulse;
yuron 5:167327a82430 238 int sum_hidariusiro_pulse;
yuron 5:167327a82430 239 //int sum_measure_pulse;
yuron 5:167327a82430 240 int sum_front_roller_pulse;
yuron 5:167327a82430 241 int sum_back_roller_pulse;
yuron 0:f73c1b076ae4 242
yuron 5:167327a82430 243 int ave_migimae_pulse;
yuron 5:167327a82430 244 int ave_migiusiro_pulse;
yuron 5:167327a82430 245 int ave_hidarimae_pulse;
yuron 5:167327a82430 246 int ave_hidariusiro_pulse;
yuron 5:167327a82430 247 //int ave_measure_pulse;
yuron 5:167327a82430 248 int ave_front_roller_pulse;
yuron 5:167327a82430 249 int ave_back_roller_pulse;
yuron 0:f73c1b076ae4 250
yuron 0:f73c1b076ae4 251 char send_data[1];
yuron 0:f73c1b076ae4 252 char true_send_data[1];
yuron 0:f73c1b076ae4 253
yuron 4:df334779a69e 254 char migimae_data[1];
yuron 4:df334779a69e 255 char migiusiro_data[1];
yuron 4:df334779a69e 256 char hidarimae_data[1];
yuron 4:df334779a69e 257 char hidariusiro_data[1];
yuron 4:df334779a69e 258 char front_roller_data[1];
yuron 4:df334779a69e 259 char back_roller_data[1];
yuron 4:df334779a69e 260 char loading_data[1];
yuron 4:df334779a69e 261 char cylinder_data[1];
yuron 4:df334779a69e 262
yuron 4:df334779a69e 263 char true_migimae_data[1];
yuron 4:df334779a69e 264 char true_migiusiro_data[1];
yuron 4:df334779a69e 265 char true_hidarimae_data[1];
yuron 4:df334779a69e 266 char true_hidariusiro_data[1];
yuron 4:df334779a69e 267
yuron 5:167327a82430 268 //各関数のプロトタイプ宣言
yuron 5:167327a82430 269 //回転数取得関数
yuron 0:f73c1b076ae4 270 void flip();
yuron 5:167327a82430 271 //前進PID(fast_mode)
yuron 4:df334779a69e 272 int front_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 273 //前進PID(slow_mode)
yuron 5:167327a82430 274 int front_PID_slow(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 275 //前進PID(fast_mode)
yuron 4:df334779a69e 276 int back_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 277 //後進PID(slow_mode)
yuron 5:167327a82430 278 int back_PID_slow(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 279 //右進PID(fast_mode)
yuron 4:df334779a69e 280 int rihgt_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 281 //左進PID(fast_mode)
yuron 4:df334779a69e 282 int left_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 283 //右前PID(slow_mode)
yuron 4:df334779a69e 284 int right_front_PID(int RB, int LF);
yuron 5:167327a82430 285 //右後PID(slow_mode)
yuron 4:df334779a69e 286 int right_back_PID(int RF, int LB);
yuron 5:167327a82430 287 //左前PID(slow_mode)
yuron 4:df334779a69e 288 int left_front_PID(int RF, int LB);
yuron 5:167327a82430 289 //左後PID(slow_mode)
yuron 4:df334779a69e 290 int left_back_PID(int RB, int RF);
yuron 5:167327a82430 291 //右旋回PID(fast mode)
yuron 4:df334779a69e 292 int turn_right_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 293 //右旋回PID(slow mode)
yuron 5:167327a82430 294 int turn_right_PID_slow(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 295 //左旋回PID(fast mode)
yuron 4:df334779a69e 296 int turn_left_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 297 //左旋回PID(sow mode)
yuron 5:167327a82430 298 int turn_left_PID_slow(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 299 //前前進後右旋回(slow_mode)
yuron 5:167327a82430 300 int front_front_back_right_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 301 //前前進後左旋回(slow_mode)
yuron 5:167327a82430 302 int front_front_back_left_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 303 //前右旋回後前進(slow_mode)
yuron 5:167327a82430 304 int front_right_back_front_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 305 //前左旋回後前進(slow_mode)
yuron 5:167327a82430 306 int front_left_back_front_PID(int RF, int RB, int LF, int LB);
yuron 5:167327a82430 307 //ローラーPID
yuron 4:df334779a69e 308 int roller_PID(int front, int back);
yuron 5:167327a82430 309
yuron 5:167327a82430 310 //ラインセンサ制御基板との通信用関数
yuron 4:df334779a69e 311 void linetrace();
yuron 5:167327a82430 312 //超音波センサ用関数
yuron 4:df334779a69e 313 void ultrasonic();
yuron 0:f73c1b076ae4 314
yuron 5:167327a82430 315 //移動平均をPID
yuron 4:df334779a69e 316 void flip() {
yuron 5:167327a82430 317
yuron 5:167327a82430 318 //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
yuron 5:167327a82430 319 sum_migimae_pulse -= migimae_pulse[i];
yuron 5:167327a82430 320 sum_migiusiro_pulse -= migiusiro_pulse[i];
yuron 5:167327a82430 321 sum_hidarimae_pulse -= hidarimae_pulse[i];
yuron 5:167327a82430 322 sum_hidariusiro_pulse -= hidariusiro_pulse[i];
yuron 5:167327a82430 323 sum_front_roller_pulse -= front_roller_pulse[i];
yuron 5:167327a82430 324 sum_back_roller_pulse -= back_roller_pulse[i];
yuron 4:df334779a69e 325
yuron 5:167327a82430 326 //配列のi番目の箱に取得パルスを代入
yuron 5:167327a82430 327 migimae_pulse[i] = migimae_wheel.getPulses();
yuron 5:167327a82430 328 migiusiro_pulse[i] = migiusiro_wheel.getPulses();
yuron 5:167327a82430 329 hidarimae_pulse[i] = hidarimae_wheel.getPulses();
yuron 5:167327a82430 330 hidariusiro_pulse[i] = hidariusiro_wheel.getPulses();
yuron 5:167327a82430 331 measure_pulse = measure_wheel.getPulses();
yuron 5:167327a82430 332 front_roller_pulse[i] = front_roller_wheel.getPulses();
yuron 5:167327a82430 333 back_roller_pulse[i] = back_roller_wheel.getPulses();
yuron 5:167327a82430 334 //migimae_pulse = migimae_wheel.getPulses();
yuron 5:167327a82430 335 //migiusiro_pulse = migiusiro_wheel.getPulses();
yuron 5:167327a82430 336 //hidarimae_pulse = hidarimae_wheel.getPulses();
yuron 5:167327a82430 337 //hidariusiro_pulse = hidariusiro_wheel.getPulses();
yuron 4:df334779a69e 338
yuron 4:df334779a69e 339 migimae_wheel.reset();
yuron 4:df334779a69e 340 migiusiro_wheel.reset();
yuron 4:df334779a69e 341 hidarimae_wheel.reset();
yuron 4:df334779a69e 342 hidariusiro_wheel.reset();
yuron 4:df334779a69e 343 front_roller_wheel.reset();
yuron 4:df334779a69e 344 back_roller_wheel.reset();
yuron 5:167327a82430 345
yuron 5:167327a82430 346 //i[0]~i[9]までの合計値を代入
yuron 5:167327a82430 347 sum_migimae_pulse += migimae_pulse[i];
yuron 5:167327a82430 348 sum_migiusiro_pulse += migiusiro_pulse[i];
yuron 5:167327a82430 349 sum_hidarimae_pulse += hidarimae_pulse[i];
yuron 5:167327a82430 350 sum_hidariusiro_pulse += hidariusiro_pulse[i];
yuron 5:167327a82430 351 sum_front_roller_pulse += front_roller_pulse[i];
yuron 5:167327a82430 352 sum_back_roller_pulse += back_roller_pulse[i];
yuron 5:167327a82430 353
yuron 5:167327a82430 354 //インクリメント
yuron 5:167327a82430 355 i++;
yuron 5:167327a82430 356
yuron 5:167327a82430 357 //iが最大値(9)になったらリセット
yuron 5:167327a82430 358 if(i > 9) {
yuron 5:167327a82430 359 i = 0;
yuron 5:167327a82430 360 }
yuron 5:167327a82430 361
yuron 5:167327a82430 362 //10回分の合計値を10で割り、取得パルスの平均を出す
yuron 5:167327a82430 363 ave_migimae_pulse = sum_migimae_pulse / 10;
yuron 5:167327a82430 364 ave_migiusiro_pulse = sum_migiusiro_pulse / 10;
yuron 5:167327a82430 365 ave_hidarimae_pulse = sum_hidarimae_pulse / 10;
yuron 5:167327a82430 366 ave_hidariusiro_pulse = sum_hidariusiro_pulse / 10;
yuron 5:167327a82430 367 ave_front_roller_pulse = sum_front_roller_pulse / 10;
yuron 5:167327a82430 368 ave_back_roller_pulse = sum_back_roller_pulse / 10;
yuron 5:167327a82430 369
yuron 5:167327a82430 370 //平均値をRPMへ変換
yuron 5:167327a82430 371 migimae_rpm = ave_migimae_pulse * 25 * 60 / 1200;
yuron 5:167327a82430 372 migiusiro_rpm = ave_migiusiro_pulse * 25 * 60 / 1200;
yuron 5:167327a82430 373 hidarimae_rpm = ave_hidarimae_pulse * 25 * 60 / 1200;
yuron 5:167327a82430 374 hidariusiro_rpm = ave_hidariusiro_pulse * 25 * 60 / 1200;
yuron 5:167327a82430 375 front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
yuron 5:167327a82430 376 back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
yuron 5:167327a82430 377
yuron 5:167327a82430 378 //pc.printf("左後 %d\n", abs(hidariusiro_rpm));
yuron 5:167327a82430 379 //pc.printf("%d\n\r", measure_pulse);
yuron 5:167327a82430 380 //pc.printf("%d %d\n", abs(front_roller_rpm), abs(back_roller_rpm));
yuron 5:167327a82430 381 //pc.printf("前 %d 後 %d %d %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance, line_state);
yuron 4:df334779a69e 382 }
yuron 5:167327a82430 383 //前進(fast mode)
yuron 4:df334779a69e 384 int front_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 385 // センサ出力値の最小、最大
yuron 4:df334779a69e 386 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 387 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 388 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 389 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 390
yuron 5:167327a82430 391 // 制御量の最小、最大
yuron 4:df334779a69e 392 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 393 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 394 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 395 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 396
yuron 5:167327a82430 397 // よくわからんやつ
yuron 4:df334779a69e 398 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 399 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 400 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 401 hidariusiro.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 402
yuron 5:167327a82430 403 // 目標値
yuron 4:df334779a69e 404 migimae.setSetPoint(RF);
yuron 4:df334779a69e 405 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 406 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 407 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 408
yuron 5:167327a82430 409 // センサ出力
yuron 4:df334779a69e 410 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 411 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 412 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 413 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 414
yuron 5:167327a82430 415 // 制御量(計算結果)
yuron 4:df334779a69e 416 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 417 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 418 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 419 hidariusiro_data[0] = hidariusiro.compute();
yuron 0:f73c1b076ae4 420
yuron 5:167327a82430 421 // 制御量をPWM値に変換
yuron 4:df334779a69e 422 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 423 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 424 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 425 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 426
yuron 4:df334779a69e 427 return 0;
yuron 0:f73c1b076ae4 428 }
yuron 5:167327a82430 429 //前進(slow mode)
yuron 5:167327a82430 430 int front_PID_slow(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 431 // センサ出力値の最小、最大
yuron 5:167327a82430 432 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 433 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 434 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 435 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 436
yuron 5:167327a82430 437 // 制御量の最小、最大
yuron 5:167327a82430 438 migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 439 migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 440 hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 441 hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 442
yuron 5:167327a82430 443 // よくわからんやつ
yuron 5:167327a82430 444 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 445 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 446 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 447 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 448
yuron 5:167327a82430 449 // 目標値
yuron 5:167327a82430 450 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 451 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 452 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 453 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 454
yuron 5:167327a82430 455 // センサ出力
yuron 5:167327a82430 456 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 457 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 458 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 459 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 460
yuron 5:167327a82430 461 // 制御量(計算結果)
yuron 5:167327a82430 462 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 463 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 464 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 465 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 466
yuron 5:167327a82430 467 // 制御量をPWM値に変換
yuron 5:167327a82430 468 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 469 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 5:167327a82430 470 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 471 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 472
yuron 5:167327a82430 473 return 0;
yuron 5:167327a82430 474 }
yuron 5:167327a82430 475 //後進(fast mode)
yuron 4:df334779a69e 476 int back_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 477 // センサ出力値の最小、最大
yuron 4:df334779a69e 478 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 479 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 480 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 481 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 482
yuron 5:167327a82430 483 // 制御量の最小、最大
yuron 4:df334779a69e 484 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 485 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 486 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 487 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 488
yuron 5:167327a82430 489 // よくわからんやつ
yuron 4:df334779a69e 490 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 491 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 492 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 493 hidariusiro.setMode(AUTO_MODE);
yuron 5:167327a82430 494
yuron 5:167327a82430 495 // 目標値
yuron 4:df334779a69e 496 migimae.setSetPoint(RF);
yuron 4:df334779a69e 497 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 498 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 499 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 500
yuron 5:167327a82430 501 // センサ出力
yuron 4:df334779a69e 502 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 4:df334779a69e 503 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 504 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 505 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 506 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 507
yuron 5:167327a82430 508 // 制御量(計算結果)
yuron 4:df334779a69e 509 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 510 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 511 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 512 hidariusiro_data[0] = hidariusiro.compute();
yuron 5:167327a82430 513
yuron 4:df334779a69e 514 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 515 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 516 true_hidarimae_data[0] = hidarimae_data[0];
yuron 4:df334779a69e 517 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 518
yuron 4:df334779a69e 519 return 0;
yuron 4:df334779a69e 520 }
yuron 5:167327a82430 521 //後進(slow mode)
yuron 5:167327a82430 522 int back_PID_slow(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 523 // センサ出力値の最小、最大
yuron 5:167327a82430 524 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 525 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 526 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 527 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 528
yuron 5:167327a82430 529 // 制御量の最小、最大
yuron 5:167327a82430 530 migimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 531 migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 532 hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 533 hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 534
yuron 5:167327a82430 535 // よくわからんやつ
yuron 5:167327a82430 536 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 537 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 538 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 539 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 540
yuron 5:167327a82430 541 // 目標値
yuron 5:167327a82430 542 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 543 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 544 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 545 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 546
yuron 5:167327a82430 547 // センサ出力
yuron 5:167327a82430 548 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 5:167327a82430 549 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 550 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 551 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 552 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 553
yuron 5:167327a82430 554 // 制御量(計算結果)
yuron 5:167327a82430 555 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 556 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 557 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 558 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 559
yuron 5:167327a82430 560 true_migimae_data[0] = migimae_data[0];
yuron 5:167327a82430 561 true_migiusiro_data[0] = migiusiro_data[0];
yuron 5:167327a82430 562 true_hidarimae_data[0] = hidarimae_data[0];
yuron 5:167327a82430 563 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 564
yuron 5:167327a82430 565 return 0;
yuron 5:167327a82430 566 }
yuron 5:167327a82430 567 //右進(fast mode)
yuron 4:df334779a69e 568 int right_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 569 // センサ出力値の最小、最大
yuron 4:df334779a69e 570 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 571 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 572 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 573 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 574
yuron 5:167327a82430 575 // 制御量の最小、最大
yuron 4:df334779a69e 576 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 577 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 578 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 579 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 580
yuron 5:167327a82430 581 // よくわからんやつ
yuron 4:df334779a69e 582 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 583 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 584 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 585 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 586
yuron 5:167327a82430 587 // 目標値
yuron 4:df334779a69e 588 migimae.setSetPoint(RF);
yuron 4:df334779a69e 589 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 590 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 591 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 592
yuron 5:167327a82430 593 // センサ出力
yuron 4:df334779a69e 594 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 595 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 596 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 597 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 598
yuron 5:167327a82430 599 // 制御量(計算結果)
yuron 4:df334779a69e 600 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 601 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 602 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 603 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 604
yuron 5:167327a82430 605 // 制御量をPWM値に変換
yuron 4:df334779a69e 606 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 607 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 608 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 609 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 610
yuron 4:df334779a69e 611 return 0;
yuron 4:df334779a69e 612 }
yuron 5:167327a82430 613 //右進(slow mode)
yuron 5:167327a82430 614 int right_PID_slow(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 615 // センサ出力値の最小、最大
yuron 5:167327a82430 616 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 617 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 618 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 619 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 620
yuron 5:167327a82430 621 // 制御量の最小、最大
yuron 5:167327a82430 622 migimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 623 migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 624 hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 625 hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 626
yuron 5:167327a82430 627 // よくわからんやつ
yuron 5:167327a82430 628 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 629 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 630 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 631 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 632
yuron 5:167327a82430 633 // 目標値
yuron 5:167327a82430 634 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 635 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 636 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 637 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 638
yuron 5:167327a82430 639 // センサ出力
yuron 5:167327a82430 640 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 641 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 642 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 643 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 644
yuron 5:167327a82430 645 // 制御量(計算結果)
yuron 5:167327a82430 646 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 647 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 648 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 649 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 650
yuron 5:167327a82430 651 // 制御量をPWM値に変換
yuron 5:167327a82430 652 true_migimae_data[0] = migimae_data[0];
yuron 5:167327a82430 653 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 5:167327a82430 654 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 655 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 656
yuron 5:167327a82430 657 return 0;
yuron 5:167327a82430 658 }
yuron 5:167327a82430 659 //左進(fast mode)
yuron 4:df334779a69e 660 int left_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 661 // センサ出力値の最小、最大
yuron 4:df334779a69e 662 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 663 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 664 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 665 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 666
yuron 5:167327a82430 667 // 制御量の最小、最大
yuron 4:df334779a69e 668 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 669 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 670 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 671 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 0:f73c1b076ae4 672
yuron 5:167327a82430 673 // よくわからんやつ
yuron 4:df334779a69e 674 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 675 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 676 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 677 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 678
yuron 5:167327a82430 679 // 目標値
yuron 4:df334779a69e 680 migimae.setSetPoint(RF);
yuron 4:df334779a69e 681 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 682 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 683 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 684
yuron 5:167327a82430 685 // センサ出力
yuron 4:df334779a69e 686 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 687 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 688 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 689 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 690
yuron 5:167327a82430 691 // 制御量(計算結果)
yuron 4:df334779a69e 692 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 693 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 694 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 695 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 696
yuron 5:167327a82430 697 // 制御量をPWM値に変換
yuron 5:167327a82430 698 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 699 true_migiusiro_data[0] = migiusiro_data[0];
yuron 5:167327a82430 700 true_hidarimae_data[0] = hidarimae_data[0];
yuron 5:167327a82430 701 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 702
yuron 5:167327a82430 703 return 0;
yuron 5:167327a82430 704 }
yuron 5:167327a82430 705 //左進(slow mode)
yuron 5:167327a82430 706 int left_PID_slow(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 707 // センサ出力値の最小、最大
yuron 5:167327a82430 708 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 709 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 710 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 711 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 712
yuron 5:167327a82430 713 // 制御量の最小、最大
yuron 5:167327a82430 714 migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 715 migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 716 hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 717 hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 718
yuron 5:167327a82430 719 // よくわからんやつ
yuron 5:167327a82430 720 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 721 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 722 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 723 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 724
yuron 5:167327a82430 725 // 目標値
yuron 5:167327a82430 726 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 727 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 728 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 729 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 730
yuron 5:167327a82430 731 // センサ出力
yuron 5:167327a82430 732 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 733 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 734 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 735 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 736
yuron 5:167327a82430 737 // 制御量(計算結果)
yuron 5:167327a82430 738 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 739 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 740 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 741 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 742
yuron 5:167327a82430 743 // 制御量をPWM値に変換
yuron 4:df334779a69e 744 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 745 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 746 true_hidarimae_data[0] = hidarimae_data[0];
yuron 4:df334779a69e 747 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 748
yuron 4:df334779a69e 749 return 0;
yuron 4:df334779a69e 750 }
yuron 5:167327a82430 751 //斜め右前(fast mode)
yuron 4:df334779a69e 752 int right_front_PID(int RB, int LF) {
yuron 5:167327a82430 753 // センサ出力値の最小、最大
yuron 5:167327a82430 754 migiusiro.setInputLimits(0, 400);
yuron 5:167327a82430 755 hidarimae.setInputLimits(0, 400);
yuron 5:167327a82430 756
yuron 5:167327a82430 757 // 制御量の最小、最大
yuron 4:df334779a69e 758 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 759 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 760
yuron 5:167327a82430 761 // よくわからんやつ
yuron 4:df334779a69e 762 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 763 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 764
yuron 5:167327a82430 765 // 目標値
yuron 4:df334779a69e 766 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 767 hidarimae.setSetPoint(LF);
yuron 5:167327a82430 768
yuron 5:167327a82430 769 // センサ出力
yuron 4:df334779a69e 770 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 771 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 772
yuron 5:167327a82430 773 // 制御量(計算結果)
yuron 4:df334779a69e 774 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 775 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 776
yuron 5:167327a82430 777 // 制御量をPWM値に変換
yuron 5:167327a82430 778 true_migimae_data[0] = 0x80;
yuron 4:df334779a69e 779 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 780 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 781 true_hidariusiro_data[0] = 0x80;
yuron 5:167327a82430 782
yuron 4:df334779a69e 783 return 0;
yuron 4:df334779a69e 784 }
yuron 5:167327a82430 785 //斜め右後(fast mode)
yuron 4:df334779a69e 786 int right_back_PID(int RF, int LB) {
yuron 5:167327a82430 787 // センサ出力値の最小、最大
yuron 4:df334779a69e 788 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 789 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 790
yuron 5:167327a82430 791 // 制御量の最小、最大
yuron 4:df334779a69e 792 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 793 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 794
yuron 5:167327a82430 795 // よくわからんやつ
yuron 4:df334779a69e 796 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 797 hidariusiro.setMode(AUTO_MODE);
yuron 5:167327a82430 798
yuron 5:167327a82430 799 // 目標値
yuron 4:df334779a69e 800 migimae.setSetPoint(RF);
yuron 4:df334779a69e 801 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 802
yuron 5:167327a82430 803 // センサ出力
yuron 4:df334779a69e 804 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 4:df334779a69e 805 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 806 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 807
yuron 5:167327a82430 808 // 制御量(計算結果)
yuron 4:df334779a69e 809 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 810 hidariusiro_data[0] = hidariusiro.compute();
yuron 5:167327a82430 811
yuron 4:df334779a69e 812 true_migimae_data[0] = migimae_data[0];
yuron 5:167327a82430 813 true_migiusiro_data[0] = 0x80;
yuron 5:167327a82430 814 true_hidarimae_data[0] = 0x80;
yuron 4:df334779a69e 815 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 816
yuron 4:df334779a69e 817 return 0;
yuron 4:df334779a69e 818 }
yuron 5:167327a82430 819 //斜め左前(fast mode)
yuron 4:df334779a69e 820 int left_front_PID(int RF, int LB) {
yuron 5:167327a82430 821 // センサ出力値の最小、最大
yuron 4:df334779a69e 822 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 823 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 824
yuron 5:167327a82430 825 // 制御量の最小、最大
yuron 4:df334779a69e 826 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 827 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 828
yuron 5:167327a82430 829 // よくわからんやつ
yuron 4:df334779a69e 830 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 831 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 832
yuron 5:167327a82430 833 // 目標値
yuron 4:df334779a69e 834 migimae.setSetPoint(RF);
yuron 4:df334779a69e 835 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 836
yuron 5:167327a82430 837 // センサ出力
yuron 4:df334779a69e 838 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 839 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 840
yuron 5:167327a82430 841 // 制御量(計算結果)
yuron 4:df334779a69e 842 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 843 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 844
yuron 5:167327a82430 845 // 制御量をPWM値に変換
yuron 4:df334779a69e 846 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 847 true_migiusiro_data[0] = 0x80;
yuron 5:167327a82430 848 true_hidarimae_data[0] = 0x80;
yuron 4:df334779a69e 849 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 850
yuron 2:c32991ba628f 851 return 0;
yuron 0:f73c1b076ae4 852 }
yuron 5:167327a82430 853 //斜め左後(fast mode)
yuron 4:df334779a69e 854 int left_back_PID(int RB, int LF) {
yuron 5:167327a82430 855 // センサ出力値の最小、最大
yuron 4:df334779a69e 856 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 857 hidarimae.setInputLimits(-400, 400);
yuron 5:167327a82430 858
yuron 5:167327a82430 859 // 制御量の最小、最大
yuron 4:df334779a69e 860 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 861 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 862
yuron 5:167327a82430 863 // よくわからんやつ
yuron 4:df334779a69e 864 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 865 hidarimae.setMode(AUTO_MODE);
yuron 5:167327a82430 866
yuron 5:167327a82430 867 // 目標値
yuron 4:df334779a69e 868 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 869 hidarimae.setSetPoint(LF);
yuron 5:167327a82430 870
yuron 5:167327a82430 871 // センサ出力
yuron 2:c32991ba628f 872 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 4:df334779a69e 873 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 874 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 875
yuron 5:167327a82430 876 // 制御量(計算結果)
yuron 4:df334779a69e 877 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 878 hidarimae_data[0] = hidarimae.compute();
yuron 5:167327a82430 879
yuron 5:167327a82430 880 true_migimae_data[0] = 0x80;
yuron 4:df334779a69e 881 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 882 true_hidarimae_data[0] = hidarimae_data[0];
yuron 5:167327a82430 883 true_hidariusiro_data[0] = 0x80;
yuron 5:167327a82430 884
yuron 2:c32991ba628f 885 return 0;
yuron 1:62b321f6c9c3 886 }
yuron 5:167327a82430 887 //右旋回(fast mode)
yuron 4:df334779a69e 888 int turn_right_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 889 // センサ出力値の最小、最大
yuron 4:df334779a69e 890 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 891 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 892 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 893 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 894
yuron 5:167327a82430 895 // 制御量の最小、最大
yuron 4:df334779a69e 896 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 897 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 898 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 899 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 900
yuron 5:167327a82430 901 // よくわからんやつ
yuron 4:df334779a69e 902 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 903 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 904 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 905 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 906
yuron 5:167327a82430 907 // 目標値
yuron 4:df334779a69e 908 migimae.setSetPoint(RF);
yuron 4:df334779a69e 909 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 910 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 911 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 912
yuron 5:167327a82430 913 // センサ出力
yuron 4:df334779a69e 914 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 915 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 916 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 917 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 918
yuron 5:167327a82430 919 // 制御量(計算結果)
yuron 4:df334779a69e 920 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 921 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 922 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 923 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 924
yuron 5:167327a82430 925 // 制御量をPWM値に変換
yuron 4:df334779a69e 926 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 927 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 928 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 929 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 930
yuron 4:df334779a69e 931 return 0;
yuron 4:df334779a69e 932 }
yuron 5:167327a82430 933 //右旋回(slow mode)
yuron 5:167327a82430 934 int turn_right_PID_slow(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 935 // センサ出力値の最小、最大
yuron 5:167327a82430 936 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 937 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 938 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 939 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 940
yuron 5:167327a82430 941 // 制御量の最小、最大
yuron 5:167327a82430 942 migimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 943 migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 944 hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 945 hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 946
yuron 5:167327a82430 947 // よくわからんやつ
yuron 5:167327a82430 948 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 949 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 950 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 951 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 952
yuron 5:167327a82430 953 // 目標値
yuron 5:167327a82430 954 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 955 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 956 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 957 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 958
yuron 5:167327a82430 959 // センサ出力
yuron 5:167327a82430 960 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 961 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 962 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 963 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 964
yuron 5:167327a82430 965 // 制御量(計算結果)
yuron 5:167327a82430 966 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 967 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 968 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 969 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 970
yuron 5:167327a82430 971 // 制御量をPWM値に変換
yuron 5:167327a82430 972 true_migimae_data[0] = migimae_data[0];
yuron 5:167327a82430 973 true_migiusiro_data[0] = migiusiro_data[0];
yuron 5:167327a82430 974 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 975 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 976
yuron 5:167327a82430 977 return 0;
yuron 5:167327a82430 978 }
yuron 5:167327a82430 979 //左旋回(fast mode)
yuron 4:df334779a69e 980 int turn_left_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 981 // センサ出力値の最小、最大
yuron 4:df334779a69e 982 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 983 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 984 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 985 hidariusiro.setInputLimits(-400, 400);
yuron 5:167327a82430 986
yuron 5:167327a82430 987 // 制御量の最小、最大
yuron 4:df334779a69e 988 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 989 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 990 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 991 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 992
yuron 5:167327a82430 993 // よくわからんやつ
yuron 4:df334779a69e 994 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 995 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 996 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 997 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 998
yuron 5:167327a82430 999 // 目標値
yuron 4:df334779a69e 1000 migimae.setSetPoint(RF);
yuron 4:df334779a69e 1001 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 1002 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 1003 hidariusiro.setSetPoint(LB);
yuron 5:167327a82430 1004
yuron 5:167327a82430 1005 // センサ出力
yuron 4:df334779a69e 1006 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 1007 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 1008 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 1009 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 1010
yuron 5:167327a82430 1011 // 制御量(計算結果)
yuron 4:df334779a69e 1012 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 1013 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 1014 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 1015 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 1016
yuron 5:167327a82430 1017 // 制御量をPWM値に変換
yuron 5:167327a82430 1018 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 1019 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 5:167327a82430 1020 true_hidarimae_data[0] = hidarimae_data[0];
yuron 5:167327a82430 1021 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 1022
yuron 5:167327a82430 1023 return 0;
yuron 5:167327a82430 1024 }
yuron 5:167327a82430 1025 //左旋回(slow mode)
yuron 5:167327a82430 1026 int turn_left_PID_slow(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 1027 // センサ出力値の最小、最大
yuron 5:167327a82430 1028 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1029 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1030 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1031 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1032
yuron 5:167327a82430 1033 // 制御量の最小、最大
yuron 5:167327a82430 1034 migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1035 migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1036 hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1037 hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1038
yuron 5:167327a82430 1039 // よくわからんやつ
yuron 5:167327a82430 1040 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1041 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1042 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1043 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1044
yuron 5:167327a82430 1045 // 目標値
yuron 5:167327a82430 1046 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 1047 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 1048 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 1049 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 1050
yuron 5:167327a82430 1051 // センサ出力
yuron 5:167327a82430 1052 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 1053 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 1054 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 1055 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 1056
yuron 5:167327a82430 1057 // 制御量(計算結果)
yuron 5:167327a82430 1058 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 1059 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 1060 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 1061 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 1062
yuron 5:167327a82430 1063 // 制御量をPWM値に変換
yuron 4:df334779a69e 1064 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 1065 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 1066 true_hidarimae_data[0] = hidarimae_data[0];
yuron 4:df334779a69e 1067 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 1068
yuron 5:167327a82430 1069 return 0;
yuron 5:167327a82430 1070 }
yuron 5:167327a82430 1071 //前前進後右旋回(slow_mode)
yuron 5:167327a82430 1072 int front_front_back_right_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 1073 // センサ出力値の最小、最大
yuron 5:167327a82430 1074 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1075 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1076 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1077 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1078
yuron 5:167327a82430 1079 // 制御量の最小、最大
yuron 5:167327a82430 1080 migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1081 migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1082 hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1083 hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1084
yuron 5:167327a82430 1085 // よくわからんやつ
yuron 5:167327a82430 1086 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1087 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1088 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1089 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1090
yuron 5:167327a82430 1091 // 目標値
yuron 5:167327a82430 1092 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 1093 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 1094 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 1095 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 1096
yuron 5:167327a82430 1097 // センサ出力
yuron 5:167327a82430 1098 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 1099 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 1100 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 1101 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 1102
yuron 5:167327a82430 1103 // 制御量(計算結果)
yuron 5:167327a82430 1104 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 1105 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 1106 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 1107 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 1108
yuron 5:167327a82430 1109 // 制御量をPWM値に変換
yuron 5:167327a82430 1110 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 1111 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 5:167327a82430 1112 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 1113 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 5:167327a82430 1114
yuron 4:df334779a69e 1115 return 0;
yuron 4:df334779a69e 1116 }
yuron 5:167327a82430 1117 //前前進後左旋回(slow_mode)
yuron 5:167327a82430 1118 int front_front_back_left_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 1119 // センサ出力値の最小、最大
yuron 5:167327a82430 1120 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1121 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1122 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1123 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1124
yuron 5:167327a82430 1125 // 制御量の最小、最大
yuron 5:167327a82430 1126 migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1127 migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1128 hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1129 hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1130
yuron 5:167327a82430 1131 // よくわからんやつ
yuron 5:167327a82430 1132 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1133 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1134 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1135 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1136
yuron 5:167327a82430 1137 // 目標値
yuron 5:167327a82430 1138 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 1139 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 1140 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 1141 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 1142
yuron 5:167327a82430 1143 // センサ出力
yuron 5:167327a82430 1144 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 1145 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 1146 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 1147 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 1148
yuron 5:167327a82430 1149 // 制御量(計算結果)
yuron 5:167327a82430 1150 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 1151 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 1152 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 1153 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 1154
yuron 5:167327a82430 1155 // 制御量をPWM値に変換
yuron 5:167327a82430 1156 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 1157 true_migiusiro_data[0] = migiusiro_data[0];
yuron 5:167327a82430 1158 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 1159 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 1160
yuron 5:167327a82430 1161 return 0;
yuron 5:167327a82430 1162 }
yuron 5:167327a82430 1163 //前右旋回後前進(slow_mode)
yuron 5:167327a82430 1164 int front_right_back_front_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 1165 // センサ出力値の最小、最大
yuron 5:167327a82430 1166 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1167 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1168 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1169 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1170
yuron 5:167327a82430 1171 // 制御量の最小、最大
yuron 5:167327a82430 1172 migimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1173 migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1174 hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1175 hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1176
yuron 5:167327a82430 1177 // よくわからんやつ
yuron 5:167327a82430 1178 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1179 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1180 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1181 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1182
yuron 5:167327a82430 1183 // 目標値
yuron 5:167327a82430 1184 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 1185 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 1186 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 1187 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 1188
yuron 5:167327a82430 1189 // センサ出力
yuron 5:167327a82430 1190 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 1191 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 1192 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 1193 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 1194
yuron 5:167327a82430 1195 // 制御量(計算結果)
yuron 5:167327a82430 1196 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 1197 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 1198 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 1199 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 1200
yuron 5:167327a82430 1201 // 制御量をPWM値に変換
yuron 5:167327a82430 1202 true_migimae_data[0] = migimae_data[0];
yuron 5:167327a82430 1203 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 5:167327a82430 1204 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 5:167327a82430 1205 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 1206
yuron 5:167327a82430 1207 return 0;
yuron 5:167327a82430 1208 }
yuron 5:167327a82430 1209 //前左旋回後前進(slow_mode)
yuron 5:167327a82430 1210 int front_left_back_front_PID(int RF, int RB, int LF, int LB) {
yuron 5:167327a82430 1211 // センサ出力値の最小、最大
yuron 5:167327a82430 1212 migimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1213 migiusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1214 hidarimae_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1215 hidariusiro_slow.setInputLimits(-400, 400);
yuron 5:167327a82430 1216
yuron 5:167327a82430 1217 // 制御量の最小、最大
yuron 5:167327a82430 1218 migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1219 migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1220 hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1221 hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron 5:167327a82430 1222
yuron 5:167327a82430 1223 // よくわからんやつ
yuron 5:167327a82430 1224 migimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1225 migiusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1226 hidarimae_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1227 hidariusiro_slow.setMode(AUTO_MODE);
yuron 5:167327a82430 1228
yuron 5:167327a82430 1229 // 目標値
yuron 5:167327a82430 1230 migimae_slow.setSetPoint(RF);
yuron 5:167327a82430 1231 migiusiro_slow.setSetPoint(RB);
yuron 5:167327a82430 1232 hidarimae_slow.setSetPoint(LF);
yuron 5:167327a82430 1233 hidariusiro_slow.setSetPoint(LB);
yuron 5:167327a82430 1234
yuron 5:167327a82430 1235 // センサ出力
yuron 5:167327a82430 1236 migimae_slow.setProcessValue(abs(migimae_rpm));
yuron 5:167327a82430 1237 migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron 5:167327a82430 1238 hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron 5:167327a82430 1239 hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron 5:167327a82430 1240
yuron 5:167327a82430 1241 // 制御量(計算結果)
yuron 5:167327a82430 1242 migimae_data[0] = migimae_slow.compute();
yuron 5:167327a82430 1243 migiusiro_data[0] = migiusiro_slow.compute();
yuron 5:167327a82430 1244 hidarimae_data[0] = hidarimae_slow.compute();
yuron 5:167327a82430 1245 hidariusiro_data[0] = hidariusiro_slow.compute();
yuron 5:167327a82430 1246
yuron 5:167327a82430 1247 // 制御量をPWM値に変換
yuron 5:167327a82430 1248 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 5:167327a82430 1249 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 5:167327a82430 1250 true_hidarimae_data[0] = hidarimae_data[0];
yuron 5:167327a82430 1251 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 5:167327a82430 1252
yuron 5:167327a82430 1253 return 0;
yuron 5:167327a82430 1254 }
yuron 5:167327a82430 1255 //ローラー
yuron 4:df334779a69e 1256 int roller_PID(int front, int back) {
yuron 4:df334779a69e 1257 front_roller.setInputLimits(-2000, 2000);
yuron 4:df334779a69e 1258 back_roller.setInputLimits(-2000, 2000);
yuron 5:167327a82430 1259
yuron 4:df334779a69e 1260 front_roller.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 1261 back_roller.setOutputLimits(0x84, 0xFF);
yuron 5:167327a82430 1262
yuron 4:df334779a69e 1263 front_roller.setMode(AUTO_MODE);
yuron 4:df334779a69e 1264 back_roller.setMode(AUTO_MODE);
yuron 5:167327a82430 1265
yuron 4:df334779a69e 1266 front_roller.setSetPoint(front);
yuron 4:df334779a69e 1267 back_roller.setSetPoint(back);
yuron 5:167327a82430 1268
yuron 4:df334779a69e 1269 front_roller.setProcessValue(abs(front_roller_rpm));
yuron 4:df334779a69e 1270 back_roller.setProcessValue(abs(back_roller_rpm));
yuron 5:167327a82430 1271
yuron 4:df334779a69e 1272 front_roller_data[0] = front_roller.compute();
yuron 4:df334779a69e 1273 back_roller_data[0] = back_roller.compute();
yuron 5:167327a82430 1274
yuron 4:df334779a69e 1275 return 0;
yuron 4:df334779a69e 1276 }
yuron 5:167327a82430 1277 //ラインセンサ基板との通信用関数
yuron 4:df334779a69e 1278 void linetrace() {
yuron 4:df334779a69e 1279 line_state = photo.getc();
yuron 5:167327a82430 1280 //7bit目が1だったら7bit目を0に戻す
yuron 5:167327a82430 1281 if((0b10000000 & line_state) == 0b10000000) {
yuron 5:167327a82430 1282 back_line_state = 0b01111111 & line_state;
yuron 8:3df97287c825 1283
yuron 6:5a051a378e42 1284 } else {
yuron 5:167327a82430 1285 front_line_state = line_state;
yuron 5:167327a82430 1286 }
yuron 5:167327a82430 1287 //4byte以上は出力できないよ
yuron 5:167327a82430 1288 //pc.printf("%d\n\r", line_state);
yuron 5:167327a82430 1289 //pc.printf("NAMA %d, front: %d, back: %d\n\r", line_state, front_line_state, back_line_state);
yuron 4:df334779a69e 1290 }
yuron 5:167327a82430 1291 //超音波センサ用関数
yuron 4:df334779a69e 1292 void ultrasonic() {
yuron 5:167327a82430 1293 //超音波発射
yuron 4:df334779a69e 1294 sonic.start();
yuron 5:167327a82430 1295 //10ms待機
yuron 4:df334779a69e 1296 wait(0.01);
yuron 4:df334779a69e 1297 //get_dist_cm関数は、計測から得られた距離(cm)を返します。
yuron 4:df334779a69e 1298 distance = sonic.get_dist_cm();
yuron 5:167327a82430 1299 //pc.printf("%d[cm]\r\n", distance);
yuron 4:df334779a69e 1300 }
yuron 4:df334779a69e 1301 int main(void) {
yuron 5:167327a82430 1302 //こんにちは世界
yuron 4:df334779a69e 1303 pc.printf("HelloWorld\n");
yuron 5:167327a82430 1304 //緊急停止用信号ピンをLow
yuron 0:f73c1b076ae4 1305 emergency = 0;
yuron 5:167327a82430 1306 //回転数取得関数のタイマ割り込み(40ms)
yuron 6:5a051a378e42 1307 get_rpm.attach_us(&flip, 40000);
yuron 5:167327a82430 1308
yuron 5:167327a82430 1309 //フォトセンサ制御基板との通信ボーレートの設定(115200)
yuron 4:df334779a69e 1310 photo.baud(115200);
yuron 5:167327a82430 1311 pc.baud(460800);
yuron 5:167327a82430 1312 //フォトセンサ制御基板との通信関数の受信割り込み
yuron 4:df334779a69e 1313 photo.attach(linetrace, Serial::RxIrq);
yuron 5:167327a82430 1314 //サイドチェンジボタンをPullDownに設定
yuron 4:df334779a69e 1315 side.mode(PullDown);
yuron 5:167327a82430 1316 limit.mode(PullDown);
yuron 5:167327a82430 1317 //超音波センサの温度設定
yuron 4:df334779a69e 1318 // setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
yuron 4:df334779a69e 1319 sonic.setTemp(25);
yuron 5:167327a82430 1320
yuron 5:167327a82430 1321 //各MD, エアシリ制御基板へ起動後0.1秒間0x80を送りつける(通信切断防止用)
yuron 2:c32991ba628f 1322 send_data[0] = 0x80;
yuron 4:df334779a69e 1323 i2c.write(0x10, send_data, 1);
yuron 4:df334779a69e 1324 i2c.write(0x12, send_data, 1);
yuron 4:df334779a69e 1325 i2c.write(0x14, send_data, 1);
yuron 4:df334779a69e 1326 i2c.write(0x16, send_data, 1);
yuron 3:1223cab367d9 1327 i2c.write(0x20, send_data, 1);
yuron 3:1223cab367d9 1328 i2c.write(0x22, send_data, 1);
yuron 4:df334779a69e 1329 i2c.write(0x30, send_data, 1);
yuron 4:df334779a69e 1330 i2c.write(0x40, send_data, 1);
yuron 0:f73c1b076ae4 1331 wait(0.1);
yuron 5:167327a82430 1332
yuron 8:3df97287c825 1333 myled1 = 0;
yuron 8:3df97287c825 1334 myled2 = 0;
yuron 8:3df97287c825 1335 myled3 = 0;
yuron 8:3df97287c825 1336 myled4 = 0;
yuron 8:3df97287c825 1337 myled5 = 0;
yuron 8:3df97287c825 1338
yuron 4:df334779a69e 1339 while(1) {
yuron 5:167327a82430 1340
yuron 5:167327a82430 1341 //超音波取得関数の呼び出し
yuron 4:df334779a69e 1342 ultrasonic();
yuron 6:5a051a378e42 1343 start_LED = 1;
yuron 8:3df97287c825 1344
yuron 5:167327a82430 1345 //赤ゾーン選択
yuron 5:167327a82430 1346 if(side == red){
yuron 4:df334779a69e 1347 red_side = 1;
yuron 4:df334779a69e 1348 blue_side = 0;
yuron 5:167327a82430 1349 //青ゾーン選択
yuron 4:df334779a69e 1350 } else {
yuron 4:df334779a69e 1351 red_side = 0;
yuron 4:df334779a69e 1352 blue_side = 1;
yuron 4:df334779a69e 1353 }
yuron 5:167327a82430 1354
yuron 6:5a051a378e42 1355 //スタートボタン押したらエアシリ伸びる
yuron 5:167327a82430 1356 if(start_button == 0){
yuron 8:3df97287c825 1357 start_flag = 1;
yuron 8:3df97287c825 1358 }
yuron 8:3df97287c825 1359 //2段下段スイッチでフラグ立てる
yuron 8:3df97287c825 1360 if(user_switch1 == 0) {
yuron 8:3df97287c825 1361 low_grade_flag = 1;
yuron 4:df334779a69e 1362 myled1 = 1;
yuron 5:167327a82430 1363 }
yuron 8:3df97287c825 1364 //2段上段スイッチでフラグ立てる
yuron 8:3df97287c825 1365 if(user_switch2 == 0) {
yuron 8:3df97287c825 1366 high_grade_flag = 1;
yuron 5:167327a82430 1367 myled2 = 1;
yuron 5:167327a82430 1368 }
yuron 8:3df97287c825 1369 //移動(低)スイッチでフラグ立てる
yuron 8:3df97287c825 1370 if(user_switch3 == 0) {
yuron 8:3df97287c825 1371 low_table_flag = 1;
yuron 5:167327a82430 1372 myled3 = 1;
yuron 5:167327a82430 1373 }
yuron 5:167327a82430 1374
yuron 8:3df97287c825 1375 //移動(中)スイッチでフラグ立てる
yuron 8:3df97287c825 1376 /*
yuron 8:3df97287c825 1377 if(user_switch4 == 0) {
yuron 8:3df97287c825 1378 medium_table_flag = 1;
yuron 8:3df97287c825 1379 myled4 = 1;
yuron 8:3df97287c825 1380 }
yuron 8:3df97287c825 1381 */
yuron 8:3df97287c825 1382
yuron 8:3df97287c825 1383 //移動(高)スイッチでフラグ立てる
yuron 8:3df97287c825 1384 if(user_switch5 == 0) {
yuron 8:3df97287c825 1385 high_table_flag = 1;
yuron 8:3df97287c825 1386 myled5 = 1;
yuron 8:3df97287c825 1387 }
yuron 8:3df97287c825 1388
yuron 8:3df97287c825 1389 //フォトインタラプタ検知でLチカ
yuron 8:3df97287c825 1390 if(photo_interrupter == 1) {
yuron 8:3df97287c825 1391 myled4 = 1;
yuron 8:3df97287c825 1392 } else {
yuron 8:3df97287c825 1393 myled4 = 0;
yuron 8:3df97287c825 1394 }
yuron 8:3df97287c825 1395 //6発発射したら弾倉切り替え
yuron 8:3df97287c825 1396 if(mouted_bottles == 6) {
yuron 8:3df97287c825 1397 if(photo_interrupter == 0) {
yuron 8:3df97287c825 1398 loading_data[0] = 0x0C;
yuron 8:3df97287c825 1399 i2c.write(0x30, loading_data, 1);
yuron 8:3df97287c825 1400
yuron 8:3df97287c825 1401 } else {
yuron 8:3df97287c825 1402 loading_data[0] = 0x80;
yuron 8:3df97287c825 1403 i2c.write(0x30, loading_data, 1);
yuron 8:3df97287c825 1404 }
yuron 8:3df97287c825 1405 }
yuron 8:3df97287c825 1406
yuron 8:3df97287c825 1407 pc.printf("%3d %3d %3d %3d %3d %3d\n\r", front_line_state, back_line_state, line_state_pettern, abs(measure_pulse), distance, mouted_bottles);
yuron 5:167327a82430 1408
yuron 7:7f16fb8b0192 1409 //スタートボタンで動作開始
yuron 7:7f16fb8b0192 1410 if(start_flag == 1) {
yuron 8:3df97287c825 1411
yuron 7:7f16fb8b0192 1412 //ローラー回転開始
yuron 7:7f16fb8b0192 1413 roller_PID(1006, 928);
yuron 7:7f16fb8b0192 1414 i2c.write(0x20, front_roller_data, 1, false);
yuron 7:7f16fb8b0192 1415 i2c.write(0x22, back_roller_data, 1, false);
yuron 7:7f16fb8b0192 1416 wait_us(20);
yuron 8:3df97287c825 1417
yuron 8:3df97287c825 1418 //2段下段
yuron 8:3df97287c825 1419 if(low_grade_flag == 0) {
yuron 8:3df97287c825 1420 //パルスがnを満たすまで無条件で横移動
yuron 8:3df97287c825 1421 //if(abs(measure_pulse) < 100) {
yuron 8:3df97287c825 1422 /*
yuron 8:3df97287c825 1423 start_timer.start();
yuron 8:3df97287c825 1424 if(start_timer.read() <= 1.0f) {
yuron 8:3df97287c825 1425 //青ゾーンの時指定のパルス値まで無条件高速右移動
yuron 8:3df97287c825 1426 if(side == blue) {
yuron 8:3df97287c825 1427 line_state_pettern = right_fast;
yuron 8:3df97287c825 1428 //赤ゾーンの時指定のパルス値まで無条件高速左移動
yuron 8:3df97287c825 1429 }
yuron 8:3df97287c825 1430 else if(side == red) {
yuron 8:3df97287c825 1431 line_state_pettern = left_fast;
yuron 8:3df97287c825 1432 }
yuron 8:3df97287c825 1433 } else {
yuron 8:3df97287c825 1434 //else if(start_timer.read() > 2.0f) {
yuron 8:3df97287c825 1435 */
yuron 8:3df97287c825 1436 //一定の距離を超えたらゆっくり移動してライン見つけたら(ry
yuron 8:3df97287c825 1437 //} else {
yuron 8:3df97287c825 1438 //距離が11cm~29cmだったらトレースせずに停止
yuron 8:3df97287c825 1439 if(distance >= 20 && distance <= 25) {
yuron 8:3df97287c825 1440 //タイマスタート
yuron 8:3df97287c825 1441 timer.start();
yuron 8:3df97287c825 1442 //トレース方向の反転
yuron 8:3df97287c825 1443 trace_direction = 1;
yuron 8:3df97287c825 1444
yuron 8:3df97287c825 1445 //発射距離に到達して1秒待って発射
yuron 8:3df97287c825 1446 if(timer.read() > 1.0f && timer.read() <= 1.5f) {
yuron 8:3df97287c825 1447 cylinder_data[0] = 0x0F;
yuron 8:3df97287c825 1448 i2c.write(0x40, cylinder_data, 1);
yuron 8:3df97287c825 1449 mouted_bottles = mouted_bottles + 1;
yuron 8:3df97287c825 1450 }
yuron 8:3df97287c825 1451 //発射して1秒たったら後進開始
yuron 8:3df97287c825 1452 else if(timer.read() > 1.5f) {
yuron 8:3df97287c825 1453 line_state_pettern = back_trace;
yuron 8:3df97287c825 1454 cylinder_data[0] = 0x80;
yuron 8:3df97287c825 1455 i2c.write(0x40, cylinder_data, 1);
yuron 8:3df97287c825 1456 pc.printf("start_back!\n\r");
yuron 8:3df97287c825 1457 } else {
yuron 8:3df97287c825 1458 line_state_pettern = stop;
yuron 8:3df97287c825 1459 cylinder_data[0] = 0x80;
yuron 8:3df97287c825 1460 i2c.write(0x40, cylinder_data, 1);
yuron 8:3df97287c825 1461 }
yuron 8:3df97287c825 1462 //上記以外の距離でライントレースするよん
yuron 7:7f16fb8b0192 1463 } else {
yuron 8:3df97287c825 1464 //タイマリセット
yuron 8:3df97287c825 1465 timer.reset();
yuron 8:3df97287c825 1466
yuron 8:3df97287c825 1467 //ライン検知するまで右移動
yuron 8:3df97287c825 1468 if(back_line_state == 0) {
yuron 8:3df97287c825 1469 //青ゾーンの時ライン検知するまで右移動
yuron 8:3df97287c825 1470 if(side == blue) {
yuron 8:3df97287c825 1471 line_state_pettern = right_slow;
yuron 8:3df97287c825 1472 //赤ゾーンの時ライン検知するまで左移動
yuron 8:3df97287c825 1473 } else {
yuron 8:3df97287c825 1474 line_state_pettern = left_slow;
yuron 8:3df97287c825 1475 }
yuron 8:3df97287c825 1476 }
yuron 8:3df97287c825 1477
yuron 8:3df97287c825 1478 //ライン検知するまで右移動
yuron 8:3df97287c825 1479 if((front_line_state == 0) && (back_line_state == 0)) {
yuron 8:3df97287c825 1480 //青ゾーンの時ライン検知するまで右移動
yuron 8:3df97287c825 1481 if(side == blue) {
yuron 8:3df97287c825 1482 line_state_pettern = right_slow;
yuron 8:3df97287c825 1483 //赤ゾーンの時ライン検知するまで左移動
yuron 8:3df97287c825 1484 } else {
yuron 8:3df97287c825 1485 line_state_pettern = left_slow;
yuron 8:3df97287c825 1486 }
yuron 8:3df97287c825 1487 }
yuron 8:3df97287c825 1488
yuron 8:3df97287c825 1489 //前はライン検知してるけど後ろは検知できない時左移動
yuron 8:3df97287c825 1490 else if((front_line_state >= 1) && (back_line_state == 0)) {
yuron 8:3df97287c825 1491 if(side == red) {
yuron 8:3df97287c825 1492 line_state_pettern = left_super_slow;
yuron 8:3df97287c825 1493 }
yuron 8:3df97287c825 1494 else if(side == blue) {
yuron 8:3df97287c825 1495 line_state_pettern = right_super_slow;
yuron 8:3df97287c825 1496 }
yuron 8:3df97287c825 1497 }
yuron 8:3df97287c825 1498 //前はライン検知できないけど後ろは検知してる時左移動
yuron 8:3df97287c825 1499 else if((front_line_state == 0) && (back_line_state >= 1)) {
yuron 8:3df97287c825 1500 if(side == red) {
yuron 8:3df97287c825 1501 line_state_pettern = left_super_slow;
yuron 8:3df97287c825 1502 }
yuron 8:3df97287c825 1503 else if(side == blue) {
yuron 8:3df97287c825 1504 line_state_pettern = right_super_slow;
yuron 8:3df97287c825 1505 }
yuron 8:3df97287c825 1506 }
yuron 8:3df97287c825 1507
yuron 8:3df97287c825 1508 //前が右寄りの時
yuron 8:3df97287c825 1509 else if((front_line_state <= 5) && (front_line_state != 0)) {
yuron 6:5a051a378e42 1510
yuron 8:3df97287c825 1511 //前も後ろも右寄りの時右移動
yuron 8:3df97287c825 1512 if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron 8:3df97287c825 1513 line_state_pettern = right_super_slow;
yuron 8:3df97287c825 1514 }
yuron 8:3df97287c825 1515 //前が右寄りで後ろが真ん中の時前右旋回後ろ前進
yuron 8:3df97287c825 1516 else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron 8:3df97287c825 1517 line_state_pettern = front_right_back_front;
yuron 8:3df97287c825 1518 }
yuron 8:3df97287c825 1519 //前が右寄りで後ろが左寄りの時左旋回
yuron 8:3df97287c825 1520 else if((back_line_state <= 5) && (back_line_state != 0)) {
yuron 8:3df97287c825 1521 line_state_pettern = turn_left;
yuron 8:3df97287c825 1522 }
yuron 8:3df97287c825 1523
yuron 8:3df97287c825 1524 }
yuron 8:3df97287c825 1525
yuron 8:3df97287c825 1526 //前が真ん中寄りの時
yuron 8:3df97287c825 1527 else if((front_line_state >= 6) && (front_line_state <= 12)) {
yuron 8:3df97287c825 1528
yuron 8:3df97287c825 1529 //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
yuron 8:3df97287c825 1530 if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron 8:3df97287c825 1531 line_state_pettern = front_front_back_right;
yuron 8:3df97287c825 1532 }
yuron 8:3df97287c825 1533 //前も後ろも真ん中の時前進
yuron 8:3df97287c825 1534 else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron 8:3df97287c825 1535 if(trace_direction == 0) {
yuron 8:3df97287c825 1536 //20cm未満で後進
yuron 8:3df97287c825 1537 if(distance < 20 && distance != 0) {
yuron 8:3df97287c825 1538 line_state_pettern = back_trace;
yuron 8:3df97287c825 1539 }
yuron 8:3df97287c825 1540 //25cmより遠くて前進
yuron 8:3df97287c825 1541 else if(distance > 25) {
yuron 8:3df97287c825 1542 line_state_pettern = front_trace;
yuron 8:3df97287c825 1543 } else {
yuron 8:3df97287c825 1544 line_state_pettern = front_trace;
yuron 8:3df97287c825 1545 }
yuron 8:3df97287c825 1546 }
yuron 8:3df97287c825 1547 else if(trace_direction == 1) {
yuron 8:3df97287c825 1548 line_state_pettern = back_trace;
yuron 8:3df97287c825 1549 }
yuron 8:3df97287c825 1550 }
yuron 8:3df97287c825 1551 //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
yuron 8:3df97287c825 1552 else if((back_line_state <= 5) && (back_line_state != 0)) {
yuron 8:3df97287c825 1553 line_state_pettern = front_front_back_left;
yuron 8:3df97287c825 1554 }
yuron 8:3df97287c825 1555 }
yuron 8:3df97287c825 1556 //前が左寄りの時
yuron 8:3df97287c825 1557 else if((front_line_state >= 13) && (front_line_state <= 17)) {
yuron 8:3df97287c825 1558
yuron 8:3df97287c825 1559 //前が左寄りで後ろが右寄りの時右旋回
yuron 8:3df97287c825 1560 if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron 8:3df97287c825 1561 line_state_pettern = turn_right;
yuron 8:3df97287c825 1562 }
yuron 8:3df97287c825 1563 //前が左寄りで後ろが真ん中の時前左旋回後ろ前進
yuron 8:3df97287c825 1564 else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron 8:3df97287c825 1565 line_state_pettern = front_left_back_front;
yuron 8:3df97287c825 1566 }
yuron 8:3df97287c825 1567 //前が左よりで後ろも左寄りの時左移動
yuron 8:3df97287c825 1568 else if((back_line_state <= 5) && (back_line_state != 0)) {
yuron 8:3df97287c825 1569 line_state_pettern = left_super_slow;
yuron 8:3df97287c825 1570
yuron 8:3df97287c825 1571 //それ以外
yuron 7:7f16fb8b0192 1572 } else {
yuron 8:3df97287c825 1573 line_state_pettern = stop;
yuron 7:7f16fb8b0192 1574 }
yuron 5:167327a82430 1575 }
yuron 8:3df97287c825 1576 }
yuron 8:3df97287c825 1577 //}
yuron 8:3df97287c825 1578 } else {
yuron 8:3df97287c825 1579 line_state_pettern = stop;
yuron 8:3df97287c825 1580 }
yuron 8:3df97287c825 1581
yuron 8:3df97287c825 1582 //lateral_fragが0(初期状態)の時
yuron 8:3df97287c825 1583 if(lateral_frag == 0) {
yuron 8:3df97287c825 1584 //リミットONでlateral_frag = 1
yuron 8:3df97287c825 1585 if(limit.read() == 0) {
yuron 8:3df97287c825 1586 lateral_frag = 1;
yuron 8:3df97287c825 1587 }
yuron 8:3df97287c825 1588 }
yuron 8:3df97287c825 1589 //リミットがONの時(1回目)
yuron 8:3df97287c825 1590 if(lateral_frag == 1) {
yuron 8:3df97287c825 1591 //トレース方向の反転(前進)
yuron 8:3df97287c825 1592 trace_direction = 0;
yuron 8:3df97287c825 1593 back_timer1.start();
yuron 8:3df97287c825 1594 if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
yuron 8:3df97287c825 1595 if(side == blue) {
yuron 8:3df97287c825 1596 line_state_pettern = right_slow;
yuron 8:3df97287c825 1597 }
yuron 8:3df97287c825 1598 else if(side == red) {
yuron 8:3df97287c825 1599 line_state_pettern = left_slow;
yuron 8:3df97287c825 1600 }
yuron 8:3df97287c825 1601 }
yuron 8:3df97287c825 1602 else if(back_timer1.read() > 1.5f) {
yuron 8:3df97287c825 1603 back_timer1.reset();
yuron 8:3df97287c825 1604 lateral_frag = 2;
yuron 8:3df97287c825 1605 }
yuron 8:3df97287c825 1606 }
yuron 8:3df97287c825 1607 else if(lateral_frag == 2) {
yuron 8:3df97287c825 1608 if(limit.read() == 0) {
yuron 8:3df97287c825 1609 lateral_frag = 3;
yuron 8:3df97287c825 1610 }
yuron 8:3df97287c825 1611 }
yuron 8:3df97287c825 1612 //リミットがONの時(2回目)
yuron 8:3df97287c825 1613 else if(lateral_frag == 3) {
yuron 8:3df97287c825 1614 trace_direction = 0;
yuron 8:3df97287c825 1615 back_timer2.start();
yuron 8:3df97287c825 1616 if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
yuron 8:3df97287c825 1617 if(side == blue) {
yuron 8:3df97287c825 1618 line_state_pettern = right_slow;
yuron 7:7f16fb8b0192 1619 }
yuron 8:3df97287c825 1620 else if(side == red) {
yuron 8:3df97287c825 1621 line_state_pettern = left_slow;
yuron 5:167327a82430 1622 }
yuron 8:3df97287c825 1623 }
yuron 8:3df97287c825 1624 else if(back_timer2.read() > 0.8f) {
yuron 8:3df97287c825 1625 back_timer2.reset();
yuron 8:3df97287c825 1626 lateral_frag = 4;
yuron 8:3df97287c825 1627 }
yuron 8:3df97287c825 1628 }
yuron 8:3df97287c825 1629 else if(lateral_frag == 4) {
yuron 8:3df97287c825 1630 if(limit.read() == 0) {
yuron 8:3df97287c825 1631 //上でback_timer3を使用しているためここでリセットをかける
yuron 8:3df97287c825 1632 //back_timer3.reset();
yuron 8:3df97287c825 1633 lateral_frag = 5;
yuron 8:3df97287c825 1634 }
yuron 8:3df97287c825 1635 }
yuron 8:3df97287c825 1636 //リミットがONの時(3回目)
yuron 8:3df97287c825 1637 else if(lateral_frag == 5) {
yuron 8:3df97287c825 1638 trace_direction = 0;
yuron 8:3df97287c825 1639 back_timer3.start();
yuron 8:3df97287c825 1640 if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
yuron 8:3df97287c825 1641 if(side == blue) {
yuron 8:3df97287c825 1642 line_state_pettern = right_slow;
yuron 8:3df97287c825 1643 }
yuron 8:3df97287c825 1644 else if(side == red) {
yuron 8:3df97287c825 1645 line_state_pettern = left_slow;
yuron 8:3df97287c825 1646 }
yuron 8:3df97287c825 1647 }
yuron 8:3df97287c825 1648 else if(back_timer3.read() > 0.8f) {
yuron 8:3df97287c825 1649 back_timer3.reset();
yuron 8:3df97287c825 1650 lateral_frag = 6;
yuron 8:3df97287c825 1651 }
yuron 8:3df97287c825 1652 }
yuron 8:3df97287c825 1653 else if(lateral_frag == 6) {
yuron 8:3df97287c825 1654 if(limit.read() == 0) {
yuron 8:3df97287c825 1655 lateral_frag = 7;
yuron 8:3df97287c825 1656 }
yuron 8:3df97287c825 1657 }
yuron 8:3df97287c825 1658 //リミットがONの時(4回目)
yuron 8:3df97287c825 1659 else if(lateral_frag == 7) {
yuron 8:3df97287c825 1660 //スタートゾーンに近づいたら減速
yuron 8:3df97287c825 1661 if(abs(measure_pulse) < 1200) {
yuron 8:3df97287c825 1662 if(side == blue) {
yuron 8:3df97287c825 1663 line_state_pettern = left_super_slow;
yuron 6:5a051a378e42 1664 }
yuron 8:3df97287c825 1665 else if(side == red) {
yuron 8:3df97287c825 1666 line_state_pettern = right_super_slow;
yuron 8:3df97287c825 1667 }
yuron 8:3df97287c825 1668 }
yuron 8:3df97287c825 1669 else if(abs(measure_pulse) < 800) {
yuron 8:3df97287c825 1670 line_state_pettern = stop;
yuron 8:3df97287c825 1671 } else {
yuron 8:3df97287c825 1672 if(side == blue) {
yuron 8:3df97287c825 1673 line_state_pettern = left_slow;
yuron 8:3df97287c825 1674 }
yuron 8:3df97287c825 1675 else if(side == red) {
yuron 8:3df97287c825 1676 line_state_pettern = right_slow;
yuron 5:167327a82430 1677 }
yuron 8:3df97287c825 1678 }
yuron 8:3df97287c825 1679 }
yuron 8:3df97287c825 1680 }
yuron 8:3df97287c825 1681
yuron 8:3df97287c825 1682 /*
yuron 8:3df97287c825 1683 //2段上段
yuron 8:3df97287c825 1684 if(high_grade_flag == 0) {
yuron 8:3df97287c825 1685
yuron 8:3df97287c825 1686 //パルスがnを満たすまで無条件で横移動
yuron 8:3df97287c825 1687 if(abs(measure_pulse) < 10000 && abs(measure_pulse) >= 0) {
yuron 8:3df97287c825 1688 right_PID(255, 300, 255, 300);
yuron 8:3df97287c825 1689 i2c.write(0x10, true_migimae_data, 1, false);
yuron 8:3df97287c825 1690 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 8:3df97287c825 1691 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 8:3df97287c825 1692 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 8:3df97287c825 1693 wait_us(20);
yuron 8:3df97287c825 1694
yuron 8:3df97287c825 1695 //一定の距離を超えたらゆっくり移動してライン見つけたら(ry
yuron 8:3df97287c825 1696 } else {
yuron 8:3df97287c825 1697
yuron 8:3df97287c825 1698 //距離が11cm~29cmだったらトレースせずに停止
yuron 8:3df97287c825 1699 if(distance > 10 && distance < 15) {
yuron 8:3df97287c825 1700 //タイマスタート
yuron 8:3df97287c825 1701 timer.start();
yuron 8:3df97287c825 1702 //トレース方向の反転
yuron 8:3df97287c825 1703 trace_direction = 1;
yuron 8:3df97287c825 1704
yuron 8:3df97287c825 1705 //発射距離に到達して1秒待って発射
yuron 8:3df97287c825 1706 if(timer.read() > 1.0f && timer.read() <= 1.5f) {
yuron 8:3df97287c825 1707 cylinder_data[0] = 0x0F;
yuron 8:3df97287c825 1708 i2c.write(0x40, cylinder_data, 1);
yuron 8:3df97287c825 1709 mouted_bottles--;
yuron 5:167327a82430 1710 }
yuron 8:3df97287c825 1711 //発射して1秒たったら後進開始
yuron 8:3df97287c825 1712 else if(timer.read() > 1.5f) {
yuron 6:5a051a378e42 1713 line_state_pettern = back_trace;
yuron 8:3df97287c825 1714 cylinder_data[0] = 0x0F;
yuron 8:3df97287c825 1715 i2c.write(0x40, cylinder_data, 1);
yuron 8:3df97287c825 1716 pc.printf("start_back!\n\r");
yuron 8:3df97287c825 1717 } else {
yuron 8:3df97287c825 1718 line_state_pettern = stop;
yuron 8:3df97287c825 1719 cylinder_data[0] = 0x0F;
yuron 8:3df97287c825 1720 i2c.write(0x40, cylinder_data, 1);
yuron 5:167327a82430 1721 }
yuron 8:3df97287c825 1722
yuron 8:3df97287c825 1723 //上記以外の距離でライントレースするよん
yuron 8:3df97287c825 1724 } else {
yuron 8:3df97287c825 1725 //タイマリセット
yuron 8:3df97287c825 1726 timer.reset();
yuron 8:3df97287c825 1727
yuron 8:3df97287c825 1728 //ライン検知するまで右移動
yuron 8:3df97287c825 1729 if(back_line_state == 0) {
yuron 8:3df97287c825 1730 //青ゾーンの時ライン検知するまで右移動
yuron 8:3df97287c825 1731 if(side == blue) {
yuron 8:3df97287c825 1732 line_state_pettern = right_slow;
yuron 8:3df97287c825 1733 //赤ゾーンの時ライン検知するまで左移動
yuron 8:3df97287c825 1734 } else {
yuron 8:3df97287c825 1735 line_state_pettern = left_slow;
yuron 8:3df97287c825 1736 }
yuron 8:3df97287c825 1737 }
yuron 8:3df97287c825 1738
yuron 8:3df97287c825 1739 //ライン検知するまで右移動
yuron 8:3df97287c825 1740 if((front_line_state == 0) && (back_line_state == 0)) {
yuron 8:3df97287c825 1741 //青ゾーンの時ライン検知するまで右移動
yuron 8:3df97287c825 1742 if(side == blue) {
yuron 8:3df97287c825 1743 line_state_pettern = right_slow;
yuron 8:3df97287c825 1744 //赤ゾーンの時ライン検知するまで左移動
yuron 8:3df97287c825 1745 } else {
yuron 8:3df97287c825 1746 line_state_pettern = left_slow;
yuron 8:3df97287c825 1747 }
yuron 8:3df97287c825 1748 }
yuron 8:3df97287c825 1749 */
yuron 8:3df97287c825 1750
yuron 8:3df97287c825 1751 /*
yuron 8:3df97287c825 1752 //前はライン検知してるけど後ろは検知できない時右移動
yuron 8:3df97287c825 1753 else if((front_line_state >= 1) && (back_line_state == 0)) {
yuron 7:7f16fb8b0192 1754 line_state_pettern = right_super_slow;
yuron 7:7f16fb8b0192 1755 }
yuron 8:3df97287c825 1756 //前はライン検知できないけど後ろは検知してる時右移動
yuron 8:3df97287c825 1757 else if((front_line_state == 0) && (back_line_state >= 1)) {
yuron 8:3df97287c825 1758 line_state_pettern = right_super_slow;
yuron 7:7f16fb8b0192 1759 }
yuron 8:3df97287c825 1760 */
yuron 8:3df97287c825 1761
yuron 8:3df97287c825 1762 /*
yuron 8:3df97287c825 1763 //前が右寄りの時
yuron 8:3df97287c825 1764 else if((front_line_state <= 5) && (front_line_state != 0)) {
yuron 8:3df97287c825 1765
yuron 8:3df97287c825 1766 //前も後ろも右寄りの時右移動
yuron 8:3df97287c825 1767 if((back_line_state <= 5) && (back_line_state != 0)) {
yuron 8:3df97287c825 1768 line_state_pettern = right_super_slow;
yuron 8:3df97287c825 1769 }
yuron 8:3df97287c825 1770 //前が右寄りで後ろが真ん中の時前右旋回後ろ前進
yuron 8:3df97287c825 1771 else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron 8:3df97287c825 1772 line_state_pettern = front_right_back_front;
yuron 8:3df97287c825 1773 }
yuron 8:3df97287c825 1774 //前が右寄りで後ろが左寄りの時左旋回
yuron 8:3df97287c825 1775 else if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron 8:3df97287c825 1776 line_state_pettern = turn_left;
yuron 8:3df97287c825 1777 }
yuron 8:3df97287c825 1778
yuron 7:7f16fb8b0192 1779 }
yuron 8:3df97287c825 1780 //前が真ん中寄りの時
yuron 8:3df97287c825 1781 else if((front_line_state >= 6) && (front_line_state <= 12)) {
yuron 8:3df97287c825 1782
yuron 8:3df97287c825 1783 //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
yuron 8:3df97287c825 1784 if((back_line_state <= 5) && (back_line_state != 0)) {
yuron 8:3df97287c825 1785 line_state_pettern = front_front_back_right;
yuron 7:7f16fb8b0192 1786 }
yuron 8:3df97287c825 1787 //前も後ろも真ん中の時前進
yuron 8:3df97287c825 1788 else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron 8:3df97287c825 1789 if(trace_direction == 0) {
yuron 8:3df97287c825 1790 line_state_pettern = front_trace;
yuron 8:3df97287c825 1791 }
yuron 8:3df97287c825 1792 else if(trace_direction == 1) {
yuron 8:3df97287c825 1793 line_state_pettern = back_trace;
yuron 8:3df97287c825 1794 }
yuron 8:3df97287c825 1795 }
yuron 8:3df97287c825 1796 //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
yuron 8:3df97287c825 1797 else if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron 8:3df97287c825 1798 line_state_pettern = front_front_back_left;
yuron 7:7f16fb8b0192 1799 }
yuron 7:7f16fb8b0192 1800 }
yuron 8:3df97287c825 1801 //前が左寄りの時
yuron 8:3df97287c825 1802 else if((front_line_state >= 13) && (front_line_state <= 17)) {
yuron 8:3df97287c825 1803
yuron 8:3df97287c825 1804 //前が左寄りで後ろが右寄りの時右旋回
yuron 8:3df97287c825 1805 if((back_line_state <= 5) && (back_line_state != 0)) {
yuron 8:3df97287c825 1806 line_state_pettern = turn_right;
yuron 8:3df97287c825 1807 }
yuron 8:3df97287c825 1808 //前が左寄りで後ろが真ん中の時前左旋回後ろ前進
yuron 8:3df97287c825 1809 else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron 8:3df97287c825 1810 line_state_pettern = front_left_back_front;
yuron 8:3df97287c825 1811 }
yuron 8:3df97287c825 1812 //前が左よりで後ろも左寄りの時左移動
yuron 8:3df97287c825 1813 else if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron 8:3df97287c825 1814 line_state_pettern = left_super_slow;
yuron 8:3df97287c825 1815 }
yuron 8:3df97287c825 1816
yuron 8:3df97287c825 1817 //それ以外
yuron 8:3df97287c825 1818 } else {
yuron 8:3df97287c825 1819 line_state_pettern = stop;
yuron 7:7f16fb8b0192 1820 }
yuron 6:5a051a378e42 1821 }
yuron 6:5a051a378e42 1822 }
yuron 8:3df97287c825 1823 } else {
yuron 8:3df97287c825 1824 line_state_pettern = stop;
yuron 5:167327a82430 1825 }
yuron 8:3df97287c825 1826
yuron 8:3df97287c825 1827 //lateral_fragが0(初期状態)の時
yuron 8:3df97287c825 1828 if(lateral_frag == 0) {
yuron 8:3df97287c825 1829 //リミットONでlateral_frag = 1
yuron 8:3df97287c825 1830 if(limit.read() == 0) {
yuron 8:3df97287c825 1831 lateral_frag = 1;
yuron 6:5a051a378e42 1832 }
yuron 5:167327a82430 1833 }
yuron 8:3df97287c825 1834 //リミットがONの時(1回目)
yuron 8:3df97287c825 1835 if(lateral_frag == 1) {
yuron 8:3df97287c825 1836 //トレース方向の反転(前進)
yuron 8:3df97287c825 1837 trace_direction = 0;
yuron 8:3df97287c825 1838 back_timer1.start();
yuron 8:3df97287c825 1839 if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
yuron 8:3df97287c825 1840 if(side == blue) {
yuron 8:3df97287c825 1841 line_state_pettern = right_slow;
yuron 8:3df97287c825 1842 }
yuron 8:3df97287c825 1843 else if(side == red) {
yuron 8:3df97287c825 1844 line_state_pettern = left_slow;
yuron 8:3df97287c825 1845 }
yuron 8:3df97287c825 1846 }
yuron 8:3df97287c825 1847 else if(back_timer1.read() > 1.5f) {
yuron 8:3df97287c825 1848 back_timer1.reset();
yuron 8:3df97287c825 1849 lateral_frag = 2;
yuron 8:3df97287c825 1850 }
yuron 5:167327a82430 1851 }
yuron 8:3df97287c825 1852 else if(lateral_frag == 2) {
yuron 8:3df97287c825 1853 if(limit.read() == 0) {
yuron 8:3df97287c825 1854 lateral_frag = 3;
yuron 6:5a051a378e42 1855 }
yuron 5:167327a82430 1856 }
yuron 8:3df97287c825 1857 //リミットがONの時(2回目)
yuron 8:3df97287c825 1858 else if(lateral_frag == 3) {
yuron 8:3df97287c825 1859 trace_direction = 0;
yuron 8:3df97287c825 1860 back_timer2.start();
yuron 8:3df97287c825 1861 if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
yuron 8:3df97287c825 1862 if(side == blue) {
yuron 8:3df97287c825 1863 line_state_pettern = right_slow;
yuron 8:3df97287c825 1864 }
yuron 8:3df97287c825 1865 else if(side == red) {
yuron 8:3df97287c825 1866 line_state_pettern = left_slow;
yuron 8:3df97287c825 1867 }
yuron 8:3df97287c825 1868 }
yuron 8:3df97287c825 1869 else if(back_timer2.read() > 0.8f) {
yuron 8:3df97287c825 1870 back_timer2.reset();
yuron 8:3df97287c825 1871 lateral_frag = 4;
yuron 8:3df97287c825 1872 }
yuron 5:167327a82430 1873 }
yuron 8:3df97287c825 1874 else if(lateral_frag == 4) {
yuron 8:3df97287c825 1875 if(limit.read() == 0) {
yuron 8:3df97287c825 1876 lateral_frag = 5;
yuron 6:5a051a378e42 1877 }
yuron 5:167327a82430 1878 }
yuron 8:3df97287c825 1879 //リミットがONの時(3回目)
yuron 8:3df97287c825 1880 else if(lateral_frag == 5) {
yuron 8:3df97287c825 1881 trace_direction = 0;
yuron 8:3df97287c825 1882 back_timer3.start();
yuron 8:3df97287c825 1883 if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
yuron 8:3df97287c825 1884 if(side == blue) {
yuron 8:3df97287c825 1885 line_state_pettern = right_slow;
yuron 8:3df97287c825 1886 }
yuron 8:3df97287c825 1887 else if(side == red) {
yuron 8:3df97287c825 1888 line_state_pettern = left_slow;
yuron 8:3df97287c825 1889 }
yuron 6:5a051a378e42 1890 }
yuron 8:3df97287c825 1891 else if(back_timer3.read() > 0.8f) {
yuron 8:3df97287c825 1892 back_timer3.reset();
yuron 8:3df97287c825 1893 lateral_frag = 6;
yuron 6:5a051a378e42 1894 }
yuron 5:167327a82430 1895 }
yuron 8:3df97287c825 1896 else if(lateral_frag == 6) {
yuron 8:3df97287c825 1897 if(limit.read() == 0) {
yuron 8:3df97287c825 1898 lateral_frag = 7;
yuron 6:5a051a378e42 1899 }
yuron 8:3df97287c825 1900 }
yuron 8:3df97287c825 1901 //リミットがONの時(4回目)
yuron 8:3df97287c825 1902 else if(lateral_frag == 7) {
yuron 8:3df97287c825 1903 //スタートゾーンに近づいたら減速
yuron 8:3df97287c825 1904 if(abs(measure_pulse) < 1200) {
yuron 8:3df97287c825 1905 if(side == blue) {
yuron 8:3df97287c825 1906 line_state_pettern = left_super_slow;
yuron 8:3df97287c825 1907 }
yuron 8:3df97287c825 1908 else if(side == red) {
yuron 8:3df97287c825 1909 line_state_pettern = right_super_slow;
yuron 8:3df97287c825 1910 }
yuron 8:3df97287c825 1911 }
yuron 8:3df97287c825 1912 else if(abs(measure_pulse) < 800) {
yuron 8:3df97287c825 1913 line_state_pettern = stop;
yuron 8:3df97287c825 1914 } else {
yuron 8:3df97287c825 1915 if(side == blue) {
yuron 8:3df97287c825 1916 line_state_pettern = left_slow;
yuron 8:3df97287c825 1917 }
yuron 8:3df97287c825 1918 else if(side == red) {
yuron 8:3df97287c825 1919 line_state_pettern = right_slow;
yuron 8:3df97287c825 1920 }
yuron 6:5a051a378e42 1921 }
yuron 5:167327a82430 1922 }
yuron 4:df334779a69e 1923 }
yuron 7:7f16fb8b0192 1924 */
yuron 8:3df97287c825 1925
yuron 5:167327a82430 1926 switch(line_state_pettern) {
yuron 5:167327a82430 1927 //青ゾーンでライン検知しないと低速右移動
yuron 5:167327a82430 1928 case right_slow:
yuron 8:3df97287c825 1929 //右前、右後ろ、左前、左後ろ
yuron 8:3df97287c825 1930 right_PID_slow(55, 55, 50, 50);
yuron 5:167327a82430 1931 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1932 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1933 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1934 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1935 wait_us(20);
yuron 5:167327a82430 1936 break;
yuron 8:3df97287c825 1937
yuron 8:3df97287c825 1938 //赤ゾーンでライン検知しないと低速左移動
yuron 5:167327a82430 1939 case left_slow:
yuron 8:3df97287c825 1940 left_PID_slow(55, 55, 50, 59);
yuron 8:3df97287c825 1941 //left_PID_slow(100, 110, 100, 110);
yuron 5:167327a82430 1942 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1943 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1944 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1945 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1946 wait_us(20);
yuron 5:167327a82430 1947 break;
yuron 5:167327a82430 1948
yuron 5:167327a82430 1949 //超低速右移動
yuron 5:167327a82430 1950 case right_super_slow:
yuron 8:3df97287c825 1951 right_PID_slow(5, 5, 5, 5);
yuron 5:167327a82430 1952 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1953 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1954 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1955 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1956 wait_us(20);
yuron 5:167327a82430 1957 break;
yuron 5:167327a82430 1958
yuron 5:167327a82430 1959 //超低速左移動
yuron 5:167327a82430 1960 case left_super_slow:
yuron 8:3df97287c825 1961 left_PID_slow(5, 5, 5, 5);
yuron 5:167327a82430 1962 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1963 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1964 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1965 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1966 wait_us(20);
yuron 5:167327a82430 1967 break;
yuron 5:167327a82430 1968
yuron 5:167327a82430 1969 //右旋回
yuron 5:167327a82430 1970 case turn_right:
yuron 8:3df97287c825 1971 turn_right_PID_slow(5, 5, 5, 5);
yuron 5:167327a82430 1972 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1973 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1974 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1975 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1976 wait_us(20);
yuron 5:167327a82430 1977 break;
yuron 5:167327a82430 1978
yuron 5:167327a82430 1979 //左旋回
yuron 5:167327a82430 1980 case turn_left:
yuron 8:3df97287c825 1981 turn_left_PID_slow(5, 5, 5, 5);
yuron 5:167327a82430 1982 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1983 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1984 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1985 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1986 wait_us(20);
yuron 5:167327a82430 1987 break;
yuron 5:167327a82430 1988
yuron 5:167327a82430 1989 //前進
yuron 5:167327a82430 1990 case front_trace:
yuron 5:167327a82430 1991 front_PID_slow(30, 30, 30, 30);
yuron 5:167327a82430 1992 //front_PID_slow(50, 50, 50, 50);
yuron 5:167327a82430 1993 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 1994 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 1995 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 1996 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 1997 wait_us(20);
yuron 5:167327a82430 1998 break;
yuron 8:3df97287c825 1999
yuron 5:167327a82430 2000 //後進
yuron 5:167327a82430 2001 case back_trace:
yuron 8:3df97287c825 2002 back_PID_slow(33, 31, 35, 35);
yuron 5:167327a82430 2003 //back_PID_slow(50, 50, 50, 50);
yuron 5:167327a82430 2004 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2005 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2006 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2007 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 2008 wait_us(20);
yuron 5:167327a82430 2009 break;
yuron 8:3df97287c825 2010
yuron 5:167327a82430 2011 //前前進後ろ右旋回
yuron 5:167327a82430 2012 case front_front_back_right:
yuron 8:3df97287c825 2013 front_front_back_right_PID(5, 5, 5, 5);
yuron 5:167327a82430 2014 //true_hidarimae_data[0] = 0x80;
yuron 5:167327a82430 2015 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2016 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2017 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2018 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 2019 wait_us(20);
yuron 5:167327a82430 2020 break;
yuron 5:167327a82430 2021
yuron 5:167327a82430 2022 //前前進後ろ左旋回
yuron 5:167327a82430 2023 case front_front_back_left:
yuron 8:3df97287c825 2024 front_front_back_left_PID(5, 5, 5, 5);
yuron 5:167327a82430 2025 //true_migimae_data[0] = 0x80;
yuron 5:167327a82430 2026 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2027 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2028 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2029 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 2030 wait_us(20);
yuron 5:167327a82430 2031 break;
yuron 5:167327a82430 2032
yuron 5:167327a82430 2033 //前右旋回後ろ前進
yuron 5:167327a82430 2034 case front_right_back_front:
yuron 8:3df97287c825 2035 front_right_back_front_PID(5, 5, 5, 5);
yuron 5:167327a82430 2036 //true_migiusiro_data[0] = 0x80;
yuron 5:167327a82430 2037 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2038 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2039 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2040 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 2041 wait_us(20);
yuron 5:167327a82430 2042 break;
yuron 5:167327a82430 2043
yuron 5:167327a82430 2044 //前左旋回後ろ前進
yuron 5:167327a82430 2045 case front_left_back_front:
yuron 8:3df97287c825 2046 front_left_back_front_PID(5, 5, 5, 5);
yuron 5:167327a82430 2047 //true_hidariusiro_data[0] = 0x80;
yuron 5:167327a82430 2048 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2049 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2050 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2051 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 2052 wait_us(20);
yuron 5:167327a82430 2053 break;
yuron 8:3df97287c825 2054
yuron 8:3df97287c825 2055 case right_fast:
yuron 8:3df97287c825 2056 //right_PID(300, 255, 300, 255);
yuron 8:3df97287c825 2057 right_PID_slow(100, 100, 100, 100);
yuron 8:3df97287c825 2058 i2c.write(0x10, true_migimae_data, 1, false);
yuron 8:3df97287c825 2059 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 8:3df97287c825 2060 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 8:3df97287c825 2061 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 8:3df97287c825 2062 wait_us(20);
yuron 8:3df97287c825 2063 break;
yuron 8:3df97287c825 2064
yuron 8:3df97287c825 2065 case left_fast:
yuron 8:3df97287c825 2066 //left_PID(255, 300, 255, 300);
yuron 8:3df97287c825 2067 left_PID_slow(100, 100, 100, 100);
yuron 8:3df97287c825 2068 i2c.write(0x10, true_migimae_data, 1, false);
yuron 8:3df97287c825 2069 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 8:3df97287c825 2070 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 8:3df97287c825 2071 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 8:3df97287c825 2072 wait_us(20);
yuron 8:3df97287c825 2073 break;
yuron 8:3df97287c825 2074 case stop:
yuron 8:3df97287c825 2075 true_migimae_data[0] = 0x80;
yuron 8:3df97287c825 2076 true_migiusiro_data[0] = 0x80;
yuron 8:3df97287c825 2077 true_hidarimae_data[0] = 0x80;
yuron 8:3df97287c825 2078 true_hidariusiro_data[0] = 0x80;
yuron 8:3df97287c825 2079 i2c.write(0x10, true_migimae_data, 1, false);
yuron 8:3df97287c825 2080 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 8:3df97287c825 2081 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 8:3df97287c825 2082 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 8:3df97287c825 2083 wait_us(20);
yuron 8:3df97287c825 2084 break;
yuron 8:3df97287c825 2085
yuron 5:167327a82430 2086 //それ以外ショートブレーキ
yuron 5:167327a82430 2087 default:
yuron 5:167327a82430 2088 true_migimae_data[0] = 0x80;
yuron 5:167327a82430 2089 true_migiusiro_data[0] = 0x80;
yuron 5:167327a82430 2090 true_hidarimae_data[0] = 0x80;
yuron 5:167327a82430 2091 true_hidariusiro_data[0] = 0x80;
yuron 5:167327a82430 2092 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2093 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2094 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2095 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 5:167327a82430 2096 wait_us(20);
yuron 5:167327a82430 2097 break;
yuron 7:7f16fb8b0192 2098 }
yuron 8:3df97287c825 2099
yuron 5:167327a82430 2100 /*
yuron 5:167327a82430 2101 //前進
yuron 5:167327a82430 2102 front_PID(300, 300, 300, 300);
yuron 4:df334779a69e 2103 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2104 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2105 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2106 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 2107 wait_us(20);
yuron 5:167327a82430 2108
yuron 5:167327a82430 2109 //後進
yuron 5:167327a82430 2110 back_PID(300, 300, 300, 300);
yuron 4:df334779a69e 2111 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2112 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2113 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2114 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2115 wait_us(20);
yuron 5:167327a82430 2116
yuron 5:167327a82430 2117 //右進行
yuron 5:167327a82430 2118 //right_PID(255, 300, 255, 300);
yuron 5:167327a82430 2119 right_PID_slow(93, 100, 93, 100);
yuron 5:167327a82430 2120 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2121 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2122 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2123 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 5:167327a82430 2124 wait_us(20);
yuron 8:3df97287c825 2125
yuron 5:167327a82430 2126 //左進行
yuron 7:7f16fb8b0192 2127 left_PID(300, 300, 300, 300);
yuron 5:167327a82430 2128 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2129 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2130 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2131 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 5:167327a82430 2132 wait_us(20);
yuron 8:3df97287c825 2133
yuron 5:167327a82430 2134 //斜め右前
yuron 5:167327a82430 2135 right_front_PID(300, 300);
yuron 4:df334779a69e 2136 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2137 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2138 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2139 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2140 wait_us(20);
yuron 5:167327a82430 2141
yuron 5:167327a82430 2142 //斜め右後
yuron 5:167327a82430 2143 right_back_PID(300, 300);
yuron 5:167327a82430 2144 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2145 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2146 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2147 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 5:167327a82430 2148 wait_us(20);
yuron 5:167327a82430 2149
yuron 5:167327a82430 2150 //斜め左前
yuron 5:167327a82430 2151 left_front_PID(300, 300);
yuron 4:df334779a69e 2152 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2153 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2154 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2155 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2156 wait_us(20);
yuron 5:167327a82430 2157
yuron 5:167327a82430 2158 //斜め左後
yuron 5:167327a82430 2159 left_back_PID(300, 300);
yuron 4:df334779a69e 2160 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2161 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2162 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2163 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2164 wait_us(20);
yuron 5:167327a82430 2165
yuron 5:167327a82430 2166 //右旋回
yuron 5:167327a82430 2167 turn_right_PID(300, 300, 300, 300);
yuron 5:167327a82430 2168 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2169 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2170 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2171 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 5:167327a82430 2172 wait_us(20);
yuron 5:167327a82430 2173
yuron 5:167327a82430 2174 //左旋回
yuron 5:167327a82430 2175 turn_left_PID(300, 300, 300, 300);
yuron 5:167327a82430 2176 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2177 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2178 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2179 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 5:167327a82430 2180 wait_us(20);
yuron 5:167327a82430 2181
yuron 5:167327a82430 2182 //前前進後ろ右旋回
yuron 5:167327a82430 2183 front_front_back_right_PID(30, 30, 30, 30);
yuron 4:df334779a69e 2184 true_hidarimae_data[0] = 0x80;
yuron 4:df334779a69e 2185 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2186 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2187 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2188 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2189 wait_us(20);
yuron 4:df334779a69e 2190
yuron 5:167327a82430 2191 //前前進後ろ左旋回
yuron 5:167327a82430 2192 front_front_back_left_PID(30, 30, 30, 30);
yuron 4:df334779a69e 2193 true_migimae_data[0] = 0x80;
yuron 4:df334779a69e 2194 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2195 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2196 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2197 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2198 wait_us(20);
yuron 4:df334779a69e 2199
yuron 5:167327a82430 2200 //前右旋回後ろ前進
yuron 5:167327a82430 2201 front_right_back_front_PID(30, 30, 30, 30);
yuron 4:df334779a69e 2202 true_migiusiro_data[0] = 0x80;
yuron 5:167327a82430 2203 i2c.write(0x10, true_migimae_data, 1, false);
yuron 5:167327a82430 2204 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 5:167327a82430 2205 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 5:167327a82430 2206 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 5:167327a82430 2207 wait_us(20);
yuron 5:167327a82430 2208
yuron 5:167327a82430 2209 //前左旋回後ろ前進
yuron 5:167327a82430 2210 front_left_back_front_PID(30, 30, 30, 30);
yuron 4:df334779a69e 2211 true_hidariusiro_data[0] = 0x80;
yuron 4:df334779a69e 2212 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 2213 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 2214 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 2215 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 2216 wait_us(20);
yuron 6:5a051a378e42 2217 */
yuron 8:3df97287c825 2218
yuron 7:7f16fb8b0192 2219 /*
yuron 5:167327a82430 2220 //ローラーぐるぐる(max930rpm)
yuron 6:5a051a378e42 2221 pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
yuron 7:7f16fb8b0192 2222 //このパラメータ(距離10cm, 移動1個め)で5~8割の確率で勃つ
yuron 7:7f16fb8b0192 2223 //roller_PID(814, 696);
yuron 7:7f16fb8b0192 2224 roller_PID(1006, 928);
yuron 4:df334779a69e 2225 i2c.write(0x20, front_roller_data, 1, false);
yuron 4:df334779a69e 2226 i2c.write(0x22, back_roller_data, 1, false);
yuron 5:167327a82430 2227 wait_us(20);
yuron 7:7f16fb8b0192 2228 */
yuron 8:3df97287c825 2229
yuron 0:f73c1b076ae4 2230 /*
yuron 2:c32991ba628f 2231 //どんどん加速(逆転)
yuron 5:167327a82430 2232 for(send_data[0] = 0x84; send_data[0] < 0xFF; send_data[0]++){
yuron 5:167327a82430 2233 i2c.write(0x10, send_data, 1);
yuron 5:167327a82430 2234 i2c.write(0x12, send_data, 1);
yuron 5:167327a82430 2235 i2c.write(0x14, send_data, 1);
yuron 5:167327a82430 2236 i2c.write(0x16, send_data, 1);
yuron 0:f73c1b076ae4 2237 wait(0.1);
yuron 0:f73c1b076ae4 2238 }
yuron 2:c32991ba628f 2239 //だんだん減速(逆転)
yuron 5:167327a82430 2240 for(send_data[0] = 0x0C; send_data[0] > 0x7C; send_data[0]--){
yuron 0:f73c1b076ae4 2241 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 2242 //ice(0xA2, send_data);
yuron 5:167327a82430 2243
yuron 5:167327a82430 2244 i2c.write(0x10, send_data, 1);
yuron 5:167327a82430 2245 i2c.write(0x12, send_data, 1);
yuron 5:167327a82430 2246 i2c.write(0x14, send_data, 1);
yuron 5:167327a82430 2247 i2c.write(0x16, send_data, 1);
yuron 0:f73c1b076ae4 2248 wait(0.1);
yuron 0:f73c1b076ae4 2249 }
yuron 0:f73c1b076ae4 2250 */
yuron 0:f73c1b076ae4 2251 }
yuron 0:f73c1b076ae4 2252 }