2019NHK_teamA / Mbed 2 deprecated 2019_A_ver8

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI PID

main.cpp@26:81346a21d301, 2019-10-08 (annotated)

Committer:
yuron
Date:
Tue Oct 08 02:03:01 2019 +0000
Revision:
26:81346a21d301
Parent:
25:ce789ea15628
Child:
27:4f2fc7172b31
aaaaaaaaaa;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuron 14:ab89b6cd9719 1 /* ------------------------------------------------------------------- */
yuron 14:ab89b6cd9719 2 /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */
yuron 14:ab89b6cd9719 3 /* Nucleo Type: F446RE */
yuron 14:ab89b6cd9719 4 /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */
yuron 22:5682246f9409 5 /* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */
yuron 22:5682246f9409 6 /* Sensor: encorder*4, limit_switch*14 */
yuron 14:ab89b6cd9719 7 /* ------------------------------------------------------------------- */
yuron 24:d12bc20c01c2 8 /* Both of areas are compleated! */
yuron 14:ab89b6cd9719 9 /* ------------------------------------------------------------------- */
yuron 0:f73c1b076ae4 10 #include "mbed.h"
yuron 0:f73c1b076ae4 11 #include "math.h"
yuron 0:f73c1b076ae4 12 #include "QEI.h"
yuron 0:f73c1b076ae4 13 #include "PID.h"
yuron 5:167327a82430 14
yuron 23:1e4d7540715f 15 //終了phase
yuron 25:ce789ea15628 16 #define FINAL_PHASE 50
yuron 23:1e4d7540715f 17
yuron 16:05b26003da50 18 #define RED 0
yuron 16:05b26003da50 19 #define BLUE 1
yuron 16:05b26003da50 20
yuron 19:f17d2e585973 21 //PID Gain of wheels(Kp, Ti, Td, control cycle)
yuron 5:167327a82430 22 //前進
yuron 14:ab89b6cd9719 23 PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 24 PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 25 PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 26 PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 27
yuron 5:167327a82430 28 //後進
yuron 14:ab89b6cd9719 29 PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 30 PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 31 PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
yuron 14:ab89b6cd9719 32 PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);
yuron 8:3df97287c825 33
yuron 14:ab89b6cd9719 34 //右進
yuron 17:de3bc1999ae7 35 PID right_migimae(6000000.0, 0.0, 0.0, 0.001);
yuron 17:de3bc1999ae7 36 PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001);
yuron 17:de3bc1999ae7 37 PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001);
yuron 17:de3bc1999ae7 38 PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
yuron 9:1359f0c813b1 39
yuron 14:ab89b6cd9719 40 //左進
yuron 17:de3bc1999ae7 41 PID left_migimae(6000000.0, 0.0, 0.0, 0.001);
yuron 17:de3bc1999ae7 42 PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001);
yuron 17:de3bc1999ae7 43 PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001);
yuron 17:de3bc1999ae7 44 PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);
yuron 10:b672aa81b226 45
yuron 14:ab89b6cd9719 46 //右旋回
yuron 22:5682246f9409 47 PID turn_right_migimae(3000000.0, 0.0, 0.0, 0.001);
yuron 22:5682246f9409 48 PID turn_right_migiusiro(3000000.0, 0.0, 0.0, 0.001);
yuron 22:5682246f9409 49 PID turn_right_hidarimae(3000000.0, 0.0, 0.0, 0.001);
yuron 22:5682246f9409 50 PID turn_right_hidariusiro(3000000.0, 0.0, 0.0, 0.001);
yuron 4:df334779a69e 51
yuron 14:ab89b6cd9719 52 //左旋回
yuron 22:5682246f9409 53 PID turn_left_migimae(3000000.0, 0.0, 0.0, 0.001);
yuron 22:5682246f9409 54 PID turn_left_migiusiro(3000000.0, 0.0, 0.0, 0.001);
yuron 22:5682246f9409 55 PID turn_left_hidarimae(3000000.0, 0.0, 0.0, 0.001);
yuron 22:5682246f9409 56 PID turn_left_hidariusiro(3000000.0, 0.0, 0.0, 0.001);
yuron 0:f73c1b076ae4 57
yuron 4:df334779a69e 58 //MDとの通信ポート
yuron 4:df334779a69e 59 I2C i2c(PB_9, PB_8); //SDA, SCL
yuron 14:ab89b6cd9719 60
yuron 4:df334779a69e 61 //PCとの通信ポート
yuron 4:df334779a69e 62 Serial pc(USBTX, USBRX); //TX, RX
yuron 4:df334779a69e 63
yuron 17:de3bc1999ae7 64 //特小モジュールとの通信ポート
yuron 17:de3bc1999ae7 65 Serial pic(A0, A1);
yuron 17:de3bc1999ae7 66
yuron 18:851f783ec516 67 //リミットスイッチ基板との通信ポート
yuron 18:851f783ec516 68 Serial limit_serial(PC_12, PD_2);
yuron 18:851f783ec516 69
yuron 4:df334779a69e 70 //12V停止信号ピン
yuron 14:ab89b6cd9719 71 DigitalOut emergency(D11);
yuron 4:df334779a69e 72
yuron 16:05b26003da50 73 DigitalOut USR_LED1(PB_7);
yuron 21:89db2a19e52e 74 //DigitalOut USR_LED2(PC_13);
yuron 16:05b26003da50 75 DigitalOut USR_LED3(PC_2);
yuron 16:05b26003da50 76 DigitalOut USR_LED4(PC_3);
yuron 21:89db2a19e52e 77 DigitalOut GREEN_LED(D8);
yuron 21:89db2a19e52e 78 DigitalOut RED_LED(D10);
yuron 24:d12bc20c01c2 79 DigitalOut YELLOW_LED(D9);
yuron 16:05b26003da50 80
yuron 17:de3bc1999ae7 81 //遠隔非常停止ユニットLED
yuron 17:de3bc1999ae7 82 AnalogOut myled(A2);
yuron 17:de3bc1999ae7 83
yuron 16:05b26003da50 84 DigitalIn start_switch(PB_12);
yuron 21:89db2a19e52e 85 DigitalIn USR_SWITCH(PC_13);
yuron 21:89db2a19e52e 86 DigitalIn zone_switch(PC_10);
yuron 8:3df97287c825 87
yuron 14:ab89b6cd9719 88 QEI wheel_x1(PA_8 , PA_6 , NC, 624);
yuron 14:ab89b6cd9719 89 QEI wheel_x2(PB_14, PB_13, NC, 624);
yuron 14:ab89b6cd9719 90 QEI wheel_y1(PB_1 , PB_15, NC, 624);
yuron 14:ab89b6cd9719 91 QEI wheel_y2(PA_12, PA_11, NC, 624);
yuron 25:ce789ea15628 92 QEI arm_enc(PB_4, PB_5 , NC, 624);
yuron 14:ab89b6cd9719 93
yuron 19:f17d2e585973 94 //移動後n秒停止タイマー
yuron 17:de3bc1999ae7 95 Timer counter;
yuron 16:05b26003da50 96
yuron 14:ab89b6cd9719 97 //エンコーダ値格納変数
yuron 23:1e4d7540715f 98 int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse, arm_pulse;
yuron 14:ab89b6cd9719 99
yuron 14:ab89b6cd9719 100 //操作の段階変数
yuron 14:ab89b6cd9719 101 unsigned int phase = 0;
yuron 19:f17d2e585973 102 int kaisyu_phase = 0;
yuron 19:f17d2e585973 103 int tyokudo_phase = 0;
yuron 16:05b26003da50 104 unsigned int start_zone = 1;
yuron 16:05b26003da50 105 bool zone = RED;
yuron 0:f73c1b076ae4 106
yuron 19:f17d2e585973 107 //i2c送信データ変数
yuron 14:ab89b6cd9719 108 char init_send_data[1];
yuron 14:ab89b6cd9719 109 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
yuron 14:ab89b6cd9719 110 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
yuron 21:89db2a19e52e 111 char arm_motor[1], drop_motor[1];
yuron 19:f17d2e585973 112 char fan_data[1];
yuron 19:f17d2e585973 113 char servo_data[1];
yuron 20:ac4954be1fe0 114 char right_arm_data[1], left_arm_data[1];
yuron 0:f73c1b076ae4 115
yuron 17:de3bc1999ae7 116 //非常停止関連変数
yuron 17:de3bc1999ae7 117 char RDATA;
yuron 17:de3bc1999ae7 118 char baff;
yuron 17:de3bc1999ae7 119 int flug = 0;
yuron 17:de3bc1999ae7 120
yuron 19:f17d2e585973 121 //リミット基板からの受信データ
yuron 18:851f783ec516 122 int limit_data = 0;
yuron 19:f17d2e585973 123 int upper_limit_data = 0;
yuron 19:f17d2e585973 124 int lower_limit_data = 0;
yuron 18:851f783ec516 125
yuron 19:f17d2e585973 126 //各辺のスイッチが押されたかのフラグ
yuron 19:f17d2e585973 127 //前部が壁に当たっているか
yuron 19:f17d2e585973 128 int front_limit = 0;
yuron 19:f17d2e585973 129 //右部が壁にあたあっているか
yuron 19:f17d2e585973 130 int right_limit = 0;
yuron 19:f17d2e585973 131 //後部が壁に当たっているか
yuron 19:f17d2e585973 132 int back_limit = 0;
yuron 20:ac4954be1fe0 133 //回収機構の下限(引っ込めてるほう)
yuron 21:89db2a19e52e 134 bool kaisyu_mae_limit = 0;
yuron 21:89db2a19e52e 135
yuron 21:89db2a19e52e 136 bool kaisyu_usiro_limit = 0;
yuron 21:89db2a19e52e 137
yuron 20:ac4954be1fe0 138 //右腕の下限
yuron 20:ac4954be1fe0 139 bool right_arm_lower_limit = 0;
yuron 19:f17d2e585973 140 //右腕の上限
yuron 19:f17d2e585973 141 bool right_arm_upper_limit = 0;
yuron 19:f17d2e585973 142 //左腕の下限
yuron 19:f17d2e585973 143 bool left_arm_lower_limit = 0;
yuron 20:ac4954be1fe0 144 //左腕の上限
yuron 20:ac4954be1fe0 145 bool left_arm_upper_limit = 0;
yuron 19:f17d2e585973 146 //吐き出し機構の上限
yuron 20:ac4954be1fe0 147 bool tyokudo_mae_limit = 0;
yuron 20:ac4954be1fe0 148 //吐き出し機構の下限
yuron 20:ac4954be1fe0 149 bool tyokudo_usiro_limit = 0;
yuron 20:ac4954be1fe0 150
yuron 24:d12bc20c01c2 151 int masked_lower_front_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 152 int masked_lower_back_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 153 int masked_lower_right_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 154 int masked_kaisyu_mae_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 155 int masked_kaisyu_usiro_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 156 int masked_right_arm_lower_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 157 int masked_right_arm_upper_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 158 int masked_left_arm_lower_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 159 int masked_left_arm_upper_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 160 int masked_tyokudo_mae_limit_data = 0b11111111;
yuron 24:d12bc20c01c2 161 int masked_tyokudo_usiro_limit_data = 0b11111111;
yuron 18:851f783ec516 162
yuron 14:ab89b6cd9719 163 //関数のプロトタイプ宣言
yuron 14:ab89b6cd9719 164 void init(void);
yuron 14:ab89b6cd9719 165 void init_send(void);
yuron 17:de3bc1999ae7 166 void get(void);
yuron 14:ab89b6cd9719 167 void get_pulses(void);
yuron 14:ab89b6cd9719 168 void print_pulses(void);
yuron 17:de3bc1999ae7 169 void get_emergency(void);
yuron 18:851f783ec516 170 void read_limit(void);
yuron 19:f17d2e585973 171 void wheel_reset(void);
yuron 21:89db2a19e52e 172 void kaisyu(int pulse, int next_phase);
yuron 23:1e4d7540715f 173 void kaisyu_nobasu(int pulse, int next_phase);
yuron 23:1e4d7540715f 174 void kaisyu_hiku(int pulse, int next_phase);
yuron 21:89db2a19e52e 175 void tyokudo(int pulse, int next_phase);
yuron 21:89db2a19e52e 176 void arm_up(int next_phase);
yuron 17:de3bc1999ae7 177 void front(int target);
yuron 17:de3bc1999ae7 178 void back(int target);
yuron 17:de3bc1999ae7 179 void right(int target);
yuron 17:de3bc1999ae7 180 void left(int target);
yuron 17:de3bc1999ae7 181 void turn_right(int target);
yuron 17:de3bc1999ae7 182 void turn_left(int target);
yuron 18:851f783ec516 183 void stop(void);
yuron 22:5682246f9409 184 void all_stop(void);
yuron 17:de3bc1999ae7 185 void front_PID(int target);
yuron 17:de3bc1999ae7 186 void back_PID(int target);
yuron 17:de3bc1999ae7 187 void right_PID(int target);
yuron 17:de3bc1999ae7 188 void left_PID(int target);
yuron 17:de3bc1999ae7 189 void turn_right_PID(int target);
yuron 17:de3bc1999ae7 190 void turn_left_PID(int target);
yuron 8:3df97287c825 191
yuron 14:ab89b6cd9719 192 int main(void) {
yuron 20:ac4954be1fe0 193
yuron 14:ab89b6cd9719 194 init();
yuron 14:ab89b6cd9719 195 init_send();
yuron 26:81346a21d301 196 //後で消せ俺さん！！
yuron 26:81346a21d301 197 //phase = 50;
yuron 21:89db2a19e52e 198
yuron 21:89db2a19e52e 199 //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
yuron 21:89db2a19e52e 200 while(1) {
yuron 21:89db2a19e52e 201 if(zone_switch == 0) {
yuron 21:89db2a19e52e 202 zone = BLUE;
yuron 21:89db2a19e52e 203 } else {
yuron 21:89db2a19e52e 204 zone = RED;
yuron 21:89db2a19e52e 205 }
yuron 21:89db2a19e52e 206 break;
yuron 21:89db2a19e52e 207 }
yuron 21:89db2a19e52e 208
yuron 14:ab89b6cd9719 209 while(1) {
yuron 20:ac4954be1fe0 210
yuron 14:ab89b6cd9719 211 get_pulses();
yuron 23:1e4d7540715f 212 //print_pulses();
yuron 17:de3bc1999ae7 213 get_emergency();
yuron 18:851f783ec516 214 read_limit();
yuron 21:89db2a19e52e 215
yuron 21:89db2a19e52e 216 //move_servo_with_using_onboard-switch
yuron 21:89db2a19e52e 217 if(USR_SWITCH == 0) {
yuron 21:89db2a19e52e 218 servo_data[0] = 0x03;
yuron 21:89db2a19e52e 219 i2c.write(0x30, servo_data, 1);
yuron 21:89db2a19e52e 220 } else {
yuron 21:89db2a19e52e 221 servo_data[0] = 0x04;
yuron 21:89db2a19e52e 222 i2c.write(0x30, servo_data, 1);
yuron 21:89db2a19e52e 223 }
yuron 19:f17d2e585973 224
yuron 26:81346a21d301 225 /*
yuron 26:81346a21d301 226 if(start_switch == 1) {
yuron 26:81346a21d301 227 phase = 37;
yuron 26:81346a21d301 228 }
yuron 26:81346a21d301 229 */
yuron 26:81346a21d301 230
yuron 19:f17d2e585973 231 //青ゾーン
yuron 18:851f783ec516 232 if(zone == BLUE) {
yuron 21:89db2a19e52e 233 GREEN_LED = 1;
yuron 21:89db2a19e52e 234 RED_LED = 0;
yuron 23:1e4d7540715f 235
yuron 18:851f783ec516 236 switch(phase) {
yuron 20:ac4954be1fe0 237
yuron 19:f17d2e585973 238 //スタート位置へセット
yuron 18:851f783ec516 239 case 0:
yuron 25:ce789ea15628 240 //スタートスイッチが押されたか
yuron 25:ce789ea15628 241 if(start_switch == 1) {
yuron 25:ce789ea15628 242 wheel_reset();
yuron 25:ce789ea15628 243 phase = 1;
yuron 25:ce789ea15628 244 }
yuron 25:ce789ea15628 245
yuron 19:f17d2e585973 246 //リミットが洗濯物台に触れているか
yuron 19:f17d2e585973 247 if(right_limit == 3) {
yuron 19:f17d2e585973 248 USR_LED1 = 1;
yuron 19:f17d2e585973 249 } else {
yuron 19:f17d2e585973 250 USR_LED1 = 0;
yuron 18:851f783ec516 251 }
yuron 18:851f783ec516 252 break;
yuron 20:ac4954be1fe0 253
yuron 23:1e4d7540715f 254 //回収アームを伸ばす
yuron 22:5682246f9409 255 case 1:
yuron 23:1e4d7540715f 256 counter.reset();
yuron 23:1e4d7540715f 257 kaisyu_nobasu(arm_pulse, 2);
yuron 23:1e4d7540715f 258 //サーボを開いておく
yuron 20:ac4954be1fe0 259 servo_data[0] = 0x03;
yuron 20:ac4954be1fe0 260 i2c.write(0x30, servo_data, 1);
yuron 19:f17d2e585973 261 break;
yuron 20:ac4954be1fe0 262
yuron 26:81346a21d301 263 //1秒停止
yuron 18:851f783ec516 264 case 2:
yuron 18:851f783ec516 265 stop();
yuron 20:ac4954be1fe0 266 servo_data[0] = 0x04;
yuron 20:ac4954be1fe0 267 i2c.write(0x30, servo_data, 1);
yuron 18:851f783ec516 268 counter.start();
yuron 26:81346a21d301 269 if(counter.read() > 1.0f) {
yuron 18:851f783ec516 270 phase = 3;
yuron 19:f17d2e585973 271 wheel_reset();
yuron 18:851f783ec516 272 }
yuron 18:851f783ec516 273 break;
yuron 20:ac4954be1fe0 274
yuron 25:ce789ea15628 275 //ちょっと前進
yuron 18:851f783ec516 276 case 3:
yuron 18:851f783ec516 277 counter.reset();
yuron 25:ce789ea15628 278 front(800);
yuron 26:81346a21d301 279 if((y_pulse1 > 700) || (y_pulse2 > 700)) {
yuron 25:ce789ea15628 280 phase = 4;
yuron 25:ce789ea15628 281 }
yuron 25:ce789ea15628 282 if(front_limit == 3) {
yuron 18:851f783ec516 283 phase = 4;
yuron 18:851f783ec516 284 }
yuron 18:851f783ec516 285 break;
yuron 20:ac4954be1fe0 286
yuron 25:ce789ea15628 287 //0.5秒停止
yuron 18:851f783ec516 288 case 4:
yuron 18:851f783ec516 289 stop();
yuron 18:851f783ec516 290 counter.start();
yuron 25:ce789ea15628 291 if(counter.read() > 0.5f) {
yuron 19:f17d2e585973 292 phase = 5;
yuron 19:f17d2e585973 293 wheel_reset();
yuron 18:851f783ec516 294 }
yuron 18:851f783ec516 295 break;
yuron 20:ac4954be1fe0 296
yuron 23:1e4d7540715f 297 //回収アーム引っ込める
yuron 18:851f783ec516 298 case 5:
yuron 26:81346a21d301 299 USR_LED3 = 1;
yuron 26:81346a21d301 300 //kaisyu_hiku(arm_pulse, 6);
yuron 26:81346a21d301 301
yuron 26:81346a21d301 302 if(kaisyu_mae_limit == 0) {
yuron 26:81346a21d301 303 arm_motor[0] = 0x4C;
yuron 26:81346a21d301 304 }
yuron 26:81346a21d301 305 else if(kaisyu_mae_limit == 1) {
yuron 26:81346a21d301 306 arm_motor[0] = 0x80;
yuron 26:81346a21d301 307 phase = 6;
yuron 26:81346a21d301 308 }
yuron 26:81346a21d301 309
yuron 18:851f783ec516 310 break;
yuron 20:ac4954be1fe0 311
yuron 25:ce789ea15628 312 //0.5秒停止
yuron 18:851f783ec516 313 case 6:
yuron 18:851f783ec516 314 stop();
yuron 18:851f783ec516 315 counter.start();
yuron 25:ce789ea15628 316 if(counter.read() > 0.5f) {
yuron 18:851f783ec516 317 phase = 7;
yuron 19:f17d2e585973 318 wheel_reset();
yuron 18:851f783ec516 319 }
yuron 18:851f783ec516 320 break;
yuron 25:ce789ea15628 321
yuron 25:ce789ea15628 322 //ちょっと後進
yuron 18:851f783ec516 323 case 7:
yuron 23:1e4d7540715f 324 counter.reset();
yuron 25:ce789ea15628 325 back(-700);
yuron 25:ce789ea15628 326 if((y_pulse1*-1 > 700) || (y_pulse2*-1 > 700)) {
yuron 18:851f783ec516 327 phase = 8;
yuron 18:851f783ec516 328 }
yuron 18:851f783ec516 329 break;
yuron 25:ce789ea15628 330
yuron 25:ce789ea15628 331 //0.5秒停止
yuron 18:851f783ec516 332 case 8:
yuron 18:851f783ec516 333 stop();
yuron 18:851f783ec516 334 counter.start();
yuron 25:ce789ea15628 335 if(counter.read() > 0.5f) {
yuron 18:851f783ec516 336 phase = 9;
yuron 19:f17d2e585973 337 wheel_reset();
yuron 18:851f783ec516 338 }
yuron 18:851f783ec516 339 break;
yuron 25:ce789ea15628 340
yuron 25:ce789ea15628 341 //左移動
yuron 18:851f783ec516 342 case 9:
yuron 18:851f783ec516 343 counter.reset();
yuron 25:ce789ea15628 344 left(11500);
yuron 25:ce789ea15628 345 if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
yuron 23:1e4d7540715f 346 phase = 10;
yuron 23:1e4d7540715f 347 }
yuron 19:f17d2e585973 348 break;
yuron 20:ac4954be1fe0 349
yuron 19:f17d2e585973 350 //1秒停止
yuron 21:89db2a19e52e 351 case 10:
yuron 19:f17d2e585973 352 stop();
yuron 19:f17d2e585973 353 counter.start();
yuron 19:f17d2e585973 354 if(counter.read() > 1.0f) {
yuron 21:89db2a19e52e 355 phase = 11;
yuron 19:f17d2e585973 356 wheel_reset();
yuron 18:851f783ec516 357 }
yuron 18:851f783ec516 358 break;
yuron 20:ac4954be1fe0 359
yuron 25:ce789ea15628 360 //右旋回(180°)
yuron 25:ce789ea15628 361 case 11:
yuron 25:ce789ea15628 362 counter.reset();
yuron 25:ce789ea15628 363 turn_right(975);
yuron 25:ce789ea15628 364 if(sum_pulse > 975) {
yuron 25:ce789ea15628 365 phase = 12;
yuron 25:ce789ea15628 366 }
yuron 25:ce789ea15628 367 break;
yuron 25:ce789ea15628 368
yuron 25:ce789ea15628 369 //0.5秒停止
yuron 25:ce789ea15628 370 case 12:
yuron 25:ce789ea15628 371 stop();
yuron 25:ce789ea15628 372 counter.start();
yuron 25:ce789ea15628 373 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 374 phase = 13;
yuron 25:ce789ea15628 375 wheel_reset();
yuron 25:ce789ea15628 376 }
yuron 25:ce789ea15628 377 break;
yuron 25:ce789ea15628 378
yuron 23:1e4d7540715f 379 //壁に当たるまで後進
yuron 25:ce789ea15628 380 case 13:
yuron 19:f17d2e585973 381 counter.reset();
yuron 23:1e4d7540715f 382
yuron 23:1e4d7540715f 383 if(back_limit == 3) {
yuron 25:ce789ea15628 384 phase = 14;
yuron 19:f17d2e585973 385 }
yuron 23:1e4d7540715f 386 else if(back_limit != 3){
yuron 23:1e4d7540715f 387 true_migimae_data[0] = 0x50;
yuron 23:1e4d7540715f 388 true_migiusiro_data[0] = 0x50;
yuron 23:1e4d7540715f 389 true_hidarimae_data[0] = 0x50;
yuron 23:1e4d7540715f 390 true_hidariusiro_data[0] = 0x50;
yuron 23:1e4d7540715f 391 i2c.write(0x10, true_migimae_data, 1, false);
yuron 23:1e4d7540715f 392 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 23:1e4d7540715f 393 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 23:1e4d7540715f 394 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 395 wait_us(20);
yuron 23:1e4d7540715f 396 }
yuron 19:f17d2e585973 397 break;
yuron 20:ac4954be1fe0 398
yuron 25:ce789ea15628 399 //0.5秒停止
yuron 25:ce789ea15628 400 case 14:
yuron 18:851f783ec516 401 stop();
yuron 18:851f783ec516 402 counter.start();
yuron 25:ce789ea15628 403 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 404 phase = 15;
yuron 19:f17d2e585973 405 wheel_reset();
yuron 19:f17d2e585973 406 }
yuron 19:f17d2e585973 407 break;
yuron 20:ac4954be1fe0 408
yuron 22:5682246f9409 409 //壁に当たるまで右移動
yuron 25:ce789ea15628 410 case 15:
yuron 19:f17d2e585973 411 counter.reset();
yuron 23:1e4d7540715f 412
yuron 21:89db2a19e52e 413 if(right_limit == 3) {
yuron 25:ce789ea15628 414 phase = 16;
yuron 19:f17d2e585973 415 }
yuron 22:5682246f9409 416 else if(right_limit != 3) {
yuron 22:5682246f9409 417 true_migimae_data[0] = 0x40;
yuron 22:5682246f9409 418 true_migiusiro_data[0] = 0xBF;
yuron 22:5682246f9409 419 true_hidarimae_data[0] = 0xBF;
yuron 22:5682246f9409 420 true_hidariusiro_data[0] = 0x40;
yuron 22:5682246f9409 421 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 422 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 423 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 424 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 425 wait_us(20);
yuron 22:5682246f9409 426 }
yuron 21:89db2a19e52e 427 break;
yuron 23:1e4d7540715f 428
yuron 25:ce789ea15628 429 //0.5秒停止
yuron 25:ce789ea15628 430 case 16:
yuron 21:89db2a19e52e 431 stop();
yuron 21:89db2a19e52e 432 counter.start();
yuron 25:ce789ea15628 433 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 434 phase = 17;
yuron 22:5682246f9409 435 wheel_reset();
yuron 22:5682246f9409 436 }
yuron 22:5682246f9409 437 break;
yuron 23:1e4d7540715f 438
yuron 23:1e4d7540715f 439 //排出
yuron 23:1e4d7540715f 440 case 17:
yuron 23:1e4d7540715f 441 counter.reset();
yuron 25:ce789ea15628 442 tyokudo(arm_enc.getPulses(), 18);
yuron 23:1e4d7540715f 443 break;
yuron 23:1e4d7540715f 444
yuron 23:1e4d7540715f 445 //1秒停止
yuron 23:1e4d7540715f 446 case 18:
yuron 23:1e4d7540715f 447 stop();
yuron 23:1e4d7540715f 448 counter.start();
yuron 23:1e4d7540715f 449 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 450 phase = 19;
yuron 23:1e4d7540715f 451 wheel_reset();
yuron 23:1e4d7540715f 452 }
yuron 23:1e4d7540715f 453 break;
yuron 23:1e4d7540715f 454
yuron 25:ce789ea15628 455 //前進
yuron 25:ce789ea15628 456 case 19:
yuron 25:ce789ea15628 457 counter.reset();
yuron 25:ce789ea15628 458 front(5000);
yuron 25:ce789ea15628 459 if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
yuron 25:ce789ea15628 460 phase = 20;
yuron 25:ce789ea15628 461 }
yuron 25:ce789ea15628 462 break;
yuron 25:ce789ea15628 463
yuron 25:ce789ea15628 464 //0.5秒停止
yuron 25:ce789ea15628 465 case 20:
yuron 25:ce789ea15628 466 stop();
yuron 25:ce789ea15628 467 counter.start();
yuron 25:ce789ea15628 468 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 469 phase = 21;
yuron 25:ce789ea15628 470 wheel_reset();
yuron 25:ce789ea15628 471 }
yuron 25:ce789ea15628 472 break;
yuron 25:ce789ea15628 473
yuron 23:1e4d7540715f 474 //壁に当たるまで右移動
yuron 25:ce789ea15628 475 case 21:
yuron 23:1e4d7540715f 476 counter.reset();
yuron 23:1e4d7540715f 477
yuron 23:1e4d7540715f 478 if(right_limit == 3) {
yuron 25:ce789ea15628 479 phase = 22;
yuron 23:1e4d7540715f 480 }
yuron 23:1e4d7540715f 481 else if(right_limit != 3) {
yuron 23:1e4d7540715f 482 true_migimae_data[0] = 0x40;
yuron 23:1e4d7540715f 483 true_migiusiro_data[0] = 0xBF;
yuron 23:1e4d7540715f 484 true_hidarimae_data[0] = 0xBF;
yuron 23:1e4d7540715f 485 true_hidariusiro_data[0] = 0x40;
yuron 23:1e4d7540715f 486 i2c.write(0x10, true_migimae_data, 1, false);
yuron 23:1e4d7540715f 487 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 23:1e4d7540715f 488 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 23:1e4d7540715f 489 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 490 wait_us(20);
yuron 23:1e4d7540715f 491 }
yuron 23:1e4d7540715f 492 break;
yuron 23:1e4d7540715f 493
yuron 25:ce789ea15628 494 //0.5秒停止
yuron 25:ce789ea15628 495 case 22:
yuron 23:1e4d7540715f 496 stop();
yuron 23:1e4d7540715f 497 counter.start();
yuron 25:ce789ea15628 498 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 499 phase = 23;
yuron 23:1e4d7540715f 500 wheel_reset();
yuron 23:1e4d7540715f 501 }
yuron 23:1e4d7540715f 502 break;
yuron 23:1e4d7540715f 503
yuron 23:1e4d7540715f 504 //壁に当たるまで後進
yuron 25:ce789ea15628 505 case 23:
yuron 23:1e4d7540715f 506 counter.reset();
yuron 23:1e4d7540715f 507
yuron 22:5682246f9409 508 if(back_limit == 3) {
yuron 25:ce789ea15628 509 phase = 24;
yuron 22:5682246f9409 510 }
yuron 22:5682246f9409 511 else if(back_limit != 3){
yuron 22:5682246f9409 512 true_migimae_data[0] = 0x50;
yuron 22:5682246f9409 513 true_migiusiro_data[0] = 0x50;
yuron 22:5682246f9409 514 true_hidarimae_data[0] = 0x50;
yuron 22:5682246f9409 515 true_hidariusiro_data[0] = 0x50;
yuron 22:5682246f9409 516 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 517 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 518 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 519 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 520 wait_us(20);
yuron 18:851f783ec516 521 }
yuron 18:851f783ec516 522 break;
yuron 20:ac4954be1fe0 523
yuron 21:89db2a19e52e 524 //シーツ装填
yuron 25:ce789ea15628 525 case 24:
yuron 24:d12bc20c01c2 526 YELLOW_LED = 1;
yuron 19:f17d2e585973 527 if(start_switch == 1) {
yuron 21:89db2a19e52e 528 wheel_reset();
yuron 25:ce789ea15628 529 phase = 25;
yuron 19:f17d2e585973 530 } else {
yuron 19:f17d2e585973 531 stop();
yuron 19:f17d2e585973 532 }
yuron 19:f17d2e585973 533 break;
yuron 20:ac4954be1fe0 534
yuron 19:f17d2e585973 535 //竿のラインまで前進
yuron 23:1e4d7540715f 536 case 25:
yuron 23:1e4d7540715f 537 counter.reset();
yuron 25:ce789ea15628 538 front(21200);
yuron 25:ce789ea15628 539 if((y_pulse1 > 21200) || (y_pulse2 > 21200)) {
yuron 23:1e4d7540715f 540 phase = 26;
yuron 18:851f783ec516 541 }
yuron 18:851f783ec516 542 break;
yuron 20:ac4954be1fe0 543
yuron 19:f17d2e585973 544 //1秒停止
yuron 23:1e4d7540715f 545 case 26:
yuron 18:851f783ec516 546 stop();
yuron 18:851f783ec516 547 counter.start();
yuron 18:851f783ec516 548 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 549 phase = 27;
yuron 19:f17d2e585973 550 wheel_reset();
yuron 19:f17d2e585973 551 }
yuron 20:ac4954be1fe0 552 break;
yuron 23:1e4d7540715f 553
yuron 25:ce789ea15628 554 //ちょっと左移動
yuron 23:1e4d7540715f 555 case 27:
yuron 19:f17d2e585973 556 counter.reset();
yuron 25:ce789ea15628 557 left(500);
yuron 25:ce789ea15628 558 if((x_pulse1 > 500) || (x_pulse2 > 500)) {
yuron 23:1e4d7540715f 559 phase = 28;
yuron 18:851f783ec516 560 }
yuron 18:851f783ec516 561 break;
yuron 23:1e4d7540715f 562
yuron 19:f17d2e585973 563 //1秒停止
yuron 23:1e4d7540715f 564 case 28:
yuron 19:f17d2e585973 565 stop();
yuron 19:f17d2e585973 566 counter.start();
yuron 19:f17d2e585973 567 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 568 phase = 29;
yuron 19:f17d2e585973 569 wheel_reset();
yuron 19:f17d2e585973 570 }
yuron 20:ac4954be1fe0 571 break;
yuron 23:1e4d7540715f 572
yuron 25:ce789ea15628 573 //90°右旋回
yuron 23:1e4d7540715f 574 case 29:
yuron 21:89db2a19e52e 575 counter.reset();
yuron 25:ce789ea15628 576 turn_right(465);
yuron 25:ce789ea15628 577 if(sum_pulse > 465) {
yuron 25:ce789ea15628 578 phase = 30;
yuron 25:ce789ea15628 579 }
yuron 25:ce789ea15628 580 break;
yuron 25:ce789ea15628 581
yuron 25:ce789ea15628 582 //1秒停止
yuron 25:ce789ea15628 583 case 30:
yuron 25:ce789ea15628 584 stop();
yuron 25:ce789ea15628 585 counter.start();
yuron 25:ce789ea15628 586 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 587 phase = 31;
yuron 25:ce789ea15628 588 wheel_reset();
yuron 25:ce789ea15628 589 }
yuron 25:ce789ea15628 590 break;
yuron 25:ce789ea15628 591
yuron 25:ce789ea15628 592 //壁に当たるまで前進
yuron 25:ce789ea15628 593 case 31:
yuron 25:ce789ea15628 594 counter.reset();
yuron 22:5682246f9409 595 if(front_limit == 3) {
yuron 25:ce789ea15628 596 phase = 32;
yuron 23:1e4d7540715f 597 }
yuron 22:5682246f9409 598 else if(front_limit != 3){
yuron 22:5682246f9409 599 true_migimae_data[0] = 0xC0;
yuron 22:5682246f9409 600 true_migiusiro_data[0] = 0xC0;
yuron 22:5682246f9409 601 true_hidarimae_data[0] = 0xC0;
yuron 22:5682246f9409 602 true_hidariusiro_data[0] = 0xC0;
yuron 22:5682246f9409 603 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 604 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 605 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 606 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 607 wait_us(20);
yuron 22:5682246f9409 608 }
yuron 22:5682246f9409 609 break;
yuron 23:1e4d7540715f 610
yuron 22:5682246f9409 611 //1秒停止
yuron 23:1e4d7540715f 612 case 32:
yuron 22:5682246f9409 613 stop();
yuron 22:5682246f9409 614 counter.start();
yuron 22:5682246f9409 615 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 616 phase = 33;
yuron 22:5682246f9409 617 wheel_reset();
yuron 22:5682246f9409 618 }
yuron 22:5682246f9409 619 break;
yuron 22:5682246f9409 620
yuron 25:ce789ea15628 621 //掛けるところまで後進
yuron 23:1e4d7540715f 622 case 33:
yuron 22:5682246f9409 623 counter.reset();
yuron 25:ce789ea15628 624 back(-9500);
yuron 25:ce789ea15628 625 if((y_pulse1*-1 > 9500) || (y_pulse2*-1 > 9500)) {
yuron 23:1e4d7540715f 626 phase = 34;
yuron 25:ce789ea15628 627 counter.start();
yuron 22:5682246f9409 628 }
yuron 22:5682246f9409 629 break;
yuron 22:5682246f9409 630
yuron 22:5682246f9409 631 //1秒停止
yuron 23:1e4d7540715f 632 case 34:
yuron 22:5682246f9409 633 stop();
yuron 22:5682246f9409 634 counter.start();
yuron 22:5682246f9409 635 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 636 phase = 35;
yuron 22:5682246f9409 637 wheel_reset();
yuron 22:5682246f9409 638 }
yuron 22:5682246f9409 639 break;
yuron 22:5682246f9409 640
yuron 25:ce789ea15628 641 //妨害防止の右旋回
yuron 25:ce789ea15628 642 case 35:
yuron 25:ce789ea15628 643 counter.reset();
yuron 25:ce789ea15628 644 turn_right(30);
yuron 25:ce789ea15628 645 if(sum_pulse > 30) {
yuron 25:ce789ea15628 646 phase = 36;
yuron 25:ce789ea15628 647 }
yuron 25:ce789ea15628 648 break;
yuron 25:ce789ea15628 649
yuron 25:ce789ea15628 650 //1秒停止
yuron 25:ce789ea15628 651 case 36:
yuron 25:ce789ea15628 652 stop();
yuron 25:ce789ea15628 653 counter.start();
yuron 25:ce789ea15628 654 if(counter.read() > 1.0f) {
yuron 26:81346a21d301 655 phase = 37;
yuron 25:ce789ea15628 656 wheel_reset();
yuron 25:ce789ea15628 657 } else {
yuron 25:ce789ea15628 658 right_arm_data[0] = 0xFF;
yuron 26:81346a21d301 659 left_arm_data[0] = 0x00;
yuron 25:ce789ea15628 660 i2c.write(0x22, right_arm_data, 1);
yuron 25:ce789ea15628 661 i2c.write(0x24, left_arm_data, 1);
yuron 25:ce789ea15628 662 wait_us(20);
yuron 25:ce789ea15628 663 }
yuron 25:ce789ea15628 664 break;
yuron 25:ce789ea15628 665
yuron 26:81346a21d301 666 //アームアップ
yuron 25:ce789ea15628 667 case 37:
yuron 22:5682246f9409 668 counter.reset();
yuron 18:851f783ec516 669 stop();
yuron 26:81346a21d301 670 arm_up(38);
yuron 21:89db2a19e52e 671 break;
yuron 23:1e4d7540715f 672
yuron 21:89db2a19e52e 673 //カウンターリセット
yuron 26:81346a21d301 674 case 38:
yuron 19:f17d2e585973 675 counter.reset();
yuron 26:81346a21d301 676 phase = 39;
yuron 25:ce789ea15628 677 break;
yuron 25:ce789ea15628 678
yuron 19:f17d2e585973 679 //シーツを掛ける
yuron 26:81346a21d301 680 case 39:
yuron 19:f17d2e585973 681 counter.start();
yuron 23:1e4d7540715f 682
yuron 21:89db2a19e52e 683 //1秒間ファン送風
yuron 25:ce789ea15628 684 if(counter.read() <= 1.0f) {
yuron 19:f17d2e585973 685 fan_data[0] = 0xFF;
yuron 19:f17d2e585973 686 i2c.write(0x26, fan_data, 1);
yuron 21:89db2a19e52e 687 i2c.write(0x28, fan_data, 1);
yuron 19:f17d2e585973 688 servo_data[0] = 0x04;
yuron 19:f17d2e585973 689 i2c.write(0x30, servo_data, 1);
yuron 19:f17d2e585973 690 }
yuron 25:ce789ea15628 691 //1~2.75秒の間でサーボを開放
yuron 25:ce789ea15628 692 else if((counter.read() > 1.0f) && (counter.read() <= 2.75f)) {
yuron 19:f17d2e585973 693 fan_data[0] = 0xFF;
yuron 19:f17d2e585973 694 i2c.write(0x26, fan_data, 1);
yuron 21:89db2a19e52e 695 i2c.write(0x28, fan_data, 1);
yuron 19:f17d2e585973 696 servo_data[0] = 0x03;
yuron 19:f17d2e585973 697 i2c.write(0x30, servo_data, 1);
yuron 19:f17d2e585973 698 }
yuron 25:ce789ea15628 699 //2.75秒過ぎたら終わり
yuron 25:ce789ea15628 700 else if(counter.read() > 2.75f) {
yuron 19:f17d2e585973 701 fan_data[0] = 0x80;
yuron 19:f17d2e585973 702 i2c.write(0x26, fan_data, 1);
yuron 21:89db2a19e52e 703 i2c.write(0x28, fan_data, 1);
yuron 19:f17d2e585973 704 servo_data[0] = 0x04;
yuron 19:f17d2e585973 705 i2c.write(0x30, servo_data, 1);
yuron 25:ce789ea15628 706 phase = FINAL_PHASE;
yuron 19:f17d2e585973 707 }
yuron 19:f17d2e585973 708 break;
yuron 20:ac4954be1fe0 709
yuron 19:f17d2e585973 710 //終了っ！(守衛さん風)
yuron 25:ce789ea15628 711 case FINAL_PHASE:
yuron 18:851f783ec516 712 default:
yuron 19:f17d2e585973 713 //駆動系統OFF
yuron 22:5682246f9409 714 all_stop();
yuron 18:851f783ec516 715 break;
yuron 18:851f783ec516 716 }
yuron 16:05b26003da50 717 }
yuron 21:89db2a19e52e 718
yuron 21:89db2a19e52e 719 //REDゾーン
yuron 21:89db2a19e52e 720 else if(zone == RED) {
yuron 21:89db2a19e52e 721 GREEN_LED = 0;
yuron 21:89db2a19e52e 722 RED_LED = 1;
yuron 22:5682246f9409 723
yuron 22:5682246f9409 724 switch(phase) {
yuron 22:5682246f9409 725
yuron 22:5682246f9409 726 //スタート位置へセット
yuron 22:5682246f9409 727 case 0:
yuron 25:ce789ea15628 728 //スタートスイッチが押されたか
yuron 25:ce789ea15628 729 if(start_switch == 1) {
yuron 25:ce789ea15628 730 wheel_reset();
yuron 25:ce789ea15628 731 phase = 1;
yuron 25:ce789ea15628 732 }
yuron 25:ce789ea15628 733
yuron 22:5682246f9409 734 //リミットが洗濯物台に触れているか
yuron 22:5682246f9409 735 if(right_limit == 3) {
yuron 22:5682246f9409 736 USR_LED1 = 1;
yuron 25:ce789ea15628 737
yuron 22:5682246f9409 738 } else {
yuron 22:5682246f9409 739 USR_LED1 = 0;
yuron 22:5682246f9409 740 }
yuron 22:5682246f9409 741 break;
yuron 22:5682246f9409 742
yuron 23:1e4d7540715f 743 //回収アームを伸ばす
yuron 22:5682246f9409 744 case 1:
yuron 23:1e4d7540715f 745 counter.reset();
yuron 23:1e4d7540715f 746 kaisyu_nobasu(arm_pulse, 2);
yuron 23:1e4d7540715f 747 //サーボを開いておく
yuron 22:5682246f9409 748 servo_data[0] = 0x03;
yuron 22:5682246f9409 749 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 750 break;
yuron 22:5682246f9409 751
yuron 26:81346a21d301 752 //1.0秒停止
yuron 22:5682246f9409 753 case 2:
yuron 22:5682246f9409 754 stop();
yuron 22:5682246f9409 755 servo_data[0] = 0x04;
yuron 22:5682246f9409 756 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 757 counter.start();
yuron 26:81346a21d301 758 if(counter.read() > 1.0f) {
yuron 22:5682246f9409 759 phase = 3;
yuron 22:5682246f9409 760 wheel_reset();
yuron 22:5682246f9409 761 }
yuron 22:5682246f9409 762 break;
yuron 22:5682246f9409 763
yuron 23:1e4d7540715f 764 //ちょっと前進
yuron 22:5682246f9409 765 case 3:
yuron 22:5682246f9409 766 counter.reset();
yuron 23:1e4d7540715f 767 front(800);
yuron 23:1e4d7540715f 768 if((y_pulse1 > 800) || (y_pulse2 > 800)) {
yuron 25:ce789ea15628 769 phase = 4;
yuron 22:5682246f9409 770 }
yuron 22:5682246f9409 771 break;
yuron 25:ce789ea15628 772
yuron 25:ce789ea15628 773 //0.5秒停止
yuron 22:5682246f9409 774 case 4:
yuron 22:5682246f9409 775 stop();
yuron 22:5682246f9409 776 counter.start();
yuron 24:d12bc20c01c2 777 if(counter.read() > 0.5f) {
yuron 22:5682246f9409 778 phase = 5;
yuron 22:5682246f9409 779 wheel_reset();
yuron 22:5682246f9409 780 }
yuron 22:5682246f9409 781 break;
yuron 25:ce789ea15628 782
yuron 23:1e4d7540715f 783 //回収アーム引っ込める
yuron 22:5682246f9409 784 case 5:
yuron 26:81346a21d301 785 USR_LED3 = 1;
yuron 25:ce789ea15628 786 kaisyu_hiku(arm_pulse, 6);
yuron 26:81346a21d301 787 /*
yuron 26:81346a21d301 788 if(kaisyu_mae_limit == 0) {
yuron 26:81346a21d301 789 arm_motor[0] = 0x4C;
yuron 26:81346a21d301 790 }
yuron 26:81346a21d301 791 else if(kaisyu_mae_limit == 1) {
yuron 26:81346a21d301 792 arm_motor[0] = 0x80;
yuron 26:81346a21d301 793 phase = 6;
yuron 26:81346a21d301 794 }
yuron 26:81346a21d301 795 */
yuron 25:ce789ea15628 796 break;
yuron 25:ce789ea15628 797
yuron 23:1e4d7540715f 798 //左移動
yuron 25:ce789ea15628 799 case 6:
yuron 23:1e4d7540715f 800 counter.reset();
yuron 23:1e4d7540715f 801 left(11500);
yuron 23:1e4d7540715f 802 if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
yuron 25:ce789ea15628 803 phase = 7;
yuron 23:1e4d7540715f 804 }
yuron 23:1e4d7540715f 805 break;
yuron 23:1e4d7540715f 806
yuron 23:1e4d7540715f 807 //1秒停止
yuron 25:ce789ea15628 808 case 7:
yuron 23:1e4d7540715f 809 stop();
yuron 23:1e4d7540715f 810 counter.start();
yuron 23:1e4d7540715f 811 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 812 phase = 8;
yuron 23:1e4d7540715f 813 wheel_reset();
yuron 23:1e4d7540715f 814 }
yuron 23:1e4d7540715f 815 break;
yuron 23:1e4d7540715f 816
yuron 23:1e4d7540715f 817 //右旋回(180°)
yuron 25:ce789ea15628 818 case 8:
yuron 23:1e4d7540715f 819 counter.reset();
yuron 26:81346a21d301 820 turn_right(940);
yuron 26:81346a21d301 821 if(sum_pulse > 940) {
yuron 25:ce789ea15628 822 phase = 9;
yuron 23:1e4d7540715f 823 }
yuron 23:1e4d7540715f 824 break;
yuron 23:1e4d7540715f 825
yuron 25:ce789ea15628 826 //0.5秒停止
yuron 25:ce789ea15628 827 case 9:
yuron 23:1e4d7540715f 828 stop();
yuron 23:1e4d7540715f 829 counter.start();
yuron 24:d12bc20c01c2 830 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 831 phase = 10;
yuron 23:1e4d7540715f 832 wheel_reset();
yuron 23:1e4d7540715f 833 }
yuron 25:ce789ea15628 834 break;
yuron 25:ce789ea15628 835
yuron 23:1e4d7540715f 836 //壁に当たるまで前進
yuron 25:ce789ea15628 837 case 10:
yuron 23:1e4d7540715f 838 counter.reset();
yuron 25:ce789ea15628 839
yuron 23:1e4d7540715f 840 if(front_limit == 3) {
yuron 25:ce789ea15628 841 phase = 11;
yuron 23:1e4d7540715f 842 }
yuron 23:1e4d7540715f 843 else if(front_limit != 3){
yuron 23:1e4d7540715f 844 true_migimae_data[0] = 0xC0;
yuron 23:1e4d7540715f 845 true_migiusiro_data[0] = 0xC0;
yuron 23:1e4d7540715f 846 true_hidarimae_data[0] = 0xC0;
yuron 23:1e4d7540715f 847 true_hidariusiro_data[0] = 0xC0;
yuron 23:1e4d7540715f 848 i2c.write(0x10, true_migimae_data, 1, false);
yuron 23:1e4d7540715f 849 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 23:1e4d7540715f 850 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 23:1e4d7540715f 851 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 852 wait_us(20);
yuron 23:1e4d7540715f 853 }
yuron 23:1e4d7540715f 854 break;
yuron 25:ce789ea15628 855
yuron 25:ce789ea15628 856 //0.5秒停止
yuron 25:ce789ea15628 857 case 11:
yuron 23:1e4d7540715f 858 stop();
yuron 23:1e4d7540715f 859 counter.start();
yuron 24:d12bc20c01c2 860 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 861 phase = 12;
yuron 23:1e4d7540715f 862 wheel_reset();
yuron 23:1e4d7540715f 863 }
yuron 22:5682246f9409 864 break;
yuron 22:5682246f9409 865
yuron 22:5682246f9409 866 //壁に当たるまで右移動
yuron 25:ce789ea15628 867 case 12:
yuron 25:ce789ea15628 868 counter.reset();
yuron 25:ce789ea15628 869
yuron 25:ce789ea15628 870 if(right_limit == 3) {
yuron 25:ce789ea15628 871 phase = 13;
yuron 25:ce789ea15628 872 }
yuron 25:ce789ea15628 873 else if(right_limit != 3) {
yuron 25:ce789ea15628 874 true_migimae_data[0] = 0x40;
yuron 25:ce789ea15628 875 true_migiusiro_data[0] = 0xBF;
yuron 25:ce789ea15628 876 true_hidarimae_data[0] = 0xBF;
yuron 25:ce789ea15628 877 true_hidariusiro_data[0] = 0x40;
yuron 25:ce789ea15628 878 i2c.write(0x10, true_migimae_data, 1, false);
yuron 25:ce789ea15628 879 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 25:ce789ea15628 880 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 25:ce789ea15628 881 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 25:ce789ea15628 882 wait_us(20);
yuron 25:ce789ea15628 883 }
yuron 25:ce789ea15628 884 break;
yuron 25:ce789ea15628 885
yuron 25:ce789ea15628 886 //0.5秒停止
yuron 23:1e4d7540715f 887 case 13:
yuron 25:ce789ea15628 888 stop();
yuron 25:ce789ea15628 889 counter.start();
yuron 25:ce789ea15628 890 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 891 phase = 14;
yuron 25:ce789ea15628 892 wheel_reset();
yuron 25:ce789ea15628 893 }
yuron 25:ce789ea15628 894 break;
yuron 25:ce789ea15628 895
yuron 25:ce789ea15628 896 //排出
yuron 25:ce789ea15628 897 case 14:
yuron 25:ce789ea15628 898 counter.reset();
yuron 25:ce789ea15628 899 tyokudo(arm_enc.getPulses(), 15);
yuron 25:ce789ea15628 900 break;
yuron 25:ce789ea15628 901
yuron 25:ce789ea15628 902 //1秒停止
yuron 25:ce789ea15628 903 case 15:
yuron 25:ce789ea15628 904 stop();
yuron 25:ce789ea15628 905 counter.start();
yuron 25:ce789ea15628 906 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 907 phase = 16;
yuron 25:ce789ea15628 908 wheel_reset();
yuron 25:ce789ea15628 909 }
yuron 25:ce789ea15628 910 break;
yuron 25:ce789ea15628 911
yuron 25:ce789ea15628 912 //後進
yuron 25:ce789ea15628 913 case 16:
yuron 25:ce789ea15628 914 counter.reset();
yuron 25:ce789ea15628 915 back(-5000);
yuron 25:ce789ea15628 916 if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
yuron 25:ce789ea15628 917 phase = 17;
yuron 25:ce789ea15628 918 }
yuron 25:ce789ea15628 919 break;
yuron 25:ce789ea15628 920
yuron 25:ce789ea15628 921 //0.5秒停止
yuron 25:ce789ea15628 922 case 17:
yuron 25:ce789ea15628 923 stop();
yuron 25:ce789ea15628 924 counter.start();
yuron 25:ce789ea15628 925 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 926 phase = 18;
yuron 25:ce789ea15628 927 wheel_reset();
yuron 25:ce789ea15628 928 }
yuron 25:ce789ea15628 929 break;
yuron 25:ce789ea15628 930
yuron 25:ce789ea15628 931 //壁に当たるまで右移動
yuron 25:ce789ea15628 932 case 18:
yuron 25:ce789ea15628 933 counter.reset();
yuron 25:ce789ea15628 934
yuron 22:5682246f9409 935 if(right_limit == 3) {
yuron 25:ce789ea15628 936 phase = 19;
yuron 22:5682246f9409 937 }
yuron 22:5682246f9409 938 else if(right_limit != 3) {
yuron 22:5682246f9409 939 true_migimae_data[0] = 0x40;
yuron 22:5682246f9409 940 true_migiusiro_data[0] = 0xBF;
yuron 22:5682246f9409 941 true_hidarimae_data[0] = 0xBF;
yuron 22:5682246f9409 942 true_hidariusiro_data[0] = 0x40;
yuron 22:5682246f9409 943 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 944 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 945 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 946 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 25:ce789ea15628 947 wait_us(20);
yuron 22:5682246f9409 948 }
yuron 22:5682246f9409 949 break;
yuron 22:5682246f9409 950
yuron 25:ce789ea15628 951 //0.5秒停止
yuron 25:ce789ea15628 952 case 19:
yuron 22:5682246f9409 953 stop();
yuron 22:5682246f9409 954 counter.start();
yuron 25:ce789ea15628 955 if(counter.read() > 0.5f) {
yuron 25:ce789ea15628 956 phase = 20;
yuron 22:5682246f9409 957 wheel_reset();
yuron 22:5682246f9409 958 }
yuron 22:5682246f9409 959 break;
yuron 22:5682246f9409 960
yuron 25:ce789ea15628 961 //壁に当たるまで前進
yuron 25:ce789ea15628 962 case 20:
yuron 22:5682246f9409 963 counter.reset();
yuron 25:ce789ea15628 964
yuron 25:ce789ea15628 965 if(front_limit == 3) {
yuron 23:1e4d7540715f 966 phase = 21;
yuron 22:5682246f9409 967 }
yuron 22:5682246f9409 968 else if(front_limit != 3){
yuron 22:5682246f9409 969 true_migimae_data[0] = 0xC0;
yuron 22:5682246f9409 970 true_migiusiro_data[0] = 0xC0;
yuron 22:5682246f9409 971 true_hidarimae_data[0] = 0xC0;
yuron 22:5682246f9409 972 true_hidariusiro_data[0] = 0xC0;
yuron 22:5682246f9409 973 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 974 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 975 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 976 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 977 wait_us(20);
yuron 22:5682246f9409 978 }
yuron 22:5682246f9409 979 break;
yuron 22:5682246f9409 980
yuron 22:5682246f9409 981 //シーツ装填
yuron 25:ce789ea15628 982 case 21:
yuron 24:d12bc20c01c2 983 YELLOW_LED = 1;
yuron 22:5682246f9409 984 if(start_switch == 1) {
yuron 22:5682246f9409 985 wheel_reset();
yuron 25:ce789ea15628 986 phase = 22;
yuron 22:5682246f9409 987 } else {
yuron 22:5682246f9409 988 stop();
yuron 22:5682246f9409 989 }
yuron 22:5682246f9409 990 break;
yuron 22:5682246f9409 991
yuron 22:5682246f9409 992 //竿のラインまで後進
yuron 25:ce789ea15628 993 case 22:
yuron 22:5682246f9409 994 counter.reset();
yuron 23:1e4d7540715f 995 back(-20500);
yuron 23:1e4d7540715f 996 if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
yuron 25:ce789ea15628 997 phase = 23;
yuron 25:ce789ea15628 998 }
yuron 25:ce789ea15628 999 break;
yuron 25:ce789ea15628 1000
yuron 25:ce789ea15628 1001 //1秒停止
yuron 25:ce789ea15628 1002 case 23:
yuron 25:ce789ea15628 1003 stop();
yuron 25:ce789ea15628 1004 counter.start();
yuron 25:ce789ea15628 1005 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1006 phase = 24;
yuron 25:ce789ea15628 1007 wheel_reset();
yuron 25:ce789ea15628 1008 }
yuron 25:ce789ea15628 1009 break;
yuron 25:ce789ea15628 1010
yuron 25:ce789ea15628 1011 //ちょっと左移動
yuron 25:ce789ea15628 1012 case 24:
yuron 25:ce789ea15628 1013 counter.reset();
yuron 25:ce789ea15628 1014 left(500);
yuron 25:ce789ea15628 1015 if((x_pulse1 > 500) || (x_pulse2 > 500)) {
yuron 25:ce789ea15628 1016 phase = 25;
yuron 22:5682246f9409 1017 }
yuron 22:5682246f9409 1018 break;
yuron 22:5682246f9409 1019
yuron 22:5682246f9409 1020 //1秒停止
yuron 25:ce789ea15628 1021 case 25:
yuron 22:5682246f9409 1022 stop();
yuron 22:5682246f9409 1023 counter.start();
yuron 22:5682246f9409 1024 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1025 phase = 26;
yuron 22:5682246f9409 1026 wheel_reset();
yuron 22:5682246f9409 1027 }
yuron 22:5682246f9409 1028 break;
yuron 25:ce789ea15628 1029
yuron 22:5682246f9409 1030 //90°左旋回
yuron 25:ce789ea15628 1031 case 26:
yuron 22:5682246f9409 1032 counter.reset();
yuron 24:d12bc20c01c2 1033 turn_left(500);
yuron 24:d12bc20c01c2 1034 if(sum_pulse > 500) {
yuron 25:ce789ea15628 1035 phase = 27;
yuron 22:5682246f9409 1036 }
yuron 22:5682246f9409 1037 break;
yuron 25:ce789ea15628 1038
yuron 22:5682246f9409 1039 //1秒停止
yuron 25:ce789ea15628 1040 case 27:
yuron 22:5682246f9409 1041 stop();
yuron 22:5682246f9409 1042 counter.start();
yuron 22:5682246f9409 1043 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 1044 phase = 28;
yuron 22:5682246f9409 1045 wheel_reset();
yuron 22:5682246f9409 1046 }
yuron 22:5682246f9409 1047 break;
yuron 25:ce789ea15628 1048
yuron 23:1e4d7540715f 1049 //壁に当たるまで後進
yuron 25:ce789ea15628 1050 case 28:
yuron 22:5682246f9409 1051 counter.reset();
yuron 25:ce789ea15628 1052
yuron 22:5682246f9409 1053 if(back_limit == 3) {
yuron 25:ce789ea15628 1054 phase = 29;
yuron 25:ce789ea15628 1055 }
yuron 22:5682246f9409 1056 else if(back_limit != 3){
yuron 22:5682246f9409 1057 true_migimae_data[0] = 0x50;
yuron 22:5682246f9409 1058 true_migiusiro_data[0] = 0x50;
yuron 22:5682246f9409 1059 true_hidarimae_data[0] = 0x50;
yuron 22:5682246f9409 1060 true_hidariusiro_data[0] = 0x50;
yuron 22:5682246f9409 1061 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 1062 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 1063 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 1064 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 25:ce789ea15628 1065 wait_us(20);
yuron 22:5682246f9409 1066 }
yuron 22:5682246f9409 1067 break;
yuron 25:ce789ea15628 1068
yuron 22:5682246f9409 1069 //1秒停止
yuron 25:ce789ea15628 1070 case 29:
yuron 22:5682246f9409 1071 stop();
yuron 22:5682246f9409 1072 counter.start();
yuron 22:5682246f9409 1073 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 1074 phase = 30;
yuron 22:5682246f9409 1075 wheel_reset();
yuron 22:5682246f9409 1076 }
yuron 22:5682246f9409 1077 break;
yuron 25:ce789ea15628 1078
yuron 22:5682246f9409 1079 //掛けるところまで前進
yuron 25:ce789ea15628 1080 case 30:
yuron 22:5682246f9409 1081 counter.reset();
yuron 26:81346a21d301 1082 front(9500);
yuron 26:81346a21d301 1083 if((y_pulse1 > 9500) || (y_pulse2 > 9500)) {
yuron 25:ce789ea15628 1084 phase = 31;
yuron 22:5682246f9409 1085 counter.start();
yuron 22:5682246f9409 1086 }
yuron 22:5682246f9409 1087 break;
yuron 25:ce789ea15628 1088
yuron 22:5682246f9409 1089 //1秒停止
yuron 25:ce789ea15628 1090 case 31:
yuron 22:5682246f9409 1091 stop();
yuron 22:5682246f9409 1092 counter.start();
yuron 22:5682246f9409 1093 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 1094 phase = 32;
yuron 22:5682246f9409 1095 wheel_reset();
yuron 22:5682246f9409 1096 }
yuron 22:5682246f9409 1097 break;
yuron 22:5682246f9409 1098
yuron 22:5682246f9409 1099 //妨害防止の左旋回
yuron 25:ce789ea15628 1100 case 32:
yuron 22:5682246f9409 1101 counter.reset();
yuron 22:5682246f9409 1102 turn_left(30);
yuron 22:5682246f9409 1103 if(sum_pulse > 30) {
yuron 25:ce789ea15628 1104 phase = 33;
yuron 22:5682246f9409 1105 }
yuron 22:5682246f9409 1106 break;
yuron 22:5682246f9409 1107
yuron 22:5682246f9409 1108 //1秒停止
yuron 25:ce789ea15628 1109 case 33:
yuron 22:5682246f9409 1110 stop();
yuron 22:5682246f9409 1111 counter.start();
yuron 22:5682246f9409 1112 if(counter.read() > 1.0f) {
yuron 25:ce789ea15628 1113 phase = 34;
yuron 22:5682246f9409 1114 wheel_reset();
yuron 26:81346a21d301 1115 } else {
yuron 26:81346a21d301 1116 right_arm_data[0] = 0xFF;
yuron 26:81346a21d301 1117 left_arm_data[0] = 0x00;
yuron 26:81346a21d301 1118 i2c.write(0x22, right_arm_data, 1);
yuron 26:81346a21d301 1119 i2c.write(0x24, left_arm_data, 1);
yuron 26:81346a21d301 1120 wait_us(20);
yuron 22:5682246f9409 1121 }
yuron 22:5682246f9409 1122 break;
yuron 22:5682246f9409 1123
yuron 22:5682246f9409 1124 //カウンターリセット
yuron 25:ce789ea15628 1125 case 34:
yuron 22:5682246f9409 1126 counter.reset();
yuron 22:5682246f9409 1127 counter.start();
yuron 25:ce789ea15628 1128 phase = 35;
yuron 22:5682246f9409 1129 break;
yuron 25:ce789ea15628 1130
yuron 22:5682246f9409 1131 //アームアップ
yuron 25:ce789ea15628 1132 case 35:
yuron 26:81346a21d301 1133 counter.reset();
yuron 22:5682246f9409 1134 stop();
yuron 26:81346a21d301 1135 arm_up(36);
yuron 22:5682246f9409 1136 break;
yuron 25:ce789ea15628 1137
yuron 25:ce789ea15628 1138 //カウンターリセット
yuron 25:ce789ea15628 1139 case 36:
yuron 25:ce789ea15628 1140 counter.reset();
yuron 25:ce789ea15628 1141 phase = 37;
yuron 25:ce789ea15628 1142 break;
yuron 25:ce789ea15628 1143
yuron 25:ce789ea15628 1144 //シーツを掛ける
yuron 26:81346a21d301 1145 case 37:
yuron 25:ce789ea15628 1146 counter.start();
yuron 25:ce789ea15628 1147
yuron 22:5682246f9409 1148 //1秒間ファン送風
yuron 24:d12bc20c01c2 1149 if(counter.read() <= 1.0f) {
yuron 22:5682246f9409 1150 fan_data[0] = 0xFF;
yuron 22:5682246f9409 1151 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1152 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1153 servo_data[0] = 0x04;
yuron 22:5682246f9409 1154 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 1155 }
yuron 25:ce789ea15628 1156 //1~2.75秒の間でサーボを開放
yuron 25:ce789ea15628 1157 else if((counter.read() > 1.0f) && (counter.read() <= 2.75f)) {
yuron 22:5682246f9409 1158 fan_data[0] = 0xFF;
yuron 22:5682246f9409 1159 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1160 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1161 servo_data[0] = 0x03;
yuron 22:5682246f9409 1162 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 1163 }
yuron 25:ce789ea15628 1164 //2.75秒過ぎたら終わり
yuron 25:ce789ea15628 1165 else if(counter.read() > 2.75f) {
yuron 22:5682246f9409 1166 fan_data[0] = 0x80;
yuron 22:5682246f9409 1167 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1168 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1169 servo_data[0] = 0x04;
yuron 22:5682246f9409 1170 i2c.write(0x30, servo_data, 1);
yuron 25:ce789ea15628 1171 phase = FINAL_PHASE;
yuron 22:5682246f9409 1172 }
yuron 22:5682246f9409 1173 break;
yuron 22:5682246f9409 1174
yuron 22:5682246f9409 1175 //終了っ！(守衛さん風)
yuron 25:ce789ea15628 1176 case FINAL_PHASE:
yuron 22:5682246f9409 1177 default:
yuron 22:5682246f9409 1178 //駆動系統OFF
yuron 22:5682246f9409 1179 all_stop();
yuron 22:5682246f9409 1180 break;
yuron 22:5682246f9409 1181 }
yuron 21:89db2a19e52e 1182 }
yuron 16:05b26003da50 1183 }
yuron 16:05b26003da50 1184 }
yuron 17:de3bc1999ae7 1185
yuron 14:ab89b6cd9719 1186 void init(void) {
yuron 10:b672aa81b226 1187
yuron 14:ab89b6cd9719 1188 //通信ボーレートの設定
yuron 16:05b26003da50 1189 pc.baud(460800);
yuron 20:ac4954be1fe0 1190
yuron 18:851f783ec516 1191 limit_serial.baud(115200);
yuron 20:ac4954be1fe0 1192
yuron 16:05b26003da50 1193 start_switch.mode(PullUp);
yuron 21:89db2a19e52e 1194 zone_switch.mode(PullDown);
yuron 24:d12bc20c01c2 1195
yuron 24:d12bc20c01c2 1196 YELLOW_LED = 0;
yuron 26:81346a21d301 1197 USR_LED3 = 0; USR_LED4 = 0;
yuron 20:ac4954be1fe0 1198
yuron 17:de3bc1999ae7 1199 //非常停止関連
yuron 17:de3bc1999ae7 1200 pic.baud(19200);
yuron 17:de3bc1999ae7 1201 pic.format(8, Serial::None, 1);
yuron 17:de3bc1999ae7 1202 pic.attach(get, Serial::RxIrq);
yuron 20:ac4954be1fe0 1203
yuron 22:5682246f9409 1204 x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; sum_pulse = 0;
yuron 14:ab89b6cd9719 1205 migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 1206 true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80;
yuron 19:f17d2e585973 1207 fan_data[0] = 0x80;
yuron 19:f17d2e585973 1208 servo_data[0] = 0x80;
yuron 21:89db2a19e52e 1209 arm_motor[0] = 0x80; drop_motor[0] = 0x80;
yuron 20:ac4954be1fe0 1210 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
yuron 14:ab89b6cd9719 1211 }
yuron 5:167327a82430 1212
yuron 14:ab89b6cd9719 1213 void init_send(void) {
yuron 20:ac4954be1fe0 1214
yuron 14:ab89b6cd9719 1215 init_send_data[0] = 0x80;
yuron 14:ab89b6cd9719 1216 i2c.write(0x10, init_send_data, 1);
yuron 14:ab89b6cd9719 1217 i2c.write(0x12, init_send_data, 1);
yuron 14:ab89b6cd9719 1218 i2c.write(0x14, init_send_data, 1);
yuron 14:ab89b6cd9719 1219 i2c.write(0x16, init_send_data, 1);
yuron 20:ac4954be1fe0 1220 i2c.write(0x18, init_send_data, 1);
yuron 20:ac4954be1fe0 1221 i2c.write(0x20, init_send_data, 1);
yuron 20:ac4954be1fe0 1222 i2c.write(0x22, init_send_data, 1);
yuron 20:ac4954be1fe0 1223 i2c.write(0x24, init_send_data, 1);
yuron 20:ac4954be1fe0 1224 i2c.write(0x30, init_send_data, 1);
yuron 14:ab89b6cd9719 1225 wait(0.1);
yuron 14:ab89b6cd9719 1226 }
yuron 0:f73c1b076ae4 1227
yuron 17:de3bc1999ae7 1228 void get(void) {
yuron 20:ac4954be1fe0 1229
yuron 20:ac4954be1fe0 1230 baff = pic.getc();
yuron 20:ac4954be1fe0 1231
yuron 17:de3bc1999ae7 1232 for(; flug; flug--)
yuron 17:de3bc1999ae7 1233 RDATA = baff;
yuron 20:ac4954be1fe0 1234
yuron 17:de3bc1999ae7 1235 if(baff == ':')
yuron 17:de3bc1999ae7 1236 flug = 1;
yuron 17:de3bc1999ae7 1237 }
yuron 17:de3bc1999ae7 1238
yuron 14:ab89b6cd9719 1239 void get_pulses(void) {
yuron 20:ac4954be1fe0 1240
yuron 14:ab89b6cd9719 1241 x_pulse1 = wheel_x1.getPulses();
yuron 14:ab89b6cd9719 1242 x_pulse2 = wheel_x2.getPulses();
yuron 14:ab89b6cd9719 1243 y_pulse1 = wheel_y1.getPulses();
yuron 14:ab89b6cd9719 1244 y_pulse2 = wheel_y2.getPulses();
yuron 22:5682246f9409 1245 sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4;
yuron 23:1e4d7540715f 1246 arm_pulse = arm_enc.getPulses();
yuron 14:ab89b6cd9719 1247 }
yuron 0:f73c1b076ae4 1248
yuron 14:ab89b6cd9719 1249 void print_pulses(void) {
yuron 26:81346a21d301 1250 //pc.printf("p: %d, kp: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
yuron 23:1e4d7540715f 1251 //pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
yuron 23:1e4d7540715f 1252 //pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse);
yuron 25:ce789ea15628 1253 //pc.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", front_limit, back_limit, right_limit, kaisyu_mae_limit, kaisyu_usiro_limit,
yuron 25:ce789ea15628 1254 //tyokudo_mae_limit, tyokudo_usiro_limit, right_arm_upper_limit, left_arm_upper_limit);
yuron 21:89db2a19e52e 1255 //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
yuron 19:f17d2e585973 1256 //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
yuron 18:851f783ec516 1257 //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
yuron 21:89db2a19e52e 1258 //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0], phase);
yuron 14:ab89b6cd9719 1259 }
yuron 4:df334779a69e 1260
yuron 17:de3bc1999ae7 1261 void get_emergency(void) {
yuron 20:ac4954be1fe0 1262
yuron 17:de3bc1999ae7 1263 if(RDATA == '1') {
yuron 17:de3bc1999ae7 1264 myled = 1;
yuron 17:de3bc1999ae7 1265 emergency = 1;
yuron 17:de3bc1999ae7 1266 }
yuron 17:de3bc1999ae7 1267 else if(RDATA == '9'){
yuron 17:de3bc1999ae7 1268 myled = 0.2;
yuron 17:de3bc1999ae7 1269 emergency = 0;
yuron 24:d12bc20c01c2 1270 /*
yuron 23:1e4d7540715f 1271 //終了phaseで駆動系統OFF
yuron 25:ce789ea15628 1272 if(phase == FINAL_PHASE) {
yuron 23:1e4d7540715f 1273 emergency = 1;
yuron 25:ce789ea15628 1274 } else {
yuron 23:1e4d7540715f 1275 emergency = 0;
yuron 23:1e4d7540715f 1276 }
yuron 24:d12bc20c01c2 1277 */
yuron 17:de3bc1999ae7 1278 }
yuron 17:de3bc1999ae7 1279 }
yuron 17:de3bc1999ae7 1280
yuron 18:851f783ec516 1281 void read_limit(void) {
yuron 20:ac4954be1fe0 1282
yuron 18:851f783ec516 1283 limit_data = limit_serial.getc();
yuron 20:ac4954be1fe0 1284
yuron 19:f17d2e585973 1285 //上位1bitが1ならば下のリミットのデータだと判断
yuron 19:f17d2e585973 1286 if((limit_data & 0b10000000) == 0b10000000) {
yuron 19:f17d2e585973 1287 lower_limit_data = limit_data;
yuron 20:ac4954be1fe0 1288
yuron 19:f17d2e585973 1289 //上位1bitが0ならば上のリミットのデータだと判断
yuron 19:f17d2e585973 1290 } else {
yuron 19:f17d2e585973 1291 upper_limit_data = limit_data;
yuron 19:f17d2e585973 1292 }
yuron 20:ac4954be1fe0 1293
yuron 20:ac4954be1fe0 1294 //下リミット基板からのデータのマスク処理
yuron 19:f17d2e585973 1295 masked_lower_front_limit_data = lower_limit_data & 0b00000011;
yuron 19:f17d2e585973 1296 masked_lower_back_limit_data = lower_limit_data & 0b00001100;
yuron 19:f17d2e585973 1297 masked_lower_right_limit_data = lower_limit_data & 0b00110000;
yuron 21:89db2a19e52e 1298 masked_kaisyu_mae_limit_data = lower_limit_data & 0b01000000;
yuron 20:ac4954be1fe0 1299
yuron 20:ac4954be1fe0 1300 //上リミット基板からのデータのマスク処理
yuron 21:89db2a19e52e 1301 //masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
yuron 21:89db2a19e52e 1302 masked_kaisyu_usiro_limit_data = upper_limit_data & 0b00000001;
yuron 20:ac4954be1fe0 1303 masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
yuron 20:ac4954be1fe0 1304 masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100;
yuron 20:ac4954be1fe0 1305 masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000;
yuron 20:ac4954be1fe0 1306 masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000;
yuron 20:ac4954be1fe0 1307 masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000;
yuron 20:ac4954be1fe0 1308
yuron 19:f17d2e585973 1309 //前部リミット
yuron 19:f17d2e585973 1310 switch(masked_lower_front_limit_data) {
yuron 19:f17d2e585973 1311 //両方押された
yuron 19:f17d2e585973 1312 case 0x00:
yuron 19:f17d2e585973 1313 front_limit = 3;
yuron 19:f17d2e585973 1314 break;
yuron 19:f17d2e585973 1315 //右が押された
yuron 19:f17d2e585973 1316 case 0b00000010:
yuron 19:f17d2e585973 1317 front_limit = 1;
yuron 19:f17d2e585973 1318 break;
yuron 19:f17d2e585973 1319 //左が押された
yuron 19:f17d2e585973 1320 case 0b00000001:
yuron 19:f17d2e585973 1321 front_limit = 2;
yuron 19:f17d2e585973 1322 break;
yuron 19:f17d2e585973 1323 default:
yuron 19:f17d2e585973 1324 front_limit = 0;
yuron 19:f17d2e585973 1325 break;
yuron 19:f17d2e585973 1326 }
yuron 20:ac4954be1fe0 1327
yuron 19:f17d2e585973 1328 //後部リミット
yuron 19:f17d2e585973 1329 switch(masked_lower_back_limit_data) {
yuron 19:f17d2e585973 1330 //両方押された
yuron 19:f17d2e585973 1331 case 0x00:
yuron 19:f17d2e585973 1332 back_limit = 3;
yuron 19:f17d2e585973 1333 break;
yuron 19:f17d2e585973 1334 //右が押された
yuron 19:f17d2e585973 1335 case 0b00001000:
yuron 19:f17d2e585973 1336 back_limit = 1;
yuron 19:f17d2e585973 1337 break;
yuron 19:f17d2e585973 1338 //左が押された
yuron 19:f17d2e585973 1339 case 0b00000100:
yuron 19:f17d2e585973 1340 back_limit = 2;
yuron 19:f17d2e585973 1341 break;
yuron 19:f17d2e585973 1342 default:
yuron 19:f17d2e585973 1343 back_limit = 0;
yuron 19:f17d2e585973 1344 break;
yuron 18:851f783ec516 1345 }
yuron 20:ac4954be1fe0 1346
yuron 19:f17d2e585973 1347 //右部リミット
yuron 19:f17d2e585973 1348 switch(masked_lower_right_limit_data) {
yuron 19:f17d2e585973 1349 //両方押された
yuron 19:f17d2e585973 1350 case 0x00:
yuron 19:f17d2e585973 1351 right_limit = 3;
yuron 19:f17d2e585973 1352 break;
yuron 19:f17d2e585973 1353 //右が押された
yuron 19:f17d2e585973 1354 case 0b00100000:
yuron 19:f17d2e585973 1355 right_limit = 1;
yuron 19:f17d2e585973 1356 break;
yuron 19:f17d2e585973 1357 //左が押された
yuron 19:f17d2e585973 1358 case 0b00010000:
yuron 19:f17d2e585973 1359 right_limit = 2;
yuron 19:f17d2e585973 1360 break;
yuron 19:f17d2e585973 1361 default:
yuron 19:f17d2e585973 1362 right_limit = 0;
yuron 19:f17d2e585973 1363 break;
yuron 19:f17d2e585973 1364 }
yuron 20:ac4954be1fe0 1365
yuron 20:ac4954be1fe0 1366 //回収機構リミット
yuron 21:89db2a19e52e 1367 switch(masked_kaisyu_mae_limit_data) {
yuron 20:ac4954be1fe0 1368 //押された
yuron 20:ac4954be1fe0 1369 case 0b00000000:
yuron 21:89db2a19e52e 1370 kaisyu_mae_limit = 1;
yuron 20:ac4954be1fe0 1371 break;
yuron 20:ac4954be1fe0 1372 case 0b01000000:
yuron 21:89db2a19e52e 1373 kaisyu_mae_limit = 0;
yuron 20:ac4954be1fe0 1374 break;
yuron 20:ac4954be1fe0 1375 default:
yuron 21:89db2a19e52e 1376 kaisyu_mae_limit = 0;
yuron 20:ac4954be1fe0 1377 break;
yuron 19:f17d2e585973 1378 }
yuron 20:ac4954be1fe0 1379
yuron 20:ac4954be1fe0 1380 //右腕下部リミット
yuron 21:89db2a19e52e 1381 /*
yuron 20:ac4954be1fe0 1382 switch(masked_right_arm_lower_limit_data) {
yuron 20:ac4954be1fe0 1383 //押された
yuron 20:ac4954be1fe0 1384 case 0b00000000:
yuron 20:ac4954be1fe0 1385 right_arm_lower_limit = 1;
yuron 20:ac4954be1fe0 1386 break;
yuron 20:ac4954be1fe0 1387 case 0b00000001:
yuron 20:ac4954be1fe0 1388 right_arm_lower_limit = 0;
yuron 20:ac4954be1fe0 1389 break;
yuron 20:ac4954be1fe0 1390 default:
yuron 20:ac4954be1fe0 1391 right_arm_lower_limit = 0;
yuron 20:ac4954be1fe0 1392 break;
yuron 18:851f783ec516 1393 }
yuron 21:89db2a19e52e 1394 */
yuron 21:89db2a19e52e 1395
yuron 21:89db2a19e52e 1396 //回収後リミット
yuron 21:89db2a19e52e 1397 switch(masked_kaisyu_usiro_limit_data) {
yuron 21:89db2a19e52e 1398 case 0b00000000:
yuron 21:89db2a19e52e 1399 kaisyu_usiro_limit = 1;
yuron 21:89db2a19e52e 1400 break;
yuron 21:89db2a19e52e 1401 case 0b00000001:
yuron 21:89db2a19e52e 1402 kaisyu_usiro_limit = 0;
yuron 21:89db2a19e52e 1403 break;
yuron 21:89db2a19e52e 1404 default:
yuron 21:89db2a19e52e 1405 kaisyu_usiro_limit = 0;
yuron 21:89db2a19e52e 1406 break;
yuron 21:89db2a19e52e 1407 }
yuron 21:89db2a19e52e 1408
yuron 20:ac4954be1fe0 1409 //右腕上部リミット
yuron 20:ac4954be1fe0 1410 switch(masked_right_arm_upper_limit_data) {
yuron 20:ac4954be1fe0 1411 //押された
yuron 20:ac4954be1fe0 1412 case 0b00000000:
yuron 20:ac4954be1fe0 1413 right_arm_upper_limit = 1;
yuron 20:ac4954be1fe0 1414 break;
yuron 20:ac4954be1fe0 1415 case 0b00000010:
yuron 20:ac4954be1fe0 1416 right_arm_upper_limit = 0;
yuron 20:ac4954be1fe0 1417 break;
yuron 20:ac4954be1fe0 1418 default:
yuron 20:ac4954be1fe0 1419 right_arm_upper_limit = 0;
yuron 20:ac4954be1fe0 1420 break;
yuron 19:f17d2e585973 1421 }
yuron 20:ac4954be1fe0 1422
yuron 20:ac4954be1fe0 1423 //左腕下部リミット
yuron 20:ac4954be1fe0 1424 switch(masked_left_arm_lower_limit_data) {
yuron 20:ac4954be1fe0 1425 //押された
yuron 20:ac4954be1fe0 1426 case 0b00000000:
yuron 20:ac4954be1fe0 1427 left_arm_lower_limit = 1;
yuron 20:ac4954be1fe0 1428 break;
yuron 20:ac4954be1fe0 1429 case 0b00000100:
yuron 20:ac4954be1fe0 1430 left_arm_lower_limit = 0;
yuron 20:ac4954be1fe0 1431 break;
yuron 20:ac4954be1fe0 1432 default:
yuron 20:ac4954be1fe0 1433 left_arm_lower_limit = 0;
yuron 20:ac4954be1fe0 1434 break;
yuron 19:f17d2e585973 1435 }
yuron 20:ac4954be1fe0 1436
yuron 20:ac4954be1fe0 1437 //左腕上部リミット
yuron 20:ac4954be1fe0 1438 switch(masked_left_arm_upper_limit_data) {
yuron 20:ac4954be1fe0 1439 //押された
yuron 20:ac4954be1fe0 1440 case 0b00000000:
yuron 20:ac4954be1fe0 1441 left_arm_upper_limit = 1;
yuron 20:ac4954be1fe0 1442 break;
yuron 20:ac4954be1fe0 1443 case 0b00001000:
yuron 20:ac4954be1fe0 1444 left_arm_upper_limit = 0;
yuron 20:ac4954be1fe0 1445 break;
yuron 20:ac4954be1fe0 1446 default:
yuron 20:ac4954be1fe0 1447 left_arm_upper_limit = 0;
yuron 20:ac4954be1fe0 1448 break;
yuron 19:f17d2e585973 1449 }
yuron 20:ac4954be1fe0 1450
yuron 20:ac4954be1fe0 1451 //直動の前
yuron 20:ac4954be1fe0 1452 switch(masked_tyokudo_mae_limit_data) {
yuron 20:ac4954be1fe0 1453 //押された
yuron 20:ac4954be1fe0 1454 case 0b00000000:
yuron 20:ac4954be1fe0 1455 tyokudo_mae_limit = 1;
yuron 20:ac4954be1fe0 1456 break;
yuron 20:ac4954be1fe0 1457 case 0b00010000:
yuron 20:ac4954be1fe0 1458 tyokudo_mae_limit = 0;
yuron 20:ac4954be1fe0 1459 break;
yuron 20:ac4954be1fe0 1460 default:
yuron 20:ac4954be1fe0 1461 tyokudo_mae_limit = 0;
yuron 20:ac4954be1fe0 1462 break;
yuron 18:851f783ec516 1463 }
yuron 20:ac4954be1fe0 1464
yuron 20:ac4954be1fe0 1465 //直動の後
yuron 20:ac4954be1fe0 1466 switch(masked_tyokudo_usiro_limit_data) {
yuron 20:ac4954be1fe0 1467 //押された
yuron 20:ac4954be1fe0 1468 case 0b00000000:
yuron 20:ac4954be1fe0 1469 tyokudo_usiro_limit = 1;
yuron 20:ac4954be1fe0 1470 break;
yuron 20:ac4954be1fe0 1471 case 0b00100000:
yuron 20:ac4954be1fe0 1472 tyokudo_usiro_limit = 0;
yuron 20:ac4954be1fe0 1473 break;
yuron 20:ac4954be1fe0 1474 default:
yuron 20:ac4954be1fe0 1475 tyokudo_usiro_limit = 0;
yuron 20:ac4954be1fe0 1476 break;
yuron 18:851f783ec516 1477 }
yuron 19:f17d2e585973 1478 }
yuron 19:f17d2e585973 1479
yuron 19:f17d2e585973 1480 void wheel_reset(void) {
yuron 20:ac4954be1fe0 1481
yuron 19:f17d2e585973 1482 wheel_x1.reset();
yuron 19:f17d2e585973 1483 wheel_x2.reset();
yuron 19:f17d2e585973 1484 wheel_y1.reset();
yuron 19:f17d2e585973 1485 wheel_y2.reset();
yuron 19:f17d2e585973 1486 }
yuron 19:f17d2e585973 1487
yuron 21:89db2a19e52e 1488 void kaisyu(int pulse, int next_phase) {
yuron 19:f17d2e585973 1489
yuron 19:f17d2e585973 1490 switch (kaisyu_phase) {
yuron 21:89db2a19e52e 1491
yuron 19:f17d2e585973 1492 case 0:
yuron 19:f17d2e585973 1493 //前進->減速
yuron 20:ac4954be1fe0 1494 //3000pulseまで高速前進
yuron 20:ac4954be1fe0 1495 if(pulse < 3000) {
yuron 21:89db2a19e52e 1496 arm_motor[0] = 0xFF;
yuron 20:ac4954be1fe0 1497 //kaisyu_phase = 1;
yuron 21:89db2a19e52e 1498 }
yuron 21:89db2a19e52e 1499
yuron 20:ac4954be1fe0 1500 //3000pulse超えたら低速前進
yuron 20:ac4954be1fe0 1501 else if(pulse >= 3000) {
yuron 21:89db2a19e52e 1502 arm_motor[0] = 0xB3;
yuron 19:f17d2e585973 1503 kaisyu_phase = 1;
yuron 19:f17d2e585973 1504 }
yuron 19:f17d2e585973 1505 break;
yuron 21:89db2a19e52e 1506
yuron 20:ac4954be1fe0 1507 case 1:
yuron 19:f17d2e585973 1508 //前進->停止->後進
yuron 20:ac4954be1fe0 1509 //3600pulseまで低速前進
yuron 20:ac4954be1fe0 1510 if(pulse < 3600) {
yuron 21:89db2a19e52e 1511 arm_motor[0] = 0xB3;
yuron 20:ac4954be1fe0 1512 //kaisyu_phase = 2;
yuron 21:89db2a19e52e 1513 }
yuron 21:89db2a19e52e 1514
yuron 20:ac4954be1fe0 1515 //3600pulse超えたら停止
yuron 20:ac4954be1fe0 1516 else if(pulse >= 3600) {
yuron 21:89db2a19e52e 1517 arm_motor[0] = 0x80;
yuron 21:89db2a19e52e 1518
yuron 20:ac4954be1fe0 1519 //1秒待ってから引っ込める
yuron 20:ac4954be1fe0 1520 counter.start();
yuron 20:ac4954be1fe0 1521 if(counter.read() > 1.0f) {
yuron 20:ac4954be1fe0 1522 kaisyu_phase = 2;
yuron 20:ac4954be1fe0 1523 }
yuron 20:ac4954be1fe0 1524 }
yuron 21:89db2a19e52e 1525 //後ろのリミットが押されたら強制停止
yuron 21:89db2a19e52e 1526 if(kaisyu_usiro_limit == 1) {
yuron 21:89db2a19e52e 1527 arm_motor[0] = 0x80;
yuron 23:1e4d7540715f 1528 //1秒待ってから引っ込める
yuron 23:1e4d7540715f 1529 counter.start();
yuron 23:1e4d7540715f 1530 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1531 kaisyu_phase = 2;
yuron 23:1e4d7540715f 1532 }
yuron 21:89db2a19e52e 1533 }
yuron 20:ac4954be1fe0 1534 break;
yuron 21:89db2a19e52e 1535
yuron 20:ac4954be1fe0 1536 case 2:
yuron 20:ac4954be1fe0 1537 //後進->減速
yuron 20:ac4954be1fe0 1538 //500pulseまで高速後進
yuron 20:ac4954be1fe0 1539 counter.reset();
yuron 20:ac4954be1fe0 1540 if(pulse > 500) {
yuron 21:89db2a19e52e 1541 arm_motor[0] = 0x00;
yuron 20:ac4954be1fe0 1542 //kaisyu_phase = 3;
yuron 21:89db2a19e52e 1543
yuron 21:89db2a19e52e 1544 }
yuron 20:ac4954be1fe0 1545 //500pulse以下になったら低速後進
yuron 20:ac4954be1fe0 1546 else if(pulse <= 500) {
yuron 21:89db2a19e52e 1547 arm_motor[0] = 0x4C;
yuron 19:f17d2e585973 1548 kaisyu_phase = 3;
yuron 19:f17d2e585973 1549 }
yuron 19:f17d2e585973 1550 break;
yuron 21:89db2a19e52e 1551
yuron 19:f17d2e585973 1552 case 3:
yuron 20:ac4954be1fe0 1553 //後進->停止
yuron 20:ac4954be1fe0 1554 //リミット押されるまで低速後進
yuron 20:ac4954be1fe0 1555 if(pulse <= 500) {
yuron 21:89db2a19e52e 1556 arm_motor[0] = 0x4C;
yuron 20:ac4954be1fe0 1557 //kaisyu_phase = 4;
yuron 20:ac4954be1fe0 1558 }
yuron 21:89db2a19e52e 1559
yuron 20:ac4954be1fe0 1560 //リミット押されたら停止
yuron 21:89db2a19e52e 1561 if(kaisyu_mae_limit == 1) {
yuron 21:89db2a19e52e 1562 arm_motor[0] = 0x80;
yuron 19:f17d2e585973 1563 kaisyu_phase = 4;
yuron 21:89db2a19e52e 1564 phase = next_phase;
yuron 19:f17d2e585973 1565 }
yuron 19:f17d2e585973 1566 break;
yuron 21:89db2a19e52e 1567
yuron 19:f17d2e585973 1568 default:
yuron 21:89db2a19e52e 1569 arm_motor[0] = 0x80;
yuron 19:f17d2e585973 1570 break;
yuron 19:f17d2e585973 1571 }
yuron 19:f17d2e585973 1572
yuron 20:ac4954be1fe0 1573 //回収MDへ書き込み
yuron 21:89db2a19e52e 1574 i2c.write(0x18, arm_motor, 1);
yuron 19:f17d2e585973 1575 }
yuron 19:f17d2e585973 1576
yuron 23:1e4d7540715f 1577 void kaisyu_nobasu(int pulse, int next_phase) {
yuron 23:1e4d7540715f 1578
yuron 23:1e4d7540715f 1579 switch (kaisyu_phase) {
yuron 23:1e4d7540715f 1580 case 0:
yuron 23:1e4d7540715f 1581 //前進->減速
yuron 23:1e4d7540715f 1582 //3000pulseまで高速前進
yuron 23:1e4d7540715f 1583 if(pulse < 3000) {
yuron 23:1e4d7540715f 1584 arm_motor[0] = 0xFF;
yuron 23:1e4d7540715f 1585 }
yuron 23:1e4d7540715f 1586 //3000pulse超えたら低速前進
yuron 23:1e4d7540715f 1587 else if(pulse >= 3000) {
yuron 23:1e4d7540715f 1588 arm_motor[0] = 0xB3;
yuron 23:1e4d7540715f 1589 kaisyu_phase = 1;
yuron 23:1e4d7540715f 1590 }
yuron 23:1e4d7540715f 1591 break;
yuron 23:1e4d7540715f 1592
yuron 23:1e4d7540715f 1593 case 1:
yuron 23:1e4d7540715f 1594 //前進->停止->後進
yuron 23:1e4d7540715f 1595 //3600pulseまで低速前進
yuron 23:1e4d7540715f 1596 if(pulse < 3600) {
yuron 23:1e4d7540715f 1597 arm_motor[0] = 0xB3;
yuron 23:1e4d7540715f 1598 }
yuron 23:1e4d7540715f 1599 //3600pulse超えたら停止
yuron 23:1e4d7540715f 1600 else if(pulse >= 3600) {
yuron 23:1e4d7540715f 1601 arm_motor[0] = 0x80;
yuron 23:1e4d7540715f 1602 phase = next_phase;
yuron 23:1e4d7540715f 1603 kaisyu_phase = 2;
yuron 23:1e4d7540715f 1604 }
yuron 23:1e4d7540715f 1605 //後ろのリミットが押されたら強制停止
yuron 23:1e4d7540715f 1606 if(kaisyu_usiro_limit == 1) {
yuron 23:1e4d7540715f 1607 arm_motor[0] = 0x80;
yuron 24:d12bc20c01c2 1608 kaisyu_phase = 2;
yuron 23:1e4d7540715f 1609 phase = next_phase;
yuron 23:1e4d7540715f 1610 }
yuron 23:1e4d7540715f 1611 break;
yuron 23:1e4d7540715f 1612
yuron 23:1e4d7540715f 1613 default:
yuron 23:1e4d7540715f 1614 arm_motor[0] = 0x80;
yuron 23:1e4d7540715f 1615 break;
yuron 23:1e4d7540715f 1616 }
yuron 23:1e4d7540715f 1617 //回収MDへ書き込み
yuron 23:1e4d7540715f 1618 i2c.write(0x18, arm_motor, 1);
yuron 23:1e4d7540715f 1619 }
yuron 23:1e4d7540715f 1620
yuron 23:1e4d7540715f 1621 void kaisyu_hiku(int pulse, int next_phase) {
yuron 23:1e4d7540715f 1622
yuron 23:1e4d7540715f 1623 switch(kaisyu_phase) {
yuron 23:1e4d7540715f 1624 case 2:
yuron 23:1e4d7540715f 1625 //後進->減速
yuron 23:1e4d7540715f 1626 //500pulseまで高速後進
yuron 23:1e4d7540715f 1627 if(pulse > 500) {
yuron 23:1e4d7540715f 1628 arm_motor[0] = 0x00;
yuron 23:1e4d7540715f 1629 }
yuron 23:1e4d7540715f 1630 //500pulse以下になったら低速後進
yuron 23:1e4d7540715f 1631 else if(pulse <= 500) {
yuron 23:1e4d7540715f 1632 arm_motor[0] = 0x4C;
yuron 23:1e4d7540715f 1633 kaisyu_phase = 3;
yuron 23:1e4d7540715f 1634 }
yuron 23:1e4d7540715f 1635 break;
yuron 23:1e4d7540715f 1636
yuron 23:1e4d7540715f 1637 case 3:
yuron 23:1e4d7540715f 1638 //後進->停止
yuron 23:1e4d7540715f 1639 //リミット押されるまで低速後進
yuron 23:1e4d7540715f 1640 if(pulse <= 500) {
yuron 23:1e4d7540715f 1641 arm_motor[0] = 0x4C;
yuron 23:1e4d7540715f 1642 }
yuron 23:1e4d7540715f 1643
yuron 23:1e4d7540715f 1644 //リミット押されたら停止
yuron 23:1e4d7540715f 1645 if(kaisyu_mae_limit == 1) {
yuron 23:1e4d7540715f 1646 arm_motor[0] = 0x80;
yuron 23:1e4d7540715f 1647 kaisyu_phase = 4;
yuron 23:1e4d7540715f 1648 phase = next_phase;
yuron 23:1e4d7540715f 1649 }
yuron 23:1e4d7540715f 1650 break;
yuron 23:1e4d7540715f 1651
yuron 23:1e4d7540715f 1652 default:
yuron 23:1e4d7540715f 1653 arm_motor[0] = 0x80;
yuron 23:1e4d7540715f 1654 break;
yuron 23:1e4d7540715f 1655 }
yuron 23:1e4d7540715f 1656 //回収MDへ書き込み
yuron 23:1e4d7540715f 1657 i2c.write(0x18, arm_motor, 1);
yuron 23:1e4d7540715f 1658 }
yuron 23:1e4d7540715f 1659
yuron 21:89db2a19e52e 1660 void tyokudo(int pulse, int next_phase) {
yuron 20:ac4954be1fe0 1661
yuron 20:ac4954be1fe0 1662 switch(tyokudo_phase) {
yuron 21:89db2a19e52e 1663
yuron 19:f17d2e585973 1664 case 0:
yuron 19:f17d2e585973 1665 //前進->減速
yuron 21:89db2a19e52e 1666
yuron 21:89db2a19e52e 1667 /* エンコーダー読まずにリミットだけ(修正必須) */
yuron 20:ac4954be1fe0 1668 //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
yuron 20:ac4954be1fe0 1669 if(tyokudo_mae_limit == 0) {
yuron 20:ac4954be1fe0 1670 //2000pulseまで高速前進
yuron 20:ac4954be1fe0 1671 if(pulse < 2000) {
yuron 21:89db2a19e52e 1672 arm_motor[0] = 0xC0;
yuron 21:89db2a19e52e 1673 drop_motor[0] = 0xE6;
yuron 21:89db2a19e52e 1674 }
yuron 20:ac4954be1fe0 1675 //2000pulse以上で低速前進
yuron 20:ac4954be1fe0 1676 else if(pulse >= 2000) {
yuron 21:89db2a19e52e 1677 arm_motor[0] = 0xC0;
yuron 21:89db2a19e52e 1678 drop_motor[0] = 0xE6;
yuron 20:ac4954be1fe0 1679 }
yuron 20:ac4954be1fe0 1680 //パルスが3600を終えたらアームのみ強制停止
yuron 20:ac4954be1fe0 1681 else if(pulse > 3600) {
yuron 21:89db2a19e52e 1682 arm_motor[0] = 0x80;
yuron 21:89db2a19e52e 1683 drop_motor[0] = 0xE6;
yuron 21:89db2a19e52e 1684 }
yuron 21:89db2a19e52e 1685
yuron 21:89db2a19e52e 1686 //後ろのリミットが押されたら強制停止
yuron 21:89db2a19e52e 1687 if(kaisyu_usiro_limit == 1) {
yuron 21:89db2a19e52e 1688 arm_motor[0] = 0x80;
yuron 20:ac4954be1fe0 1689 }
yuron 20:ac4954be1fe0 1690 }
yuron 21:89db2a19e52e 1691
yuron 20:ac4954be1fe0 1692 //直動の前リミットが押されたら
yuron 20:ac4954be1fe0 1693 else if(tyokudo_mae_limit == 1) {
yuron 21:89db2a19e52e 1694 //高速後進
yuron 24:d12bc20c01c2 1695 arm_motor[0] = 0x4C;
yuron 21:89db2a19e52e 1696 drop_motor[0] = 0x00;
yuron 21:89db2a19e52e 1697 tyokudo_phase = 1;
yuron 21:89db2a19e52e 1698 }
yuron 21:89db2a19e52e 1699 break;
yuron 21:89db2a19e52e 1700
yuron 21:89db2a19e52e 1701 case 1:
yuron 24:d12bc20c01c2 1702 //後進->停止
yuron 21:89db2a19e52e 1703 if(tyokudo_usiro_limit == 1) {
yuron 24:d12bc20c01c2 1704 drop_motor[0] = 0x80;
yuron 24:d12bc20c01c2 1705
yuron 24:d12bc20c01c2 1706 if(kaisyu_mae_limit == 1) {
yuron 24:d12bc20c01c2 1707 arm_motor[0] = 0x80;
yuron 24:d12bc20c01c2 1708 tyokudo_phase = 2;
yuron 24:d12bc20c01c2 1709 phase = next_phase;
yuron 24:d12bc20c01c2 1710 }
yuron 24:d12bc20c01c2 1711 }
yuron 24:d12bc20c01c2 1712 if(kaisyu_mae_limit == 1) {
yuron 21:89db2a19e52e 1713 arm_motor[0] = 0x80;
yuron 24:d12bc20c01c2 1714
yuron 24:d12bc20c01c2 1715 if(tyokudo_usiro_limit == 1) {
yuron 24:d12bc20c01c2 1716 drop_motor[0] = 0x80;
yuron 24:d12bc20c01c2 1717 tyokudo_phase = 2;
yuron 24:d12bc20c01c2 1718 phase = next_phase;
yuron 24:d12bc20c01c2 1719 }
yuron 19:f17d2e585973 1720 }
yuron 19:f17d2e585973 1721 break;
yuron 20:ac4954be1fe0 1722
yuron 19:f17d2e585973 1723 default:
yuron 21:89db2a19e52e 1724 arm_motor[0] = 0x80;
yuron 21:89db2a19e52e 1725 drop_motor[0] = 0x80;
yuron 19:f17d2e585973 1726 break;
yuron 19:f17d2e585973 1727 }
yuron 24:d12bc20c01c2 1728 //回収MD・排出MDへ書き込み
yuron 21:89db2a19e52e 1729 i2c.write(0x18, arm_motor, 1);
yuron 21:89db2a19e52e 1730 i2c.write(0x20, drop_motor, 1);
yuron 18:851f783ec516 1731 }
yuron 18:851f783ec516 1732
yuron 21:89db2a19e52e 1733 void arm_up(int next_phase) {
yuron 20:ac4954be1fe0 1734
yuron 20:ac4954be1fe0 1735 //両腕、上限リミットが押されてなかったら上昇
yuron 20:ac4954be1fe0 1736 if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
yuron 26:81346a21d301 1737 right_arm_data[0] = 0xFF; left_arm_data[0] = 0x00;
yuron 20:ac4954be1fe0 1738 }
yuron 20:ac4954be1fe0 1739 //右腕のみリミットが押されたら左腕のみ上昇
yuron 20:ac4954be1fe0 1740 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
yuron 26:81346a21d301 1741 right_arm_data[0] = 0x80; left_arm_data[0] = 0x00;
yuron 20:ac4954be1fe0 1742 }
yuron 20:ac4954be1fe0 1743 //左腕のみリミットが押されたら右腕のみ上昇
yuron 20:ac4954be1fe0 1744 else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
yuron 20:ac4954be1fe0 1745 right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80;
yuron 20:ac4954be1fe0 1746 }
yuron 20:ac4954be1fe0 1747 //両腕、上限リミットが押されたら停止
yuron 20:ac4954be1fe0 1748 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
yuron 20:ac4954be1fe0 1749 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
yuron 21:89db2a19e52e 1750 phase = next_phase;
yuron 20:ac4954be1fe0 1751 }
yuron 20:ac4954be1fe0 1752
yuron 20:ac4954be1fe0 1753 i2c.write(0x22, right_arm_data, 1);
yuron 20:ac4954be1fe0 1754 i2c.write(0x24, left_arm_data, 1);
yuron 21:89db2a19e52e 1755 wait_us(20);
yuron 20:ac4954be1fe0 1756 }
yuron 20:ac4954be1fe0 1757
yuron 17:de3bc1999ae7 1758 void front(int target) {
yuron 20:ac4954be1fe0 1759
yuron 14:ab89b6cd9719 1760 front_PID(target);
yuron 14:ab89b6cd9719 1761 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1762 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1763 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1764 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1765 wait_us(20);
yuron 14:ab89b6cd9719 1766 }
yuron 4:df334779a69e 1767
yuron 17:de3bc1999ae7 1768 void back(int target) {
yuron 20:ac4954be1fe0 1769
yuron 14:ab89b6cd9719 1770 back_PID(target);
yuron 14:ab89b6cd9719 1771 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1772 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1773 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1774 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1775 wait_us(20);
yuron 14:ab89b6cd9719 1776 }
yuron 5:167327a82430 1777
yuron 17:de3bc1999ae7 1778 void right(int target) {
yuron 20:ac4954be1fe0 1779
yuron 14:ab89b6cd9719 1780 right_PID(target);
yuron 14:ab89b6cd9719 1781 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1782 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1783 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1784 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1785 wait_us(20);
yuron 14:ab89b6cd9719 1786 }
yuron 5:167327a82430 1787
yuron 17:de3bc1999ae7 1788 void left(int target) {
yuron 20:ac4954be1fe0 1789
yuron 14:ab89b6cd9719 1790 left_PID(target);
yuron 14:ab89b6cd9719 1791 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1792 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1793 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1794 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1795 wait_us(20);
yuron 14:ab89b6cd9719 1796 }
yuron 4:df334779a69e 1797
yuron 17:de3bc1999ae7 1798 void turn_right(int target) {
yuron 20:ac4954be1fe0 1799
yuron 14:ab89b6cd9719 1800 turn_right_PID(target);
yuron 14:ab89b6cd9719 1801 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1802 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1803 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1804 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1805 wait_us(20);
yuron 14:ab89b6cd9719 1806 }
yuron 4:df334779a69e 1807
yuron 17:de3bc1999ae7 1808 void turn_left(int target) {
yuron 20:ac4954be1fe0 1809
yuron 14:ab89b6cd9719 1810 turn_left_PID(target);
yuron 14:ab89b6cd9719 1811 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1812 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1813 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1814 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1815 wait_us(20);
yuron 14:ab89b6cd9719 1816 }
yuron 5:167327a82430 1817
yuron 18:851f783ec516 1818 void stop(void) {
yuron 20:ac4954be1fe0 1819
yuron 18:851f783ec516 1820 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 1821 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 1822 true_hidarimae_data[0] = 0x80;
yuron 20:ac4954be1fe0 1823 true_hidariusiro_data[0] = 0x80;
yuron 20:ac4954be1fe0 1824
yuron 18:851f783ec516 1825 i2c.write(0x10, true_migimae_data, 1, false);
yuron 18:851f783ec516 1826 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 18:851f783ec516 1827 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 18:851f783ec516 1828 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 18:851f783ec516 1829 wait_us(20);
yuron 18:851f783ec516 1830 }
yuron 18:851f783ec516 1831
yuron 22:5682246f9409 1832 void all_stop(void) {
yuron 22:5682246f9409 1833
yuron 22:5682246f9409 1834 true_migimae_data[0] = 0x80;
yuron 22:5682246f9409 1835 true_migiusiro_data[0] = 0x80;
yuron 22:5682246f9409 1836 true_hidarimae_data[0] = 0x80;
yuron 22:5682246f9409 1837 true_hidariusiro_data[0] = 0x80;
yuron 22:5682246f9409 1838 arm_motor[0] = 0x80;
yuron 22:5682246f9409 1839 drop_motor[0] = 0x80;
yuron 22:5682246f9409 1840 right_arm_data[0] = 0x80;
yuron 22:5682246f9409 1841 left_arm_data[0] = 0x80;
yuron 22:5682246f9409 1842 fan_data[0] = 0x80;
yuron 22:5682246f9409 1843 servo_data[0] = 0x04;
yuron 22:5682246f9409 1844
yuron 22:5682246f9409 1845 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 1846 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 1847 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 1848 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 1849 i2c.write(0x18, arm_motor, 1);
yuron 22:5682246f9409 1850 i2c.write(0x20, drop_motor, 1);
yuron 22:5682246f9409 1851 i2c.write(0x22, right_arm_data, 1);
yuron 22:5682246f9409 1852 i2c.write(0x24, left_arm_data, 1);
yuron 22:5682246f9409 1853 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1854 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1855 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 1856 wait_us(20);
yuron 22:5682246f9409 1857 }
yuron 22:5682246f9409 1858
yuron 17:de3bc1999ae7 1859 void front_PID(int target) {
yuron 5:167327a82430 1860
yuron 14:ab89b6cd9719 1861 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 1862 front_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1863 front_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1864 front_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1865 front_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 5:167327a82430 1866
yuron 14:ab89b6cd9719 1867 //制御量の最小、最大
yuron 14:ab89b6cd9719 1868 //正転(目標に達してない)
yuron 19:f17d2e585973 1869 if((y_pulse1 < target) && (y_pulse2 < target)) {
yuron 22:5682246f9409 1870 front_migimae.setOutputLimits(0x84, 0xF5);
yuron 22:5682246f9409 1871 front_migiusiro.setOutputLimits(0x84, 0xF5);
yuron 16:05b26003da50 1872 front_hidarimae.setOutputLimits(0x84, 0xFF);
yuron 16:05b26003da50 1873 front_hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 14:ab89b6cd9719 1874 }
yuron 18:851f783ec516 1875 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1876 else if((y_pulse1 > target) && (y_pulse2 > target)) {
yuron 18:851f783ec516 1877 front_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1878 front_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1879 front_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1880 front_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 14:ab89b6cd9719 1881 }
yuron 5:167327a82430 1882
yuron 14:ab89b6cd9719 1883 //よくわからんやつ
yuron 16:05b26003da50 1884 front_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1885 front_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 1886 front_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1887 front_hidariusiro.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 1888
yuron 14:ab89b6cd9719 1889 //目標値
yuron 16:05b26003da50 1890 front_migimae.setSetPoint(target);
yuron 16:05b26003da50 1891 front_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 1892 front_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 1893 front_hidariusiro.setSetPoint(target);
yuron 5:167327a82430 1894
yuron 14:ab89b6cd9719 1895 //センサ出力
yuron 16:05b26003da50 1896 front_migimae.setProcessValue(y_pulse1);
yuron 16:05b26003da50 1897 front_migiusiro.setProcessValue(y_pulse1);
yuron 16:05b26003da50 1898 front_hidarimae.setProcessValue(y_pulse2);
yuron 16:05b26003da50 1899 front_hidariusiro.setProcessValue(y_pulse2);
yuron 5:167327a82430 1900
yuron 14:ab89b6cd9719 1901 //制御量(計算結果)
yuron 16:05b26003da50 1902 migimae_data[0] = front_migimae.compute();
yuron 16:05b26003da50 1903 migiusiro_data[0] = front_migiusiro.compute();
yuron 16:05b26003da50 1904 hidarimae_data[0] = front_hidarimae.compute();
yuron 16:05b26003da50 1905 hidariusiro_data[0] = front_hidariusiro.compute();
yuron 4:df334779a69e 1906
yuron 14:ab89b6cd9719 1907 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 1908 //正転(目標に達してない)
yuron 19:f17d2e585973 1909 if((y_pulse1 < target) && (y_pulse2 < target)) {
yuron 14:ab89b6cd9719 1910 true_migimae_data[0] = migimae_data[0];
yuron 14:ab89b6cd9719 1911 true_migiusiro_data[0] = migiusiro_data[0];
yuron 14:ab89b6cd9719 1912 true_hidarimae_data[0] = hidarimae_data[0];
yuron 14:ab89b6cd9719 1913 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 14:ab89b6cd9719 1914 }
yuron 18:851f783ec516 1915 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1916 else if((y_pulse1 > target) && (y_pulse2 > target)) {
yuron 18:851f783ec516 1917 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 1918 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 1919 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 1920 true_hidariusiro_data[0] = 0x80;
yuron 17:de3bc1999ae7 1921 }
yuron 5:167327a82430 1922 }
yuron 5:167327a82430 1923
yuron 17:de3bc1999ae7 1924 void back_PID(int target) {
yuron 20:ac4954be1fe0 1925
yuron 14:ab89b6cd9719 1926 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 1927 back_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1928 back_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1929 back_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1930 back_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 5:167327a82430 1931
yuron 14:ab89b6cd9719 1932 //制御量の最小、最大
yuron 14:ab89b6cd9719 1933 //逆転(目標に達してない)
yuron 19:f17d2e585973 1934 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
yuron 16:05b26003da50 1935 back_migimae.setOutputLimits(0x00, 0x7B);
yuron 16:05b26003da50 1936 back_migiusiro.setOutputLimits(0x00, 0x7B);
yuron 22:5682246f9409 1937 back_hidarimae.setOutputLimits(0x00, 0x70);
yuron 22:5682246f9409 1938 back_hidariusiro.setOutputLimits(0x00, 0x70);
yuron 22:5682246f9409 1939 //back_hidarimae.setOutputLimits(0x00, 0x7B);
yuron 22:5682246f9409 1940 //back_hidariusiro.setOutputLimits(0x00, 0x7B);
yuron 14:ab89b6cd9719 1941 }
yuron 18:851f783ec516 1942 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1943 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 1944 back_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1945 back_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1946 back_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1947 back_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 4:df334779a69e 1948 }
yuron 5:167327a82430 1949
yuron 14:ab89b6cd9719 1950 //よくわからんやつ
yuron 16:05b26003da50 1951 back_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1952 back_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 1953 back_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1954 back_hidariusiro.setMode(AUTO_MODE);
yuron 14:ab89b6cd9719 1955
yuron 14:ab89b6cd9719 1956 //目標値
yuron 17:de3bc1999ae7 1957 back_migimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1958 back_migiusiro.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1959 back_hidarimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1960 back_hidariusiro.setSetPoint(target*-1);
yuron 14:ab89b6cd9719 1961
yuron 14:ab89b6cd9719 1962 //センサ出力
yuron 17:de3bc1999ae7 1963 back_migimae.setProcessValue(y_pulse1*-1);
yuron 17:de3bc1999ae7 1964 back_migiusiro.setProcessValue(y_pulse1*-1);
yuron 17:de3bc1999ae7 1965 back_hidarimae.setProcessValue(y_pulse2*-1);
yuron 17:de3bc1999ae7 1966 back_hidariusiro.setProcessValue(y_pulse2*-1);
yuron 14:ab89b6cd9719 1967
yuron 14:ab89b6cd9719 1968 //制御量(計算結果)
yuron 16:05b26003da50 1969 migimae_data[0] = back_migimae.compute();
yuron 16:05b26003da50 1970 migiusiro_data[0] = back_migiusiro.compute();
yuron 16:05b26003da50 1971 hidarimae_data[0] = back_hidarimae.compute();
yuron 16:05b26003da50 1972 hidariusiro_data[0] = back_hidariusiro.compute();
yuron 14:ab89b6cd9719 1973
yuron 14:ab89b6cd9719 1974 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 1975 //逆転(目標に達してない)
yuron 19:f17d2e585973 1976 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
yuron 14:ab89b6cd9719 1977 true_migimae_data[0] = 0x7B - migimae_data[0];
yuron 14:ab89b6cd9719 1978 true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
yuron 14:ab89b6cd9719 1979 true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
yuron 14:ab89b6cd9719 1980 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
yuron 5:167327a82430 1981 }
yuron 18:851f783ec516 1982 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1983 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 1984 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 1985 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 1986 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 1987 true_hidariusiro_data[0] = 0x80;
yuron 17:de3bc1999ae7 1988 }
yuron 14:ab89b6cd9719 1989 }
yuron 14:ab89b6cd9719 1990
yuron 17:de3bc1999ae7 1991 void right_PID(int target) {
yuron 14:ab89b6cd9719 1992
yuron 14:ab89b6cd9719 1993 //センサ出力値の最小、最大
yuron 16:05b26003da50 1994 right_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1995 right_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1996 right_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1997 right_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 14:ab89b6cd9719 1998
yuron 14:ab89b6cd9719 1999 //制御量の最小、最大
yuron 14:ab89b6cd9719 2000 //右進(目標まで達していない)
yuron 19:f17d2e585973 2001 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
yuron 22:5682246f9409 2002 right_migimae.setOutputLimits(0x6A, 0x6C);
yuron 22:5682246f9409 2003 //right_migimae.setOutputLimits(0x7A, 0x7B);
yuron 21:89db2a19e52e 2004 right_migiusiro.setOutputLimits(0xFE, 0xFF);
yuron 22:5682246f9409 2005 right_hidarimae.setOutputLimits(0xEF, 0xF0);
yuron 22:5682246f9409 2006 //right_hidarimae.setOutputLimits(0xFE, 0xFF);
yuron 21:89db2a19e52e 2007 right_hidariusiro.setOutputLimits(0x7A, 0x7B);
yuron 17:de3bc1999ae7 2008
yuron 17:de3bc1999ae7 2009 }
yuron 18:851f783ec516 2010 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 2011 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 2012 right_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2013 right_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2014 right_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2015 right_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 5:167327a82430 2016 }
yuron 5:167327a82430 2017
yuron 14:ab89b6cd9719 2018 //よくわからんやつ
yuron 16:05b26003da50 2019 right_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2020 right_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2021 right_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2022 right_hidariusiro.setMode(AUTO_MODE);
yuron 14:ab89b6cd9719 2023
yuron 14:ab89b6cd9719 2024 //目標値
yuron 17:de3bc1999ae7 2025 right_migimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 2026 right_migiusiro.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 2027 right_hidarimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 2028 right_hidariusiro.setSetPoint(target*-1);
yuron 8:3df97287c825 2029
yuron 14:ab89b6cd9719 2030 //センサ出力
yuron 17:de3bc1999ae7 2031 right_migimae.setProcessValue(x_pulse1*-1);
yuron 17:de3bc1999ae7 2032 right_migiusiro.setProcessValue(x_pulse2*-1);
yuron 17:de3bc1999ae7 2033 right_hidarimae.setProcessValue(x_pulse1*-1);
yuron 17:de3bc1999ae7 2034 right_hidariusiro.setProcessValue(x_pulse2*-1);
yuron 14:ab89b6cd9719 2035
yuron 14:ab89b6cd9719 2036 //制御量(計算結果)
yuron 16:05b26003da50 2037 migimae_data[0] = right_migimae.compute();
yuron 16:05b26003da50 2038 migiusiro_data[0] = right_migiusiro.compute();
yuron 16:05b26003da50 2039 hidarimae_data[0] = right_hidarimae.compute();
yuron 16:05b26003da50 2040 hidariusiro_data[0] = right_hidariusiro.compute();
yuron 8:3df97287c825 2041
yuron 14:ab89b6cd9719 2042 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 2043 //右進(目標まで達していない)
yuron 19:f17d2e585973 2044 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
yuron 14:ab89b6cd9719 2045 true_migimae_data[0] = 0x7B - migimae_data[0];
yuron 14:ab89b6cd9719 2046 true_migiusiro_data[0] = migiusiro_data[0];
yuron 14:ab89b6cd9719 2047 true_hidarimae_data[0] = hidarimae_data[0];
yuron 14:ab89b6cd9719 2048 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
yuron 9:1359f0c813b1 2049 }
yuron 17:de3bc1999ae7 2050 //左進(目標より行き過ぎ)
yuron 19:f17d2e585973 2051 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 2052 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2053 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2054 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2055 true_hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 2056 }
yuron 14:ab89b6cd9719 2057 }
yuron 9:1359f0c813b1 2058
yuron 17:de3bc1999ae7 2059 void left_PID(int target) {
yuron 20:ac4954be1fe0 2060
yuron 14:ab89b6cd9719 2061 //センサ出力値の最小、最大
yuron 16:05b26003da50 2062 left_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2063 left_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2064 left_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2065 left_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 9:1359f0c813b1 2066
yuron 14:ab89b6cd9719 2067 //制御量の最小、最大
yuron 14:ab89b6cd9719 2068 //左進(目標まで達していない)
yuron 22:5682246f9409 2069 if((x_pulse1 < target) && (x_pulse2 < target)) {
yuron 24:d12bc20c01c2 2070 left_migimae.setOutputLimits(0xEC, 0xED);
yuron 24:d12bc20c01c2 2071 left_migiusiro.setOutputLimits(0x7A, 0x7B);
yuron 24:d12bc20c01c2 2072 left_hidarimae.setOutputLimits(0x6E, 0x6F);
yuron 24:d12bc20c01c2 2073 left_hidariusiro.setOutputLimits(0xFE, 0xFF);
yuron 17:de3bc1999ae7 2074 }
yuron 18:851f783ec516 2075 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2076 else if((x_pulse1 > target) && (x_pulse2 > target)) {
yuron 18:851f783ec516 2077 left_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2078 left_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2079 left_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2080 left_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 8:3df97287c825 2081 }
yuron 20:ac4954be1fe0 2082
yuron 14:ab89b6cd9719 2083 //よくわからんやつ
yuron 16:05b26003da50 2084 left_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2085 left_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2086 left_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2087 left_hidariusiro.setMode(AUTO_MODE);
yuron 10:b672aa81b226 2088
yuron 14:ab89b6cd9719 2089 //目標値
yuron 16:05b26003da50 2090 left_migimae.setSetPoint(target);
yuron 16:05b26003da50 2091 left_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 2092 left_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 2093 left_hidariusiro.setSetPoint(target);
yuron 9:1359f0c813b1 2094
yuron 14:ab89b6cd9719 2095 //センサ出力
yuron 16:05b26003da50 2096 left_migimae.setProcessValue(x_pulse1);
yuron 16:05b26003da50 2097 left_migiusiro.setProcessValue(x_pulse2);
yuron 16:05b26003da50 2098 left_hidarimae.setProcessValue(x_pulse1);
yuron 16:05b26003da50 2099 left_hidariusiro.setProcessValue(x_pulse2);
yuron 8:3df97287c825 2100
yuron 14:ab89b6cd9719 2101 //制御量(計算結果)
yuron 16:05b26003da50 2102 migimae_data[0] = left_migimae.compute();
yuron 16:05b26003da50 2103 migiusiro_data[0] = left_migiusiro.compute();
yuron 16:05b26003da50 2104 hidarimae_data[0] = left_hidarimae.compute();
yuron 16:05b26003da50 2105 hidariusiro_data[0] = left_hidariusiro.compute();
yuron 8:3df97287c825 2106
yuron 14:ab89b6cd9719 2107 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 2108 //左進(目標まで達していない)
yuron 22:5682246f9409 2109 if((x_pulse1 < target) && (x_pulse2 < target)) {
yuron 14:ab89b6cd9719 2110 true_migimae_data[0] = migimae_data[0];
yuron 14:ab89b6cd9719 2111 true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
yuron 14:ab89b6cd9719 2112 true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
yuron 14:ab89b6cd9719 2113 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 14:ab89b6cd9719 2114 }
yuron 18:851f783ec516 2115 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2116 else if((x_pulse1 > target) && (x_pulse2 > target)) {
yuron 18:851f783ec516 2117 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2118 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2119 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2120 true_hidariusiro_data[0] = 0x80;
yuron 17:de3bc1999ae7 2121 }
yuron 14:ab89b6cd9719 2122 }
yuron 12:1a22b9797004 2123
yuron 17:de3bc1999ae7 2124 void turn_right_PID(int target) {
yuron 14:ab89b6cd9719 2125
yuron 14:ab89b6cd9719 2126 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 2127 turn_right_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2128 turn_right_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2129 turn_right_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2130 turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 9:1359f0c813b1 2131
yuron 14:ab89b6cd9719 2132 //制御量の最小、最大
yuron 14:ab89b6cd9719 2133 //右旋回(目標に達してない)
yuron 22:5682246f9409 2134 if(sum_pulse < target) {
yuron 17:de3bc1999ae7 2135 turn_right_migimae.setOutputLimits(0x10, 0x7B);
yuron 17:de3bc1999ae7 2136 turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
yuron 17:de3bc1999ae7 2137 turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
yuron 17:de3bc1999ae7 2138 turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
yuron 8:3df97287c825 2139 }
yuron 18:851f783ec516 2140 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2141 else if(sum_pulse > target) {
yuron 18:851f783ec516 2142 turn_right_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2143 turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2144 turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2145 turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 4:df334779a69e 2146 }
yuron 8:3df97287c825 2147
yuron 14:ab89b6cd9719 2148 //よくわからんやつ
yuron 16:05b26003da50 2149 turn_right_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2150 turn_right_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2151 turn_right_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2152 turn_right_hidariusiro.setMode(AUTO_MODE);
yuron 8:3df97287c825 2153
yuron 14:ab89b6cd9719 2154 //目標値
yuron 16:05b26003da50 2155 turn_right_migimae.setSetPoint(target);
yuron 16:05b26003da50 2156 turn_right_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 2157 turn_right_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 2158 turn_right_hidariusiro.setSetPoint(target);
yuron 5:167327a82430 2159
yuron 14:ab89b6cd9719 2160 //センサ出力
yuron 22:5682246f9409 2161 turn_right_migimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2162 turn_right_migiusiro.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2163 turn_right_hidarimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2164 turn_right_hidariusiro.setProcessValue(sum_pulse);
yuron 5:167327a82430 2165
yuron 14:ab89b6cd9719 2166 //制御量(計算結果)
yuron 16:05b26003da50 2167 migimae_data[0] = turn_right_migimae.compute();
yuron 16:05b26003da50 2168 migiusiro_data[0] = turn_right_migiusiro.compute();
yuron 16:05b26003da50 2169 hidarimae_data[0] = turn_right_hidarimae.compute();
yuron 16:05b26003da50 2170 hidariusiro_data[0] = turn_right_hidariusiro.compute();
yuron 8:3df97287c825 2171
yuron 14:ab89b6cd9719 2172 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 2173 //右旋回(目標に達してない)
yuron 22:5682246f9409 2174 if(sum_pulse < target) {
yuron 14:ab89b6cd9719 2175 true_migimae_data[0] = 0x7B - migimae_data[0];
yuron 14:ab89b6cd9719 2176 true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
yuron 14:ab89b6cd9719 2177 true_hidarimae_data[0] = hidarimae_data[0];
yuron 14:ab89b6cd9719 2178 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 14:ab89b6cd9719 2179 }
yuron 18:851f783ec516 2180 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2181 else if(sum_pulse > target) {
yuron 18:851f783ec516 2182 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2183 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2184 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2185 true_hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 2186 }
yuron 14:ab89b6cd9719 2187 }
yuron 8:3df97287c825 2188
yuron 17:de3bc1999ae7 2189 void turn_left_PID(int target) {
yuron 20:ac4954be1fe0 2190
yuron 14:ab89b6cd9719 2191 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 2192 turn_left_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2193 turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2194 turn_left_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2195 turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 9:1359f0c813b1 2196
yuron 14:ab89b6cd9719 2197 //制御量の最小、最大
yuron 18:851f783ec516 2198 //左旋回(目標に達してない)
yuron 22:5682246f9409 2199 if(sum_pulse < target) {
yuron 17:de3bc1999ae7 2200 turn_left_migimae.setOutputLimits(0x94, 0xFF);
yuron 17:de3bc1999ae7 2201 turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
yuron 17:de3bc1999ae7 2202 turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
yuron 17:de3bc1999ae7 2203 turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
yuron 14:ab89b6cd9719 2204 }
yuron 18:851f783ec516 2205 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2206 else if(sum_pulse > target) {
yuron 18:851f783ec516 2207 turn_left_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2208 turn_left_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2209 turn_left_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2210 turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 7:7f16fb8b0192 2211 }
yuron 8:3df97287c825 2212
yuron 14:ab89b6cd9719 2213 //よくわからんやつ
yuron 16:05b26003da50 2214 turn_left_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2215 turn_left_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2216 turn_left_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2217 turn_left_hidariusiro.setMode(AUTO_MODE);
yuron 5:167327a82430 2218
yuron 14:ab89b6cd9719 2219 //目標値
yuron 16:05b26003da50 2220 turn_left_migimae.setSetPoint(target);
yuron 16:05b26003da50 2221 turn_left_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 2222 turn_left_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 2223 turn_left_hidariusiro.setSetPoint(target);
yuron 8:3df97287c825 2224
yuron 14:ab89b6cd9719 2225 //センサ出力
yuron 22:5682246f9409 2226 turn_left_migimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2227 turn_left_migiusiro.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2228 turn_left_hidarimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2229 turn_left_hidariusiro.setProcessValue(sum_pulse);
yuron 5:167327a82430 2230
yuron 14:ab89b6cd9719 2231 //制御量(計算結果)
yuron 16:05b26003da50 2232 migimae_data[0] = turn_left_migimae.compute();
yuron 16:05b26003da50 2233 migiusiro_data[0] = turn_left_migiusiro.compute();
yuron 16:05b26003da50 2234 hidarimae_data[0] = turn_left_hidarimae.compute();
yuron 16:05b26003da50 2235 hidariusiro_data[0] = turn_left_hidariusiro.compute();
yuron 5:167327a82430 2236
yuron 14:ab89b6cd9719 2237 //制御量をPWM値に変換
yuron 18:851f783ec516 2238 //左旋回(目標に達してない)
yuron 22:5682246f9409 2239 if(sum_pulse < target) {
yuron 14:ab89b6cd9719 2240 true_migimae_data[0] = migimae_data[0];
yuron 14:ab89b6cd9719 2241 true_migiusiro_data[0] = migiusiro_data[0];
yuron 14:ab89b6cd9719 2242 true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
yuron 14:ab89b6cd9719 2243 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
yuron 14:ab89b6cd9719 2244 }
yuron 14:ab89b6cd9719 2245 //左旋回(目標より行き過ぎ)
yuron 22:5682246f9409 2246 else if(sum_pulse > target) {
yuron 18:851f783ec516 2247 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2248 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2249 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2250 true_hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 2251 }
yuron 14:ab89b6cd9719 2252 }