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@27:4f2fc7172b31, 2019-10-19 (annotated)

Committer:
yuron
Date:
Sat Oct 19 01:10:39 2019 +0000
Revision:
27:4f2fc7172b31
Parent:
26:81346a21d301
Child:
28:e95db716197d
aaa;

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