INCTRC Information Sharing Test / Mbed 2 deprecated Lets_move_Seki2018_ver2

2018/09/05現在のBチーム自動機の制御プログラム(バックアップも兼ねて)

Dependencies:   HCSR04 PID QEI mbed

Fork of Lets_move_Seki2018 by INCTRC Information Sharing Test

main.cpp@4:df334779a69e, 2018-09-05 (annotated)

Committer:
yuron
Date:
Wed Sep 05 15:07:15 2018 +0000
Revision:
4:df334779a69e
Parent:
3:1223cab367d9 
2018/09/05???B??????????????(??????????)

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 0:f73c1b076ae4 13 #include "mbed.h"
yuron 0:f73c1b076ae4 14 #include "math.h"
yuron 0:f73c1b076ae4 15 #include "QEI.h"
yuron 0:f73c1b076ae4 16 #include "PID.h"
yuron 4:df334779a69e 17 #include "hcsr04.h"
yuron 0:f73c1b076ae4 18 #define PI 3.14159265359
yuron 4:df334779a69e 19 #define Kp 20.0
yuron 4:df334779a69e 20 #define Ki 0.02
yuron 4:df334779a69e 21 #define Kd 0.0
yuron 4:df334779a69e 22 //#define roller_Kp 0.5
yuron 4:df334779a69e 23 //#define roller_Ki 0.3
yuron 4:df334779a69e 24 #define roller_Kp 2.5
yuron 4:df334779a69e 25 #define roller_Ki 0.01
yuron 4:df334779a69e 26
yuron 4:df334779a69e 27 //右前オムニ
yuron 4:df334779a69e 28 PID migimae(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 29 //右後オムニ
yuron 4:df334779a69e 30 PID migiusiro(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 31 //左前オムニ
yuron 4:df334779a69e 32 PID hidarimae(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 33 //左後オムニ
yuron 4:df334779a69e 34 PID hidariusiro(Kp, Ki, Kd, 0.001);
yuron 4:df334779a69e 35 //前ローラー
yuron 4:df334779a69e 36 PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
yuron 4:df334779a69e 37 //後ローラー
yuron 4:df334779a69e 38 PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);
yuron 0:f73c1b076ae4 39
yuron 4:df334779a69e 40 //MDとの通信ポート
yuron 4:df334779a69e 41 I2C i2c(PB_9, PB_8); //SDA, SCL
yuron 4:df334779a69e 42 //PCとの通信ポート
yuron 4:df334779a69e 43 Serial pc(USBTX, USBRX); //TX, RX
yuron 4:df334779a69e 44 //フォトインタラプタ制御基板からの受信ポート
yuron 4:df334779a69e 45 Serial photo(NC, PC_11); //TX, RX
yuron 4:df334779a69e 46 //TWE-Liteからの受信ポート
yuron 4:df334779a69e 47 Serial twe(PC_12, PD_2); //TX, RX
yuron 4:df334779a69e 48
yuron 4:df334779a69e 49 //超音波センサ1
yuron 4:df334779a69e 50 HCSR04 sonic(PB_3, PB_10); //Trig, Echo
yuron 4:df334779a69e 51 //超音波センサ2
yuron 4:df334779a69e 52 //HCSR04 sonic2(PC_13, PA_15); //Trig, Echo
yuron 4:df334779a69e 53 //超音波センサ3
yuron 4:df334779a69e 54 //HCSR04 sonic3(PC_15, PC_14); //Trig, Echo
yuron 4:df334779a69e 55 //超音波センサ4
yuron 4:df334779a69e 56 //HCSR04 sonic4(PH_1 , PH_0 ); //Trig, Echo
yuron 4:df334779a69e 57
yuron 4:df334779a69e 58 //赤、青ゾーン切り替え用スイッチ
yuron 4:df334779a69e 59 DigitalIn side(PC_1);
yuron 4:df334779a69e 60 //スタートボタン
yuron 4:df334779a69e 61 DigitalIn start_button(PC_4);
yuron 4:df334779a69e 62 //スイッチ1
yuron 4:df334779a69e 63 DigitalIn user_switch1(PB_0);
yuron 4:df334779a69e 64 //スイッチ2
yuron 4:df334779a69e 65 DigitalIn user_switch2(PA_4);
yuron 4:df334779a69e 66 //スイッチ3
yuron 4:df334779a69e 67 DigitalIn user_switch3(PA_3);
yuron 4:df334779a69e 68 //スイッチ4
yuron 4:df334779a69e 69 //以下の文を入れるとロリコンが読めなくなる
yuron 4:df334779a69e 70 //DigitalIn user_switch4(PA_2);
yuron 4:df334779a69e 71 //スイッチ5
yuron 4:df334779a69e 72 DigitalIn user_switch5(PA_10);
yuron 4:df334779a69e 73
yuron 4:df334779a69e 74 //フォトインタラプタ
yuron 4:df334779a69e 75 DigitalIn photo_interrupter(PA_15);
yuron 4:df334779a69e 76
yuron 4:df334779a69e 77 //12V停止信号ピン
yuron 0:f73c1b076ae4 78 DigitalOut emergency(PA_13);
yuron 4:df334779a69e 79
yuron 4:df334779a69e 80 //赤ゾーンLED
yuron 4:df334779a69e 81 DigitalOut blue_side(PC_0);
yuron 4:df334779a69e 82 //青ゾーンLED
yuron 4:df334779a69e 83 DigitalOut red_side(PC_3);
yuron 4:df334779a69e 84 //スタートLED
yuron 4:df334779a69e 85 DigitalOut start_LED(PA_8);
yuron 4:df334779a69e 86 //LED1
yuron 4:df334779a69e 87 DigitalOut myled1(PC_6);
yuron 4:df334779a69e 88 //LED2
yuron 4:df334779a69e 89 DigitalOut myled2(PC_5);
yuron 4:df334779a69e 90 //LED3
yuron 4:df334779a69e 91 DigitalOut myled3(PA_12);
yuron 4:df334779a69e 92 //LED4
yuron 4:df334779a69e 93 DigitalOut myled4(PB_1);
yuron 4:df334779a69e 94 //LED5
yuron 4:df334779a69e 95 DigitalOut myled5(PA_5);
yuron 0:f73c1b076ae4 96
yuron 4:df334779a69e 97 //前ローラー
yuron 4:df334779a69e 98 QEI front_roller_wheel(PA_0, PA_1, NC, 624);
yuron 4:df334779a69e 99 //後ローラー
yuron 4:df334779a69e 100 QEI back_roller_wheel(PB_5, PB_4, NC, 624);
yuron 4:df334779a69e 101 //計測オムニ1
yuron 4:df334779a69e 102 //QEI measure1_wheel(PC_9, PC_8, NC, 624);
yuron 4:df334779a69e 103 //計測オムニ2(使用不可)
yuron 4:df334779a69e 104 //QEI measure2_wheel(PB_3, PB_10, NC, 624);
yuron 4:df334779a69e 105 //右前オムニ
yuron 4:df334779a69e 106 QEI migimae_wheel(PB_6 , PA_7 , NC, 624);
yuron 4:df334779a69e 107 //右後オムニ
yuron 4:df334779a69e 108 QEI migiusiro_wheel(PA_11, PB_12, NC, 624);
yuron 4:df334779a69e 109 //左前オムニ
yuron 4:df334779a69e 110 QEI hidarimae_wheel(PB_2, PB_15, NC, 624);
yuron 4:df334779a69e 111 //左後オムニ
yuron 4:df334779a69e 112 QEI hidariusiro_wheel(PB_14, PB_13, NC, 624);
yuron 0:f73c1b076ae4 113
yuron 4:df334779a69e 114 Ticker get_rps;
yuron 4:df334779a69e 115 Ticker get_distance;
yuron 1:62b321f6c9c3 116 Timer timer;
yuron 0:f73c1b076ae4 117
yuron 4:df334779a69e 118 int roller_flag = 0;
yuron 4:df334779a69e 119 int loading_state = 0;
yuron 0:f73c1b076ae4 120
yuron 4:df334779a69e 121 int migimae_rpm;
yuron 4:df334779a69e 122 int migiusiro_rpm;
yuron 4:df334779a69e 123 int hidarimae_rpm;
yuron 4:df334779a69e 124 int hidariusiro_rpm;
yuron 4:df334779a69e 125 int measure1_rpm;
yuron 4:df334779a69e 126 //int measure2_rpm;
yuron 4:df334779a69e 127 int front_roller_rpm;
yuron 4:df334779a69e 128 int back_roller_rpm;
yuron 0:f73c1b076ae4 129
yuron 4:df334779a69e 130 int migimae_pulse;
yuron 4:df334779a69e 131 int migiusiro_pulse;
yuron 4:df334779a69e 132 int hidarimae_pulse;
yuron 4:df334779a69e 133 int hidariusiro_pulse;
yuron 4:df334779a69e 134 int measure1_pulse;
yuron 4:df334779a69e 135 //int measure2_pulse;
yuron 4:df334779a69e 136 int front_roller_pulse;
yuron 4:df334779a69e 137 int back_roller_pulse;
yuron 0:f73c1b076ae4 138
yuron 4:df334779a69e 139 int ave_migimae_pulse[10];
yuron 4:df334779a69e 140 int ave_migiusiro_pulse[10];
yuron 4:df334779a69e 141 int ave_hidarimae_pulse[10];
yuron 4:df334779a69e 142 int ave_hidariusiro_pulse[10];
yuron 4:df334779a69e 143 int ave_measure1_pulse[10];
yuron 4:df334779a69e 144 //int ave_measure2_pulse[10];
yuron 4:df334779a69e 145 int ave_front_roller_pulse[10];
yuron 4:df334779a69e 146 int ave_back_roller_pulse[10];
yuron 0:f73c1b076ae4 147
yuron 4:df334779a69e 148
yuron 4:df334779a69e 149 int migimae_counter;
yuron 4:df334779a69e 150 int migiusiro_counter;
yuron 4:df334779a69e 151 int hidarimae_counter;
yuron 4:df334779a69e 152 int hidariusiro_counter;
yuron 4:df334779a69e 153 int measure1_counter;
yuron 4:df334779a69e 154 //int measure2_counter;
yuron 4:df334779a69e 155 int front_roller_counter;
yuron 4:df334779a69e 156 int back_roller_counter;
yuron 0:f73c1b076ae4 157
yuron 0:f73c1b076ae4 158 char send_data[1];
yuron 0:f73c1b076ae4 159 char true_send_data[1];
yuron 0:f73c1b076ae4 160
yuron 4:df334779a69e 161 char migimae_data[1];
yuron 4:df334779a69e 162 char migiusiro_data[1];
yuron 4:df334779a69e 163 char hidarimae_data[1];
yuron 4:df334779a69e 164 char hidariusiro_data[1];
yuron 4:df334779a69e 165 char front_roller_data[1];
yuron 4:df334779a69e 166 char back_roller_data[1];
yuron 4:df334779a69e 167 char loading_data[1];
yuron 4:df334779a69e 168 char cylinder_data[1];
yuron 4:df334779a69e 169
yuron 4:df334779a69e 170 char true_migimae_data[1];
yuron 4:df334779a69e 171 char true_migiusiro_data[1];
yuron 4:df334779a69e 172 char true_hidarimae_data[1];
yuron 4:df334779a69e 173 char true_hidariusiro_data[1];
yuron 4:df334779a69e 174
yuron 4:df334779a69e 175 int line_state = 0;
yuron 4:df334779a69e 176
yuron 4:df334779a69e 177 unsigned int distance;
yuron 0:f73c1b076ae4 178
yuron 0:f73c1b076ae4 179 /* ロリコン処理関数 */
yuron 0:f73c1b076ae4 180 void flip();
yuron 4:df334779a69e 181 int front_PID(int RF, int RB, int LF, int LB);
yuron 4:df334779a69e 182 int back_PID(int RF, int RB, int LF, int LB);
yuron 4:df334779a69e 183 int rihgt_PID(int RF, int RB, int LF, int LB);
yuron 4:df334779a69e 184 int left_PID(int RF, int RB, int LF, int LB);
yuron 4:df334779a69e 185 int right_front_PID(int RB, int LF);
yuron 4:df334779a69e 186 int right_back_PID(int RF, int LB);
yuron 4:df334779a69e 187 int left_front_PID(int RF, int LB);
yuron 4:df334779a69e 188 int left_back_PID(int RB, int RF);
yuron 4:df334779a69e 189 int turn_right_PID(int RF, int RB, int LF, int LB);
yuron 4:df334779a69e 190 int turn_left_PID(int RF, int RB, int LF, int LB);
yuron 4:df334779a69e 191 int roller_PID(int front, int back);
yuron 4:df334779a69e 192 void linetrace();
yuron 4:df334779a69e 193 void ultrasonic();
yuron 0:f73c1b076ae4 194
yuron 4:df334779a69e 195 void flip() {
yuron 4:df334779a69e 196 migimae_pulse = migimae_wheel.getPulses();
yuron 4:df334779a69e 197 migiusiro_pulse = migiusiro_wheel.getPulses();
yuron 4:df334779a69e 198 hidarimae_pulse = hidarimae_wheel.getPulses();
yuron 4:df334779a69e 199 hidariusiro_pulse = hidariusiro_wheel.getPulses();
yuron 4:df334779a69e 200 //measure1_pulse = measure1_wheel.getPulses();
yuron 4:df334779a69e 201 //measure2_pulse = measure2_wheel.getPulses();
yuron 4:df334779a69e 202 front_roller_pulse = front_roller_wheel.getPulses();
yuron 4:df334779a69e 203 back_roller_pulse = back_roller_wheel.getPulses();
yuron 4:df334779a69e 204
yuron 4:df334779a69e 205 //((40ms*25 = 1s)* 60 = 1min) / 分解能
yuron 4:df334779a69e 206 migimae_rpm = migimae_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 207 migiusiro_rpm = migiusiro_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 208 hidarimae_rpm = hidarimae_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 209 hidariusiro_rpm = hidariusiro_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 210 //measure1_rpm = measure1_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 211 //measure2_rpm = measure2_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 212 front_roller_rpm = front_roller_pulse * 25 * 60 / 1200;
yuron 4:df334779a69e 213 back_roller_rpm = back_roller_pulse * 25 * 60 / 1200;
yuron 0:f73c1b076ae4 214
yuron 4:df334779a69e 215 //pc.printf("RF: %d RB: %d LF: %d LB: %d\n", migimae_rpm, migiusiro_rpm, hidarimae_rpm, hidariusiro_rpm);
yuron 4:df334779a69e 216 //pc.printf("前: %d, 後: %d, %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance);
yuron 4:df334779a69e 217 //pc.printf("%d\n", abs(back_roller_rpm));
yuron 4:df334779a69e 218 //pc.printf("RF: %d, RB: %d\n", migimae_rpm, migiusiro_rpm);
yuron 4:df334779a69e 219
yuron 4:df334779a69e 220 migimae_wheel.reset();
yuron 4:df334779a69e 221 migiusiro_wheel.reset();
yuron 4:df334779a69e 222 hidarimae_wheel.reset();
yuron 4:df334779a69e 223 hidariusiro_wheel.reset();
yuron 4:df334779a69e 224 //measure1_wheel.reset();
yuron 4:df334779a69e 225 //measure2_wheel.reset();
yuron 4:df334779a69e 226 front_roller_wheel.reset();
yuron 4:df334779a69e 227 back_roller_wheel.reset();
yuron 4:df334779a69e 228 }
yuron 4:df334779a69e 229 int front_PID(int RF, int RB, int LF, int LB) {
yuron 4:df334779a69e 230 // センサ出力値の最小、最大
yuron 4:df334779a69e 231 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 232 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 233 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 234 hidariusiro.setInputLimits(-400, 400);
yuron 0:f73c1b076ae4 235
yuron 4:df334779a69e 236 // 制御量の最小、最大
yuron 4:df334779a69e 237 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 238 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 239 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 240 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 241
yuron 4:df334779a69e 242 // よくわからんやつ
yuron 4:df334779a69e 243 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 244 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 245 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 246 hidariusiro.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 247
yuron 4:df334779a69e 248 // 目標値
yuron 4:df334779a69e 249 migimae.setSetPoint(RF);
yuron 4:df334779a69e 250 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 251 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 252 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 253
yuron 4:df334779a69e 254 // センサ出力
yuron 4:df334779a69e 255 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 256 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 257 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 258 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 259
yuron 4:df334779a69e 260 // 制御量(計算結果)
yuron 4:df334779a69e 261 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 262 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 263 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 264 hidariusiro_data[0] = hidariusiro.compute();
yuron 0:f73c1b076ae4 265
yuron 4:df334779a69e 266 // 制御量をPWM値に変換
yuron 4:df334779a69e 267 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 268 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 269 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 270 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 4:df334779a69e 271
yuron 4:df334779a69e 272 return 0;
yuron 0:f73c1b076ae4 273 }
yuron 4:df334779a69e 274 int back_PID(int RF, int RB, int LF, int LB) {
yuron 0:f73c1b076ae4 275 // センサ出力値の最小、最大
yuron 4:df334779a69e 276 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 277 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 278 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 279 hidariusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 280
yuron 4:df334779a69e 281 // 制御量の最小、最大
yuron 4:df334779a69e 282 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 283 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 284 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 285 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 286
yuron 4:df334779a69e 287 // よくわからんやつ
yuron 4:df334779a69e 288 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 289 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 290 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 291 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 292
yuron 4:df334779a69e 293 // 目標値
yuron 4:df334779a69e 294 migimae.setSetPoint(RF);
yuron 4:df334779a69e 295 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 296 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 297 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 298
yuron 4:df334779a69e 299 // センサ出力
yuron 4:df334779a69e 300 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 4:df334779a69e 301 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 302 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 303 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 304 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 305
yuron 4:df334779a69e 306 // 制御量(計算結果)
yuron 4:df334779a69e 307 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 308 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 309 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 310 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 311
yuron 4:df334779a69e 312 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 313 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 314 true_hidarimae_data[0] = hidarimae_data[0];
yuron 4:df334779a69e 315 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 4:df334779a69e 316
yuron 4:df334779a69e 317 return 0;
yuron 4:df334779a69e 318 }
yuron 4:df334779a69e 319 int right_PID(int RF, int RB, int LF, int LB) {
yuron 4:df334779a69e 320 // センサ出力値の最小、最大
yuron 4:df334779a69e 321 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 322 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 323 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 324 hidariusiro.setInputLimits(-400, 400);
yuron 0:f73c1b076ae4 325
yuron 0:f73c1b076ae4 326 // 制御量の最小、最大
yuron 4:df334779a69e 327 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 328 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 329 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 330 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 331
yuron 4:df334779a69e 332 // よくわからんやつ
yuron 4:df334779a69e 333 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 334 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 335 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 336 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 337
yuron 4:df334779a69e 338 // 目標値
yuron 4:df334779a69e 339 migimae.setSetPoint(RF);
yuron 4:df334779a69e 340 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 341 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 342 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 343
yuron 4:df334779a69e 344 // センサ出力
yuron 4:df334779a69e 345 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 346 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 347 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 348 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 349
yuron 4:df334779a69e 350 // 制御量(計算結果)
yuron 4:df334779a69e 351 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 352 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 353 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 354 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 355
yuron 4:df334779a69e 356 // 制御量をPWM値に変換
yuron 4:df334779a69e 357 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 358 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 359 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 360 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 4:df334779a69e 361
yuron 4:df334779a69e 362 return 0;
yuron 4:df334779a69e 363 }
yuron 4:df334779a69e 364 int left_PID(int RF, int RB, int LF, int LB) {
yuron 4:df334779a69e 365 // センサ出力値の最小、最大
yuron 4:df334779a69e 366 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 367 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 368 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 369 hidariusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 370
yuron 4:df334779a69e 371 // 制御量の最小、最大
yuron 4:df334779a69e 372 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 373 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 374 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 375 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 0:f73c1b076ae4 376
yuron 0:f73c1b076ae4 377 // よくわからんやつ
yuron 4:df334779a69e 378 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 379 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 380 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 381 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 382
yuron 4:df334779a69e 383 // 目標値
yuron 4:df334779a69e 384 migimae.setSetPoint(RF);
yuron 4:df334779a69e 385 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 386 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 387 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 388
yuron 4:df334779a69e 389 // センサ出力
yuron 4:df334779a69e 390 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 391 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 392 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 393 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 394
yuron 4:df334779a69e 395 // 制御量(計算結果)
yuron 4:df334779a69e 396 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 397 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 398 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 399 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 400
yuron 4:df334779a69e 401 // 制御量をPWM値に変換
yuron 4:df334779a69e 402 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 403 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 404 true_hidarimae_data[0] = hidarimae_data[0];
yuron 4:df334779a69e 405 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 4:df334779a69e 406
yuron 4:df334779a69e 407 return 0;
yuron 4:df334779a69e 408 }
yuron 4:df334779a69e 409 //斜め右前
yuron 4:df334779a69e 410 int right_front_PID(int RB, int LF) {
yuron 4:df334779a69e 411 // センサ出力値の最小、最大
yuron 4:df334779a69e 412 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 413 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 414
yuron 4:df334779a69e 415 // 制御量の最小、最大
yuron 4:df334779a69e 416 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 417 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 418
yuron 4:df334779a69e 419 // よくわからんやつ
yuron 4:df334779a69e 420 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 421 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 422
yuron 4:df334779a69e 423 // 目標値
yuron 4:df334779a69e 424 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 425 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 426
yuron 4:df334779a69e 427 // センサ出力
yuron 4:df334779a69e 428 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 429 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 430
yuron 4:df334779a69e 431 // 制御量(計算結果)
yuron 4:df334779a69e 432 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 433 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 434
yuron 4:df334779a69e 435 // 制御量をPWM値に変換
yuron 4:df334779a69e 436 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 437 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 438
yuron 4:df334779a69e 439 return 0;
yuron 4:df334779a69e 440 }
yuron 4:df334779a69e 441 //斜め右後
yuron 4:df334779a69e 442 int right_back_PID(int RF, int LB) {
yuron 4:df334779a69e 443 // センサ出力値の最小、最大
yuron 4:df334779a69e 444 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 445 hidariusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 446
yuron 4:df334779a69e 447 // 制御量の最小、最大
yuron 4:df334779a69e 448 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 449 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 450
yuron 4:df334779a69e 451 // よくわからんやつ
yuron 4:df334779a69e 452 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 453 hidariusiro.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 454
yuron 0:f73c1b076ae4 455 // 目標値
yuron 4:df334779a69e 456 migimae.setSetPoint(RF);
yuron 4:df334779a69e 457 hidariusiro.setSetPoint(LB);
yuron 0:f73c1b076ae4 458
yuron 0:f73c1b076ae4 459 // センサ出力
yuron 4:df334779a69e 460 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 4:df334779a69e 461 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 462 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 0:f73c1b076ae4 463
yuron 0:f73c1b076ae4 464 // 制御量(計算結果)
yuron 4:df334779a69e 465 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 466 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 467
yuron 4:df334779a69e 468 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 469 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 0:f73c1b076ae4 470
yuron 4:df334779a69e 471 return 0;
yuron 4:df334779a69e 472 }
yuron 4:df334779a69e 473 //斜め左前
yuron 4:df334779a69e 474 int left_front_PID(int RF, int LB) {
yuron 4:df334779a69e 475 // センサ出力値の最小、最大
yuron 4:df334779a69e 476 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 477 hidariusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 478
yuron 4:df334779a69e 479 // 制御量の最小、最大
yuron 4:df334779a69e 480 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 481 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 482
yuron 4:df334779a69e 483 // よくわからんやつ
yuron 4:df334779a69e 484 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 485 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 486
yuron 4:df334779a69e 487 // 目標値
yuron 4:df334779a69e 488 migimae.setSetPoint(RF);
yuron 4:df334779a69e 489 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 490
yuron 4:df334779a69e 491 // センサ出力
yuron 4:df334779a69e 492 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 493 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 494
yuron 4:df334779a69e 495 // 制御量(計算結果)
yuron 4:df334779a69e 496 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 497 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 498
yuron 0:f73c1b076ae4 499 // 制御量をPWM値に変換
yuron 4:df334779a69e 500 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 501 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 2:c32991ba628f 502
yuron 2:c32991ba628f 503 return 0;
yuron 0:f73c1b076ae4 504 }
yuron 4:df334779a69e 505 //斜め左後
yuron 4:df334779a69e 506 int left_back_PID(int RB, int LF) {
yuron 1:62b321f6c9c3 507 // センサ出力値の最小、最大
yuron 4:df334779a69e 508 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 509 hidarimae.setInputLimits(-400, 400);
yuron 1:62b321f6c9c3 510
yuron 1:62b321f6c9c3 511 // 制御量の最小、最大
yuron 4:df334779a69e 512 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 513 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 514
yuron 1:62b321f6c9c3 515 // よくわからんやつ
yuron 4:df334779a69e 516 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 517 hidarimae.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 518
yuron 1:62b321f6c9c3 519 // 目標値
yuron 4:df334779a69e 520 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 521 hidarimae.setSetPoint(LF);
yuron 1:62b321f6c9c3 522
yuron 1:62b321f6c9c3 523 // センサ出力
yuron 2:c32991ba628f 524 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 4:df334779a69e 525 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 526 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 1:62b321f6c9c3 527
yuron 1:62b321f6c9c3 528 // 制御量(計算結果)
yuron 4:df334779a69e 529 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 530 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 531
yuron 4:df334779a69e 532 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 533 true_hidarimae_data[0] = hidarimae_data[0];
yuron 1:62b321f6c9c3 534
yuron 2:c32991ba628f 535 return 0;
yuron 1:62b321f6c9c3 536 }
yuron 4:df334779a69e 537 int turn_right_PID(int RF, int RB, int LF, int LB) {
yuron 4:df334779a69e 538 // センサ出力値の最小、最大
yuron 4:df334779a69e 539 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 540 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 541 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 542 hidariusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 543
yuron 4:df334779a69e 544 // 制御量の最小、最大
yuron 4:df334779a69e 545 migimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 546 migiusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 547 hidarimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 548 hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 549
yuron 4:df334779a69e 550 // よくわからんやつ
yuron 4:df334779a69e 551 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 552 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 553 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 554 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 555
yuron 4:df334779a69e 556 // 目標値
yuron 4:df334779a69e 557 migimae.setSetPoint(RF);
yuron 4:df334779a69e 558 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 559 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 560 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 561
yuron 4:df334779a69e 562 // センサ出力
yuron 4:df334779a69e 563 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 564 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 565 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 566 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 567
yuron 4:df334779a69e 568 // 制御量(計算結果)
yuron 4:df334779a69e 569 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 570 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 571 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 572 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 573
yuron 4:df334779a69e 574 // 制御量をPWM値に変換
yuron 4:df334779a69e 575 true_migimae_data[0] = migimae_data[0];
yuron 4:df334779a69e 576 true_migiusiro_data[0] = migiusiro_data[0];
yuron 4:df334779a69e 577 true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron 4:df334779a69e 578 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron 4:df334779a69e 579
yuron 4:df334779a69e 580 return 0;
yuron 4:df334779a69e 581 }
yuron 4:df334779a69e 582 int turn_left_PID(int RF, int RB, int LF, int LB) {
yuron 4:df334779a69e 583 // センサ出力値の最小、最大
yuron 4:df334779a69e 584 migimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 585 migiusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 586 hidarimae.setInputLimits(-400, 400);
yuron 4:df334779a69e 587 hidariusiro.setInputLimits(-400, 400);
yuron 4:df334779a69e 588
yuron 4:df334779a69e 589 // 制御量の最小、最大
yuron 4:df334779a69e 590 migimae.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 591 migiusiro.setOutputLimits(0x0C, 0x7C);
yuron 4:df334779a69e 592 hidarimae.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 593 hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 594
yuron 4:df334779a69e 595 // よくわからんやつ
yuron 4:df334779a69e 596 migimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 597 migiusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 598 hidarimae.setMode(AUTO_MODE);
yuron 4:df334779a69e 599 hidariusiro.setMode(AUTO_MODE);
yuron 4:df334779a69e 600
yuron 4:df334779a69e 601 // 目標値
yuron 4:df334779a69e 602 migimae.setSetPoint(RF);
yuron 4:df334779a69e 603 migiusiro.setSetPoint(RB);
yuron 4:df334779a69e 604 hidarimae.setSetPoint(LF);
yuron 4:df334779a69e 605 hidariusiro.setSetPoint(LB);
yuron 4:df334779a69e 606
yuron 4:df334779a69e 607 // センサ出力
yuron 4:df334779a69e 608 migimae.setProcessValue(abs(migimae_rpm));
yuron 4:df334779a69e 609 migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron 4:df334779a69e 610 hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron 4:df334779a69e 611 hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron 4:df334779a69e 612
yuron 4:df334779a69e 613 // 制御量(計算結果)
yuron 4:df334779a69e 614 migimae_data[0] = migimae.compute();
yuron 4:df334779a69e 615 migiusiro_data[0] = migiusiro.compute();
yuron 4:df334779a69e 616 hidarimae_data[0] = hidarimae.compute();
yuron 4:df334779a69e 617 hidariusiro_data[0] = hidariusiro.compute();
yuron 4:df334779a69e 618
yuron 4:df334779a69e 619 // 制御量をPWM値に変換
yuron 4:df334779a69e 620 true_migimae_data[0] = 0x7D - migimae_data[0];
yuron 4:df334779a69e 621 true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron 4:df334779a69e 622 true_hidarimae_data[0] = hidarimae_data[0];
yuron 4:df334779a69e 623 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 4:df334779a69e 624
yuron 4:df334779a69e 625 return 0;
yuron 4:df334779a69e 626 }
yuron 4:df334779a69e 627 int roller_PID(int front, int back) {
yuron 4:df334779a69e 628 front_roller.setInputLimits(-2000, 2000);
yuron 4:df334779a69e 629 back_roller.setInputLimits(-2000, 2000);
yuron 4:df334779a69e 630
yuron 4:df334779a69e 631 front_roller.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 632 back_roller.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 633
yuron 4:df334779a69e 634 front_roller.setMode(AUTO_MODE);
yuron 4:df334779a69e 635 back_roller.setMode(AUTO_MODE);
yuron 4:df334779a69e 636
yuron 4:df334779a69e 637 front_roller.setSetPoint(front);
yuron 4:df334779a69e 638 back_roller.setSetPoint(back);
yuron 4:df334779a69e 639
yuron 4:df334779a69e 640 front_roller.setProcessValue(abs(front_roller_rpm));
yuron 4:df334779a69e 641 back_roller.setProcessValue(abs(back_roller_rpm));
yuron 4:df334779a69e 642
yuron 4:df334779a69e 643 front_roller_data[0] = front_roller.compute();
yuron 4:df334779a69e 644 back_roller_data[0] = back_roller.compute();
yuron 4:df334779a69e 645
yuron 4:df334779a69e 646 return 0;
yuron 4:df334779a69e 647 }
yuron 4:df334779a69e 648 void linetrace() {
yuron 4:df334779a69e 649 line_state = photo.getc();
yuron 4:df334779a69e 650 }
yuron 4:df334779a69e 651 void ultrasonic() {
yuron 4:df334779a69e 652 sonic.start();
yuron 4:df334779a69e 653 wait(0.01);
yuron 4:df334779a69e 654 //get_dist_cm関数は、計測から得られた距離(cm)を返します。
yuron 4:df334779a69e 655 distance = sonic.get_dist_cm();
yuron 4:df334779a69e 656 //pc.printf("%d[cm]\r\n", distance);
yuron 4:df334779a69e 657 }
yuron 4:df334779a69e 658 int main(void) {
yuron 4:df334779a69e 659 pc.printf("HelloWorld\n");
yuron 0:f73c1b076ae4 660 emergency = 0;
yuron 4:df334779a69e 661 /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
yuron 4:df334779a69e 662 get_rps.attach_us(&flip, 40000);
yuron 4:df334779a69e 663 //get_distance.attach_us(&ultrasonic, 100000);
yuron 4:df334779a69e 664 photo.baud(115200);
yuron 4:df334779a69e 665 photo.attach(linetrace, Serial::RxIrq);
yuron 4:df334779a69e 666 side.mode(PullDown);
yuron 4:df334779a69e 667
yuron 4:df334779a69e 668 // setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
yuron 4:df334779a69e 669 sonic.setTemp(25);
yuron 4:df334779a69e 670
yuron 2:c32991ba628f 671 send_data[0] = 0x80;
yuron 4:df334779a69e 672 i2c.write(0x10, send_data, 1);
yuron 4:df334779a69e 673 i2c.write(0x12, send_data, 1);
yuron 4:df334779a69e 674 i2c.write(0x14, send_data, 1);
yuron 4:df334779a69e 675 i2c.write(0x16, send_data, 1);
yuron 3:1223cab367d9 676 i2c.write(0x20, send_data, 1);
yuron 3:1223cab367d9 677 i2c.write(0x22, send_data, 1);
yuron 4:df334779a69e 678 i2c.write(0x30, send_data, 1);
yuron 4:df334779a69e 679 i2c.write(0x40, send_data, 1);
yuron 0:f73c1b076ae4 680 wait(0.1);
yuron 0:f73c1b076ae4 681
yuron 4:df334779a69e 682 while(1) {
yuron 4:df334779a69e 683 ultrasonic();
yuron 4:df334779a69e 684 //pc.printf("%d[cm]\r\n", distance);
yuron 4:df334779a69e 685 if(side == 1){
yuron 4:df334779a69e 686 red_side = 1;
yuron 4:df334779a69e 687 blue_side = 0;
yuron 4:df334779a69e 688 } else {
yuron 4:df334779a69e 689 red_side = 0;
yuron 4:df334779a69e 690 blue_side = 1;
yuron 4:df334779a69e 691 }
yuron 4:df334779a69e 692 if(start_button == 1){
yuron 4:df334779a69e 693 /*
yuron 4:df334779a69e 694 true_migimae_data[0] = 0x80;
yuron 4:df334779a69e 695 true_migiusiro_data[0] = 0x80;
yuron 4:df334779a69e 696 true_hidarimae_data[0] = 0x80;
yuron 4:df334779a69e 697 true_hidariusiro_data[0] = 0x80;
yuron 4:df334779a69e 698 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 699 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 700 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 701 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 702 wait_us(20);
yuron 4:df334779a69e 703 */
yuron 4:df334779a69e 704 cylinder_data[0] = 0x80;
yuron 4:df334779a69e 705 myled1 = 0;
yuron 4:df334779a69e 706 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 707 } else {
yuron 4:df334779a69e 708 /*
yuron 4:df334779a69e 709 front_PID(270, 270, 270, 270);
yuron 4:df334779a69e 710 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 711 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 712 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 713 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 714 wait_us(20);
yuron 4:df334779a69e 715 */
yuron 4:df334779a69e 716 myled1 = 1;
yuron 4:df334779a69e 717 cylinder_data[0] = 0x0F;
yuron 4:df334779a69e 718 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 719 }
yuron 4:df334779a69e 720 if(user_switch2 == 0 && user_switch3 == 1){
yuron 4:df334779a69e 721 loading_data[0] = 0x0C;
yuron 4:df334779a69e 722 myled2 = 1;
yuron 4:df334779a69e 723 i2c.write(0x30, loading_data, 1);
yuron 4:df334779a69e 724 }
yuron 4:df334779a69e 725 else if(user_switch3 == 0 && user_switch2 == 1){
yuron 4:df334779a69e 726 loading_data[0] = 0xFF;
yuron 4:df334779a69e 727 myled3 = 1;
yuron 4:df334779a69e 728 i2c.write(0x30, loading_data, 1);
yuron 4:df334779a69e 729 } else {
yuron 4:df334779a69e 730 loading_data[0] = 0x80;
yuron 4:df334779a69e 731 myled2 = 0;
yuron 4:df334779a69e 732 myled3 = 0;
yuron 4:df334779a69e 733 i2c.write(0x30, loading_data, 1);
yuron 4:df334779a69e 734 }
yuron 4:df334779a69e 735
yuron 2:c32991ba628f 736 /*
yuron 4:df334779a69e 737 loading_data[0] = 0x0C;
yuron 4:df334779a69e 738 i2c.write(0x30, loading_data, 1);
yuron 4:df334779a69e 739 if(photo_interrupter.read() == 1) {
yuron 4:df334779a69e 740 myled4 = 0;
yuron 4:df334779a69e 741 loading_state = 1;
yuron 4:df334779a69e 742 loading_data[0] = 0x80;
yuron 4:df334779a69e 743 i2c.write(0x30, loading_data, 1);
yuron 4:df334779a69e 744 }
yuron 2:c32991ba628f 745 */
yuron 0:f73c1b076ae4 746
yuron 4:df334779a69e 747 /*
yuron 4:df334779a69e 748 if(user_switch5 == 0){
yuron 4:df334779a69e 749 roller_flag = 1;
yuron 4:df334779a69e 750 }
yuron 4:df334779a69e 751 if(roller_flag == 1) {
yuron 4:df334779a69e 752 myled5 = 1;
yuron 4:df334779a69e 753 timer.reset();
yuron 4:df334779a69e 754 timer.start();
yuron 4:df334779a69e 755 roller_PID(325, 650);
yuron 4:df334779a69e 756 i2c.write(0x20, front_roller_data, 1, false);
yuron 4:df334779a69e 757 i2c.write(0x22, back_roller_data, 1, false);
yuron 4:df334779a69e 758 wait_us(20);
yuron 4:df334779a69e 759
yuron 4:df334779a69e 760 if((timer.read() >= 5.0f) && (timer.read() < 5.5f)) {
yuron 4:df334779a69e 761 cylinder_data[0] = 0x0F;
yuron 4:df334779a69e 762 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 763 }
yuron 4:df334779a69e 764 else if((timer.read() >= 5.5f) && (timer.read() < 6.0f)) {
yuron 4:df334779a69e 765 cylinder_data[0] = 0x80;
yuron 4:df334779a69e 766 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 767 }
yuron 4:df334779a69e 768 else if((timer.read() >= 6.0f) && (timer.read() < 6.5f)) {
yuron 4:df334779a69e 769 cylinder_data[0] = 0x0F;
yuron 4:df334779a69e 770 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 771 }
yuron 4:df334779a69e 772 else if((timer.read() >= 6.5f) && (timer.read() < 7.0f)) {
yuron 4:df334779a69e 773 cylinder_data[0] = 0x80;
yuron 4:df334779a69e 774 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 775 }
yuron 4:df334779a69e 776 else if((timer.read() >= 7.0f) && (timer.read() < 7.5f)) {
yuron 4:df334779a69e 777 cylinder_data[0] = 0x0F;
yuron 4:df334779a69e 778 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 779 } else {
yuron 4:df334779a69e 780 cylinder_data[0] = 0x80;
yuron 4:df334779a69e 781 i2c.write(0x40, cylinder_data, 1);
yuron 4:df334779a69e 782 }
yuron 4:df334779a69e 783 timer.stop();
yuron 4:df334779a69e 784 } else {
yuron 4:df334779a69e 785 myled5 = 0;
yuron 4:df334779a69e 786 front_roller_data[0] = 0x80;
yuron 4:df334779a69e 787 back_roller_data[0] = 0x80;
yuron 4:df334779a69e 788 i2c.write(0x20, front_roller_data, 1, false);
yuron 4:df334779a69e 789 i2c.write(0x22, back_roller_data, 1, false);
yuron 4:df334779a69e 790 }
yuron 4:df334779a69e 791 */
yuron 3:1223cab367d9 792
yuron 4:df334779a69e 793 /*
yuron 4:df334779a69e 794 if(distance < 30 && distance != 0) {
yuron 4:df334779a69e 795 //front_PID(200, 200, 200, 200);
yuron 4:df334779a69e 796 //i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 797 //i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 798 //i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 799 //i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 800 } else {
yuron 4:df334779a69e 801 //send_data[0] = 0x80;
yuron 4:df334779a69e 802 //i2c.write(0x10, send_data, 1, false);
yuron 4:df334779a69e 803 //i2c.write(0x12, send_data, 1, false);
yuron 4:df334779a69e 804 //i2c.write(0x14, send_data, 1, false);
yuron 4:df334779a69e 805 //i2c.write(0x16, send_data, 1, false);
yuron 4:df334779a69e 806 }
yuron 4:df334779a69e 807 */
yuron 4:df334779a69e 808
yuron 4:df334779a69e 809 //pc.printf("%d\n", line_state);
yuron 4:df334779a69e 810 if(line_state == 0){
yuron 4:df334779a69e 811 pc.printf("真ん中\n");
yuron 4:df334779a69e 812 front_PID(70, 70, 70, 70);
yuron 4:df334779a69e 813 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 814 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 815 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 816 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 817 }
yuron 4:df334779a69e 818 else if(line_state == 1) {
yuron 4:df334779a69e 819 pc.printf("右\n");
yuron 4:df334779a69e 820 turn_right_PID(30, 30, 30, 30);
yuron 4:df334779a69e 821 //right_front_PID(50, 50);
yuron 4:df334779a69e 822 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 823 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 824 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 825 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 826 }
yuron 4:df334779a69e 827 else if(line_state == 2) {
yuron 4:df334779a69e 828 pc.printf("左\n");
yuron 4:df334779a69e 829 turn_left_PID(30, 30, 30, 30);
yuron 4:df334779a69e 830 //left_front_PID(50, 50);
yuron 4:df334779a69e 831 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 832 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 833 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 834 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 835 }
yuron 4:df334779a69e 836 else if(line_state == 3) {
yuron 4:df334779a69e 837 pc.printf("範囲外\n");
yuron 4:df334779a69e 838 send_data[0] = 0x80;
yuron 4:df334779a69e 839 i2c.write(0x10, send_data, 1, false);
yuron 4:df334779a69e 840 i2c.write(0x12, send_data, 1, false);
yuron 4:df334779a69e 841 i2c.write(0x14, send_data, 1, false);
yuron 4:df334779a69e 842 i2c.write(0x16, send_data, 1, false);
yuron 4:df334779a69e 843 }
yuron 3:1223cab367d9 844
yuron 3:1223cab367d9 845 /*
yuron 4:df334779a69e 846 front_PID(270, 270, 270, 270);
yuron 4:df334779a69e 847 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 848 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 849 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 850 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 4:df334779a69e 851 wait_us(20);
yuron 4:df334779a69e 852
yuron 4:df334779a69e 853 back_PID(270, 270, 270, 270);
yuron 4:df334779a69e 854 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 855 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 856 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 857 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 858 wait_us(20);
yuron 4:df334779a69e 859
yuron 4:df334779a69e 860 right_PID(100, 100, 100, 100);
yuron 4:df334779a69e 861 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 862 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 863 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 864 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 865 wait_us(20);
yuron 4:df334779a69e 866
yuron 4:df334779a69e 867 left_PID(100, 100, 100, 100);
yuron 4:df334779a69e 868 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 869 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 870 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 871 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 872 wait_us(20);
yuron 4:df334779a69e 873
yuron 4:df334779a69e 874 //斜め右前
yuron 4:df334779a69e 875 right_front_PID(270, 270);
yuron 4:df334779a69e 876 true_migiusiro_data[0] = 0x80;
yuron 4:df334779a69e 877 true_hidariusiro_data[0] = 0x80;
yuron 4:df334779a69e 878 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 879 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 880 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 881 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 882 wait_us(20);
yuron 4:df334779a69e 883
yuron 4:df334779a69e 884 //斜め右後
yuron 4:df334779a69e 885 right_back_PID(270, 270);
yuron 4:df334779a69e 886 true_migimae_data[0] = 0x80;
yuron 4:df334779a69e 887 true_hidarimae_data[0] = 0x80;
yuron 4:df334779a69e 888 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 889 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 890 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 891 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 892 wait_us(20);
yuron 4:df334779a69e 893
yuron 4:df334779a69e 894 //斜め左前
yuron 4:df334779a69e 895 left_front_PID(270, 270);
yuron 4:df334779a69e 896 true_migimae_data[0] = 0x80;
yuron 4:df334779a69e 897 true_hidarimae_data[0] = 0x80;
yuron 4:df334779a69e 898 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 899 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 900 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 901 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 902 wait_us(20);
yuron 4:df334779a69e 903
yuron 4:df334779a69e 904 //斜め左後
yuron 4:df334779a69e 905 left_back_PID(270, 270);
yuron 4:df334779a69e 906 true_migiusiro_data[0] = 0x80;
yuron 4:df334779a69e 907 true_hidariusiro_data[0] = 0x80;
yuron 4:df334779a69e 908 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 909 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 910 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 911 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 912 wait_us(20);
yuron 4:df334779a69e 913
yuron 4:df334779a69e 914 turn_right_PID(200, 200, 200, 200);
yuron 4:df334779a69e 915 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 916 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 917 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 918 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 919 wait_us(20);
yuron 4:df334779a69e 920
yuron 4:df334779a69e 921 turn_left_PID(100, 100, 100, 100);
yuron 4:df334779a69e 922 i2c.write(0x10, true_migimae_data, 1, false);
yuron 4:df334779a69e 923 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 4:df334779a69e 924 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 4:df334779a69e 925 i2c.write(0x16, true_hidariusiro_data,1, false);
yuron 4:df334779a69e 926 wait_us(20);
yuron 4:df334779a69e 927
yuron 4:df334779a69e 928 roller_PID(1000, 1000);
yuron 4:df334779a69e 929 i2c.write(0x20, front_roller_data, 1, false);
yuron 4:df334779a69e 930 i2c.write(0x22, back_roller_data, 1, false);
yuron 4:df334779a69e 931 wait_us(20);
yuron 0:f73c1b076ae4 932 */
yuron 0:f73c1b076ae4 933
yuron 0:f73c1b076ae4 934 /*
yuron 2:c32991ba628f 935 //どんどん加速(逆転)
yuron 2:c32991ba628f 936 for(send_data[0] = 0x81; send_data[0] < 0xF5; send_data[0]++){
yuron 0:f73c1b076ae4 937 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 938 //ice(0xA2, send_data);
yuron 2:c32991ba628f 939
yuron 0:f73c1b076ae4 940 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 941 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 942 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 943 i2c.write(0xA6, send_data, 1);
yuron 2:c32991ba628f 944
yuron 2:c32991ba628f 945 i2c.write(0x20, send_data, 1);
yuron 0:f73c1b076ae4 946 wait(0.1);
yuron 0:f73c1b076ae4 947 }
yuron 2:c32991ba628f 948 //だんだん減速(逆転)
yuron 2:c32991ba628f 949 for(send_data[0] = 0xF4; send_data[0] > 0x80; send_data[0]--){
yuron 0:f73c1b076ae4 950 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 951 //ice(0xA2, send_data);
yuron 2:c32991ba628f 952
yuron 0:f73c1b076ae4 953 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 954 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 955 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 956 i2c.write(0xA6, send_data, 1);
yuron 2:c32991ba628f 957
yuron 2:c32991ba628f 958 i2c.write(0x20, send_data, 1);
yuron 0:f73c1b076ae4 959 wait(0.1);
yuron 0:f73c1b076ae4 960 }
yuron 2:c32991ba628f 961 send_data[0] = 0x80;
yuron 2:c32991ba628f 962 i2c.write(0x20, send_data, 1);
yuron 2:c32991ba628f 963 wait(5);
yuron 0:f73c1b076ae4 964 */
yuron 0:f73c1b076ae4 965 }
yuron 0:f73c1b076ae4 966 }