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