NITICRobotClub 2018Team-B / Mbed 2 deprecated UNKO_FINAL_PlanA

Bteam tactics plan A _ move table ver

Dependencies:   HCSR04 PID QEI mbed

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

main.cpp@14:677e9f0785b8, 2018-10-11 (annotated)

Committer:
BIGBOSS04
Date:
Thu Oct 11 15:55:47 2018 +0000
Revision:
14:677e9f0785b8
Parent:
13:93321c73df60 
Bteam tactics plan A _ center table ver;

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