NITICRobotClub 2018Team-B / Mbed 2 deprecated UNKO_FINAL_PlanA

Bteam tactics plan A _ move table ver

Dependencies:   HCSR04 PID QEI mbed

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

main.cpp@7:7f16fb8b0192, 2018-10-03 (annotated)

Committer:
yuron
Date:
Wed Oct 03 12:03:03 2018 +0000
Revision:
7:7f16fb8b0192
Parent:
6:5a051a378e42
Child:
8:3df97287c825
?????????????(?????????????????????????)

Who changed what in which revision?

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