2019NHK_teamA / Mbed 2 deprecated 2019_A_ver7-5

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

Dependencies:   mbed QEI PID

main.cpp@24:d12bc20c01c2, 2019-09-29 (annotated)

Committer:
yuron
Date:
Sun Sep 29 17:01:17 2019 +0000
Revision:
24:d12bc20c01c2
Parent:
23:1e4d7540715f
Child:
25:ce789ea15628
aaaaa

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