2019NHK_teamA / Mbed 2 deprecated 2019_A_ver7-5

2019年10月02日AM11:14現在のもの(青ゾーンは変更なし)

Dependencies:   mbed QEI PID

main.cpp@25:ce789ea15628, 2019-10-02 (annotated)

Committer:
yuron
Date:
Wed Oct 02 02:13:36 2019 +0000
Revision:
25:ce789ea15628
Parent:
24:d12bc20c01c2 
aaaaaaaaaa; ;

Who changed what in which revision?

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