2019NHK_teamA / Mbed 2 deprecated 2019_A_ver8-2

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@28:e95db716197d, 2019-10-23 (annotated)

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