2019NHK_teamA / Mbed 2 deprecated 2019_A_ver8-2

ウオッチドッグタイマを追加したよん

Dependencies:   mbed QEI PID

main.cpp@29:81b1ec07b5c2, 2019-10-29 (annotated)

Committer:
yuron
Date:
Tue Oct 29 16:00:11 2019 +0000
Revision:
29:81b1ec07b5c2
Parent:
28:e95db716197d 
aaa

Who changed what in which revision?

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