Bteam tactics plan A _ move table ver

Dependencies:   HCSR04 PID QEI mbed

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

Committer:
yuron
Date:
Wed Oct 10 11:27:09 2018 +0000
Revision:
11:5a0d6f69e751
Parent:
10:b672aa81b226
Child:
12:1a22b9797004
??????2

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