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@10:b672aa81b226, 2018-10-10 (annotated)

Committer:
yuron
Date:
Wed Oct 10 08:20:24 2018 +0000
Revision:
10:b672aa81b226
Parent:
9:1359f0c813b1
Child:
11:5a0d6f69e751
?????????

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