Yuhi Takaku / Mbed 2 deprecated 2019_A_ver8

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

Dependencies:   mbed QEI PID

main.cpp@30:2aa3d5dd117d, 2019-11-02 (annotated)

Committer:
yuron
Date:
Sat Nov 02 19:18:29 2019 +0000
Revision:
30:2aa3d5dd117d
Parent:
29:81b1ec07b5c2 
aaa

Who changed what in which revision?

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