2019NHK_teamA / Mbed 2 deprecated 2019_A_ver7-6

Dependencies:   mbed QEI PID

main.cpp@23:1e4d7540715f, 2019-09-28 (annotated)

Committer:
yuron
Date:
Sat Sep 28 01:24:03 2019 +0000
Revision:
23:1e4d7540715f
Parent:
22:5682246f9409
Child:
24:d12bc20c01c2
aaaaaaaaa;

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 22:5682246f9409 8 /* added red zone(checked) */
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 16:05b26003da50 82
yuron 17:de3bc1999ae7 83 //遠隔非常停止ユニットLED
yuron 17:de3bc1999ae7 84 AnalogOut myled(A2);
yuron 17:de3bc1999ae7 85
yuron 16:05b26003da50 86 DigitalIn start_switch(PB_12);
yuron 21:89db2a19e52e 87 DigitalIn USR_SWITCH(PC_13);
yuron 21:89db2a19e52e 88 DigitalIn zone_switch(PC_10);
yuron 8:3df97287c825 89
yuron 14:ab89b6cd9719 90 QEI wheel_x1(PA_8 , PA_6 , NC, 624);
yuron 14:ab89b6cd9719 91 QEI wheel_x2(PB_14, PB_13, NC, 624);
yuron 14:ab89b6cd9719 92 QEI wheel_y1(PB_1 , PB_15, NC, 624);
yuron 14:ab89b6cd9719 93 QEI wheel_y2(PA_12, PA_11, NC, 624);
yuron 19:f17d2e585973 94 QEI arm_enc(PB_5, PB_4 , NC, 624);
yuron 14:ab89b6cd9719 95
yuron 19:f17d2e585973 96 //移動後n秒停止タイマー
yuron 17:de3bc1999ae7 97 Timer counter;
yuron 16:05b26003da50 98
yuron 14:ab89b6cd9719 99 //エンコーダ値格納変数
yuron 23:1e4d7540715f 100 int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse, arm_pulse;
yuron 14:ab89b6cd9719 101
yuron 14:ab89b6cd9719 102 //操作の段階変数
yuron 14:ab89b6cd9719 103 unsigned int phase = 0;
yuron 19:f17d2e585973 104 int kaisyu_phase = 0;
yuron 19:f17d2e585973 105 int tyokudo_phase = 0;
yuron 16:05b26003da50 106 unsigned int start_zone = 1;
yuron 16:05b26003da50 107 bool zone = RED;
yuron 0:f73c1b076ae4 108
yuron 19:f17d2e585973 109 //i2c送信データ変数
yuron 14:ab89b6cd9719 110 char init_send_data[1];
yuron 14:ab89b6cd9719 111 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
yuron 14:ab89b6cd9719 112 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
yuron 21:89db2a19e52e 113 char arm_motor[1], drop_motor[1];
yuron 19:f17d2e585973 114 char fan_data[1];
yuron 19:f17d2e585973 115 char servo_data[1];
yuron 20:ac4954be1fe0 116 char right_arm_data[1], left_arm_data[1];
yuron 0:f73c1b076ae4 117
yuron 17:de3bc1999ae7 118 //非常停止関連変数
yuron 17:de3bc1999ae7 119 char RDATA;
yuron 17:de3bc1999ae7 120 char baff;
yuron 17:de3bc1999ae7 121 int flug = 0;
yuron 17:de3bc1999ae7 122
yuron 19:f17d2e585973 123 //リミット基板からの受信データ
yuron 18:851f783ec516 124 int limit_data = 0;
yuron 19:f17d2e585973 125 int upper_limit_data = 0;
yuron 19:f17d2e585973 126 int lower_limit_data = 0;
yuron 18:851f783ec516 127
yuron 19:f17d2e585973 128 //各辺のスイッチが押されたかのフラグ
yuron 19:f17d2e585973 129 //前部が壁に当たっているか
yuron 19:f17d2e585973 130 int front_limit = 0;
yuron 19:f17d2e585973 131 //右部が壁にあたあっているか
yuron 19:f17d2e585973 132 int right_limit = 0;
yuron 19:f17d2e585973 133 //後部が壁に当たっているか
yuron 19:f17d2e585973 134 int back_limit = 0;
yuron 20:ac4954be1fe0 135 //回収機構の下限(引っ込めてるほう)
yuron 21:89db2a19e52e 136 bool kaisyu_mae_limit = 0;
yuron 21:89db2a19e52e 137
yuron 21:89db2a19e52e 138 bool kaisyu_usiro_limit = 0;
yuron 21:89db2a19e52e 139
yuron 20:ac4954be1fe0 140 //右腕の下限
yuron 20:ac4954be1fe0 141 bool right_arm_lower_limit = 0;
yuron 19:f17d2e585973 142 //右腕の上限
yuron 19:f17d2e585973 143 bool right_arm_upper_limit = 0;
yuron 19:f17d2e585973 144 //左腕の下限
yuron 19:f17d2e585973 145 bool left_arm_lower_limit = 0;
yuron 20:ac4954be1fe0 146 //左腕の上限
yuron 20:ac4954be1fe0 147 bool left_arm_upper_limit = 0;
yuron 19:f17d2e585973 148 //吐き出し機構の上限
yuron 20:ac4954be1fe0 149 bool tyokudo_mae_limit = 0;
yuron 20:ac4954be1fe0 150 //吐き出し機構の下限
yuron 20:ac4954be1fe0 151 bool tyokudo_usiro_limit = 0;
yuron 20:ac4954be1fe0 152
yuron 21:89db2a19e52e 153 int masked_lower_front_limit_data = 0xFF;
yuron 21:89db2a19e52e 154 int masked_lower_back_limit_data = 0xFF;
yuron 21:89db2a19e52e 155 int masked_lower_right_limit_data = 0xFF;
yuron 21:89db2a19e52e 156 int masked_kaisyu_mae_limit_data = 0xFF;
yuron 21:89db2a19e52e 157 int masked_kaisyu_usiro_limit_data = 0xFF;
yuron 20:ac4954be1fe0 158 int masked_right_arm_lower_limit_data = 0xFF;
yuron 20:ac4954be1fe0 159 int masked_right_arm_upper_limit_data = 0xFF;
yuron 21:89db2a19e52e 160 int masked_left_arm_lower_limit_data = 0xFF;
yuron 21:89db2a19e52e 161 int masked_left_arm_upper_limit_data = 0xFF;
yuron 21:89db2a19e52e 162 int masked_tyokudo_mae_limit_data = 0xFF;
yuron 21:89db2a19e52e 163 int masked_tyokudo_usiro_limit_data = 0xFF;
yuron 18:851f783ec516 164
yuron 14:ab89b6cd9719 165 //関数のプロトタイプ宣言
yuron 14:ab89b6cd9719 166 void init(void);
yuron 14:ab89b6cd9719 167 void init_send(void);
yuron 17:de3bc1999ae7 168 void get(void);
yuron 14:ab89b6cd9719 169 void get_pulses(void);
yuron 14:ab89b6cd9719 170 void print_pulses(void);
yuron 17:de3bc1999ae7 171 void get_emergency(void);
yuron 18:851f783ec516 172 void read_limit(void);
yuron 19:f17d2e585973 173 void wheel_reset(void);
yuron 21:89db2a19e52e 174 void kaisyu(int pulse, int next_phase);
yuron 23:1e4d7540715f 175 void kaisyu_nobasu(int pulse, int next_phase);
yuron 23:1e4d7540715f 176 void kaisyu_hiku(int pulse, int next_phase);
yuron 21:89db2a19e52e 177 void tyokudo(int pulse, int next_phase);
yuron 21:89db2a19e52e 178 void arm_up(int next_phase);
yuron 17:de3bc1999ae7 179 void front(int target);
yuron 17:de3bc1999ae7 180 void back(int target);
yuron 17:de3bc1999ae7 181 void right(int target);
yuron 17:de3bc1999ae7 182 void left(int target);
yuron 17:de3bc1999ae7 183 void turn_right(int target);
yuron 17:de3bc1999ae7 184 void turn_left(int target);
yuron 18:851f783ec516 185 void stop(void);
yuron 22:5682246f9409 186 void all_stop(void);
yuron 17:de3bc1999ae7 187 void front_PID(int target);
yuron 17:de3bc1999ae7 188 void back_PID(int target);
yuron 17:de3bc1999ae7 189 void right_PID(int target);
yuron 17:de3bc1999ae7 190 void left_PID(int target);
yuron 17:de3bc1999ae7 191 void turn_right_PID(int target);
yuron 17:de3bc1999ae7 192 void turn_left_PID(int target);
yuron 8:3df97287c825 193
yuron 14:ab89b6cd9719 194 int main(void) {
yuron 20:ac4954be1fe0 195
yuron 14:ab89b6cd9719 196 init();
yuron 14:ab89b6cd9719 197 init_send();
yuron 20:ac4954be1fe0 198
yuron 19:f17d2e585973 199 //とりあえず(後で消してね)
yuron 23:1e4d7540715f 200 //phase = 22; //スタートゾーン2から
yuron 23:1e4d7540715f 201 //phase = 36; //アームアップ
yuron 23:1e4d7540715f 202 //phase = 50; //default
yuron 21:89db2a19e52e 203
yuron 21:89db2a19e52e 204 //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
yuron 21:89db2a19e52e 205 while(1) {
yuron 21:89db2a19e52e 206 if(zone_switch == 0) {
yuron 21:89db2a19e52e 207 zone = BLUE;
yuron 21:89db2a19e52e 208 } else {
yuron 21:89db2a19e52e 209 zone = RED;
yuron 21:89db2a19e52e 210 }
yuron 21:89db2a19e52e 211 break;
yuron 21:89db2a19e52e 212 }
yuron 21:89db2a19e52e 213
yuron 14:ab89b6cd9719 214 while(1) {
yuron 20:ac4954be1fe0 215
yuron 14:ab89b6cd9719 216 get_pulses();
yuron 23:1e4d7540715f 217 //print_pulses();
yuron 17:de3bc1999ae7 218 get_emergency();
yuron 18:851f783ec516 219 read_limit();
yuron 21:89db2a19e52e 220
yuron 21:89db2a19e52e 221 //move_servo_with_using_onboard-switch
yuron 21:89db2a19e52e 222 if(USR_SWITCH == 0) {
yuron 21:89db2a19e52e 223 servo_data[0] = 0x03;
yuron 21:89db2a19e52e 224 i2c.write(0x30, servo_data, 1);
yuron 21:89db2a19e52e 225 } else {
yuron 21:89db2a19e52e 226 servo_data[0] = 0x04;
yuron 21:89db2a19e52e 227 i2c.write(0x30, servo_data, 1);
yuron 21:89db2a19e52e 228 }
yuron 19:f17d2e585973 229
yuron 23:1e4d7540715f 230 /*
yuron 21:89db2a19e52e 231 if(start_switch == 1) {
yuron 23:1e4d7540715f 232 phase = 39;
yuron 21:89db2a19e52e 233 }
yuron 23:1e4d7540715f 234 */
yuron 22:5682246f9409 235
yuron 19:f17d2e585973 236 //青ゾーン
yuron 18:851f783ec516 237 if(zone == BLUE) {
yuron 21:89db2a19e52e 238 GREEN_LED = 1;
yuron 21:89db2a19e52e 239 RED_LED = 0;
yuron 23:1e4d7540715f 240
yuron 18:851f783ec516 241 switch(phase) {
yuron 20:ac4954be1fe0 242
yuron 19:f17d2e585973 243 //スタート位置へセット
yuron 18:851f783ec516 244 case 0:
yuron 19:f17d2e585973 245 //リミットが洗濯物台に触れているか
yuron 19:f17d2e585973 246 if(right_limit == 3) {
yuron 19:f17d2e585973 247 USR_LED1 = 1;
yuron 19:f17d2e585973 248 //スタートスイッチが押されたか
yuron 19:f17d2e585973 249 if(start_switch == 1) {
yuron 19:f17d2e585973 250 wheel_reset();
yuron 19:f17d2e585973 251 phase = 1;
yuron 19:f17d2e585973 252 }
yuron 19:f17d2e585973 253 } else {
yuron 19:f17d2e585973 254 USR_LED1 = 0;
yuron 18:851f783ec516 255 }
yuron 18:851f783ec516 256 break;
yuron 20:ac4954be1fe0 257
yuron 23:1e4d7540715f 258 //回収アームを伸ばす
yuron 22:5682246f9409 259 case 1:
yuron 23:1e4d7540715f 260 counter.reset();
yuron 23:1e4d7540715f 261 //kaisyu(arm_enc.getPulses(), 2);
yuron 23:1e4d7540715f 262 kaisyu_nobasu(arm_pulse, 2);
yuron 23:1e4d7540715f 263 //サーボを開いておく
yuron 20:ac4954be1fe0 264 servo_data[0] = 0x03;
yuron 20:ac4954be1fe0 265 i2c.write(0x30, servo_data, 1);
yuron 19:f17d2e585973 266 break;
yuron 20:ac4954be1fe0 267
yuron 19:f17d2e585973 268 //1秒停止
yuron 18:851f783ec516 269 case 2:
yuron 18:851f783ec516 270 stop();
yuron 20:ac4954be1fe0 271 servo_data[0] = 0x04;
yuron 20:ac4954be1fe0 272 i2c.write(0x30, servo_data, 1);
yuron 18:851f783ec516 273 counter.start();
yuron 18:851f783ec516 274 if(counter.read() > 1.0f) {
yuron 18:851f783ec516 275 phase = 3;
yuron 19:f17d2e585973 276 wheel_reset();
yuron 18:851f783ec516 277 }
yuron 18:851f783ec516 278 break;
yuron 20:ac4954be1fe0 279
yuron 23:1e4d7540715f 280 //ちょっと後進
yuron 18:851f783ec516 281 case 3:
yuron 18:851f783ec516 282 counter.reset();
yuron 23:1e4d7540715f 283 back(-800);
yuron 23:1e4d7540715f 284 if((y_pulse1*-1 > 800) || (y_pulse2*-1 > 800)) {
yuron 18:851f783ec516 285 phase = 4;
yuron 18:851f783ec516 286 }
yuron 18:851f783ec516 287 break;
yuron 20:ac4954be1fe0 288
yuron 19:f17d2e585973 289 //1秒停止
yuron 18:851f783ec516 290 case 4:
yuron 18:851f783ec516 291 stop();
yuron 18:851f783ec516 292 counter.start();
yuron 18:851f783ec516 293 if(counter.read() > 1.0f) {
yuron 19:f17d2e585973 294 phase = 5;
yuron 19:f17d2e585973 295 wheel_reset();
yuron 18:851f783ec516 296 }
yuron 18:851f783ec516 297 break;
yuron 20:ac4954be1fe0 298
yuron 23:1e4d7540715f 299 //回収アーム引っ込める
yuron 18:851f783ec516 300 case 5:
yuron 18:851f783ec516 301 counter.reset();
yuron 23:1e4d7540715f 302 kaisyu_hiku(arm_pulse, 6);
yuron 18:851f783ec516 303 break;
yuron 20:ac4954be1fe0 304
yuron 19:f17d2e585973 305 //1秒停止
yuron 18:851f783ec516 306 case 6:
yuron 18:851f783ec516 307 stop();
yuron 18:851f783ec516 308 counter.start();
yuron 18:851f783ec516 309 if(counter.read() > 1.0f) {
yuron 18:851f783ec516 310 phase = 7;
yuron 19:f17d2e585973 311 wheel_reset();
yuron 18:851f783ec516 312 }
yuron 18:851f783ec516 313 break;
yuron 20:ac4954be1fe0 314
yuron 23:1e4d7540715f 315 //左移動
yuron 18:851f783ec516 316 case 7:
yuron 23:1e4d7540715f 317 counter.reset();
yuron 23:1e4d7540715f 318 left(11500);
yuron 23:1e4d7540715f 319 if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
yuron 18:851f783ec516 320 phase = 8;
yuron 18:851f783ec516 321 }
yuron 18:851f783ec516 322 break;
yuron 20:ac4954be1fe0 323
yuron 19:f17d2e585973 324 //1秒停止
yuron 18:851f783ec516 325 case 8:
yuron 18:851f783ec516 326 stop();
yuron 18:851f783ec516 327 counter.start();
yuron 18:851f783ec516 328 if(counter.read() > 1.0f) {
yuron 18:851f783ec516 329 phase = 9;
yuron 19:f17d2e585973 330 wheel_reset();
yuron 18:851f783ec516 331 }
yuron 18:851f783ec516 332 break;
yuron 20:ac4954be1fe0 333
yuron 23:1e4d7540715f 334 //右旋回(180°)
yuron 18:851f783ec516 335 case 9:
yuron 18:851f783ec516 336 counter.reset();
yuron 23:1e4d7540715f 337 turn_right(975);
yuron 23:1e4d7540715f 338 if(sum_pulse > 975) {
yuron 23:1e4d7540715f 339 phase = 10;
yuron 23:1e4d7540715f 340 }
yuron 19:f17d2e585973 341 break;
yuron 20:ac4954be1fe0 342
yuron 19:f17d2e585973 343 //1秒停止
yuron 21:89db2a19e52e 344 case 10:
yuron 19:f17d2e585973 345 stop();
yuron 19:f17d2e585973 346 counter.start();
yuron 19:f17d2e585973 347 if(counter.read() > 1.0f) {
yuron 21:89db2a19e52e 348 phase = 11;
yuron 19:f17d2e585973 349 wheel_reset();
yuron 18:851f783ec516 350 }
yuron 18:851f783ec516 351 break;
yuron 20:ac4954be1fe0 352
yuron 23:1e4d7540715f 353 //壁に当たるまで後進
yuron 21:89db2a19e52e 354 case 11:
yuron 19:f17d2e585973 355 counter.reset();
yuron 23:1e4d7540715f 356
yuron 23:1e4d7540715f 357 if(back_limit == 3) {
yuron 21:89db2a19e52e 358 phase = 12;
yuron 19:f17d2e585973 359 }
yuron 23:1e4d7540715f 360 else if(back_limit != 3){
yuron 23:1e4d7540715f 361 true_migimae_data[0] = 0x50;
yuron 23:1e4d7540715f 362 true_migiusiro_data[0] = 0x50;
yuron 23:1e4d7540715f 363 true_hidarimae_data[0] = 0x50;
yuron 23:1e4d7540715f 364 true_hidariusiro_data[0] = 0x50;
yuron 23:1e4d7540715f 365 i2c.write(0x10, true_migimae_data, 1, false);
yuron 23:1e4d7540715f 366 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 23:1e4d7540715f 367 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 23:1e4d7540715f 368 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 369 wait_us(20);
yuron 23:1e4d7540715f 370 }
yuron 19:f17d2e585973 371 break;
yuron 20:ac4954be1fe0 372
yuron 19:f17d2e585973 373 //1秒停止
yuron 21:89db2a19e52e 374 case 12:
yuron 18:851f783ec516 375 stop();
yuron 18:851f783ec516 376 counter.start();
yuron 18:851f783ec516 377 if(counter.read() > 1.0f) {
yuron 21:89db2a19e52e 378 phase = 13;
yuron 19:f17d2e585973 379 wheel_reset();
yuron 19:f17d2e585973 380 }
yuron 19:f17d2e585973 381 break;
yuron 20:ac4954be1fe0 382
yuron 22:5682246f9409 383 //壁に当たるまで右移動
yuron 21:89db2a19e52e 384 case 13:
yuron 19:f17d2e585973 385 counter.reset();
yuron 23:1e4d7540715f 386
yuron 21:89db2a19e52e 387 if(right_limit == 3) {
yuron 21:89db2a19e52e 388 phase = 14;
yuron 19:f17d2e585973 389 }
yuron 22:5682246f9409 390 else if(right_limit != 3) {
yuron 22:5682246f9409 391 true_migimae_data[0] = 0x40;
yuron 22:5682246f9409 392 true_migiusiro_data[0] = 0xBF;
yuron 22:5682246f9409 393 true_hidarimae_data[0] = 0xBF;
yuron 22:5682246f9409 394 true_hidariusiro_data[0] = 0x40;
yuron 22:5682246f9409 395 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 396 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 397 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 398 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 399 wait_us(20);
yuron 22:5682246f9409 400 }
yuron 21:89db2a19e52e 401 break;
yuron 23:1e4d7540715f 402
yuron 21:89db2a19e52e 403 //1秒停止
yuron 21:89db2a19e52e 404 case 14:
yuron 21:89db2a19e52e 405 stop();
yuron 21:89db2a19e52e 406 counter.start();
yuron 21:89db2a19e52e 407 if(counter.read() > 1.0f) {
yuron 22:5682246f9409 408 phase = 15;
yuron 22:5682246f9409 409 wheel_reset();
yuron 22:5682246f9409 410 }
yuron 22:5682246f9409 411 break;
yuron 23:1e4d7540715f 412
yuron 23:1e4d7540715f 413 //排出
yuron 22:5682246f9409 414 case 15:
yuron 22:5682246f9409 415 counter.reset();
yuron 23:1e4d7540715f 416 tyokudo(arm_enc.getPulses(), 16);
yuron 23:1e4d7540715f 417 break;
yuron 23:1e4d7540715f 418
yuron 23:1e4d7540715f 419 //1秒停止
yuron 23:1e4d7540715f 420 case 16:
yuron 23:1e4d7540715f 421 stop();
yuron 23:1e4d7540715f 422 counter.start();
yuron 23:1e4d7540715f 423 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 424 phase = 17;
yuron 23:1e4d7540715f 425 wheel_reset();
yuron 23:1e4d7540715f 426 }
yuron 23:1e4d7540715f 427 break;
yuron 23:1e4d7540715f 428
yuron 23:1e4d7540715f 429 //前進
yuron 23:1e4d7540715f 430 case 17:
yuron 23:1e4d7540715f 431 counter.reset();
yuron 23:1e4d7540715f 432 front(5000);
yuron 23:1e4d7540715f 433 if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
yuron 23:1e4d7540715f 434 phase = 18;
yuron 23:1e4d7540715f 435 }
yuron 23:1e4d7540715f 436 break;
yuron 23:1e4d7540715f 437
yuron 23:1e4d7540715f 438 //1秒停止
yuron 23:1e4d7540715f 439 case 18:
yuron 23:1e4d7540715f 440 stop();
yuron 23:1e4d7540715f 441 counter.start();
yuron 23:1e4d7540715f 442 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 443 phase = 19;
yuron 23:1e4d7540715f 444 wheel_reset();
yuron 23:1e4d7540715f 445 }
yuron 23:1e4d7540715f 446 break;
yuron 23:1e4d7540715f 447
yuron 23:1e4d7540715f 448 //壁に当たるまで右移動
yuron 23:1e4d7540715f 449 case 19:
yuron 23:1e4d7540715f 450 counter.reset();
yuron 23:1e4d7540715f 451
yuron 23:1e4d7540715f 452 if(right_limit == 3) {
yuron 23:1e4d7540715f 453 phase = 20;
yuron 23:1e4d7540715f 454 }
yuron 23:1e4d7540715f 455 else if(right_limit != 3) {
yuron 23:1e4d7540715f 456 true_migimae_data[0] = 0x40;
yuron 23:1e4d7540715f 457 true_migiusiro_data[0] = 0xBF;
yuron 23:1e4d7540715f 458 true_hidarimae_data[0] = 0xBF;
yuron 23:1e4d7540715f 459 true_hidariusiro_data[0] = 0x40;
yuron 23:1e4d7540715f 460 i2c.write(0x10, true_migimae_data, 1, false);
yuron 23:1e4d7540715f 461 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 23:1e4d7540715f 462 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 23:1e4d7540715f 463 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 464 wait_us(20);
yuron 23:1e4d7540715f 465 }
yuron 23:1e4d7540715f 466 break;
yuron 23:1e4d7540715f 467
yuron 23:1e4d7540715f 468 //1秒停止
yuron 23:1e4d7540715f 469 case 20:
yuron 23:1e4d7540715f 470 stop();
yuron 23:1e4d7540715f 471 counter.start();
yuron 23:1e4d7540715f 472 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 473 phase = 21;
yuron 23:1e4d7540715f 474 wheel_reset();
yuron 23:1e4d7540715f 475 }
yuron 23:1e4d7540715f 476 break;
yuron 23:1e4d7540715f 477
yuron 23:1e4d7540715f 478 //壁に当たるまで後進
yuron 23:1e4d7540715f 479 case 21:
yuron 23:1e4d7540715f 480 counter.reset();
yuron 23:1e4d7540715f 481
yuron 22:5682246f9409 482 if(back_limit == 3) {
yuron 23:1e4d7540715f 483 phase = 22;
yuron 22:5682246f9409 484 }
yuron 22:5682246f9409 485 else if(back_limit != 3){
yuron 22:5682246f9409 486 true_migimae_data[0] = 0x50;
yuron 22:5682246f9409 487 true_migiusiro_data[0] = 0x50;
yuron 22:5682246f9409 488 true_hidarimae_data[0] = 0x50;
yuron 22:5682246f9409 489 true_hidariusiro_data[0] = 0x50;
yuron 22:5682246f9409 490 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 491 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 492 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 493 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 494 wait_us(20);
yuron 18:851f783ec516 495 }
yuron 18:851f783ec516 496 break;
yuron 20:ac4954be1fe0 497
yuron 21:89db2a19e52e 498 //シーツ装填
yuron 23:1e4d7540715f 499 case 22:
yuron 19:f17d2e585973 500 if(start_switch == 1) {
yuron 21:89db2a19e52e 501 wheel_reset();
yuron 23:1e4d7540715f 502 phase = 23;
yuron 19:f17d2e585973 503 } else {
yuron 19:f17d2e585973 504 stop();
yuron 19:f17d2e585973 505 }
yuron 19:f17d2e585973 506 break;
yuron 20:ac4954be1fe0 507
yuron 19:f17d2e585973 508 //竿のラインまで前進
yuron 23:1e4d7540715f 509 case 23:
yuron 18:851f783ec516 510 counter.reset();
yuron 23:1e4d7540715f 511 front(20500);
yuron 23:1e4d7540715f 512 if((y_pulse1 > 20500) || (y_pulse2 > 20500)) {
yuron 23:1e4d7540715f 513 phase = 24;
yuron 23:1e4d7540715f 514 }
yuron 23:1e4d7540715f 515 break;
yuron 23:1e4d7540715f 516
yuron 23:1e4d7540715f 517 //1秒停止
yuron 23:1e4d7540715f 518 case 24:
yuron 23:1e4d7540715f 519 stop();
yuron 23:1e4d7540715f 520 counter.start();
yuron 23:1e4d7540715f 521 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 522 phase = 25;
yuron 23:1e4d7540715f 523 wheel_reset();
yuron 23:1e4d7540715f 524 }
yuron 23:1e4d7540715f 525 break;
yuron 23:1e4d7540715f 526
yuron 23:1e4d7540715f 527 //ちょっと左移動
yuron 23:1e4d7540715f 528 case 25:
yuron 23:1e4d7540715f 529 counter.reset();
yuron 23:1e4d7540715f 530 left(500);
yuron 23:1e4d7540715f 531 if((x_pulse1 > 500) || (x_pulse2 > 500)) {
yuron 23:1e4d7540715f 532 phase = 26;
yuron 18:851f783ec516 533 }
yuron 18:851f783ec516 534 break;
yuron 20:ac4954be1fe0 535
yuron 19:f17d2e585973 536 //1秒停止
yuron 23:1e4d7540715f 537 case 26:
yuron 18:851f783ec516 538 stop();
yuron 18:851f783ec516 539 counter.start();
yuron 18:851f783ec516 540 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 541 phase = 27;
yuron 19:f17d2e585973 542 wheel_reset();
yuron 19:f17d2e585973 543 }
yuron 20:ac4954be1fe0 544 break;
yuron 23:1e4d7540715f 545
yuron 23:1e4d7540715f 546 //90°右旋回
yuron 23:1e4d7540715f 547 case 27:
yuron 19:f17d2e585973 548 counter.reset();
yuron 22:5682246f9409 549 turn_right(465);
yuron 22:5682246f9409 550 if(sum_pulse > 465) {
yuron 23:1e4d7540715f 551 phase = 28;
yuron 18:851f783ec516 552 }
yuron 18:851f783ec516 553 break;
yuron 23:1e4d7540715f 554
yuron 19:f17d2e585973 555 //1秒停止
yuron 23:1e4d7540715f 556 case 28:
yuron 19:f17d2e585973 557 stop();
yuron 19:f17d2e585973 558 counter.start();
yuron 19:f17d2e585973 559 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 560 phase = 29;
yuron 19:f17d2e585973 561 wheel_reset();
yuron 19:f17d2e585973 562 }
yuron 20:ac4954be1fe0 563 break;
yuron 23:1e4d7540715f 564
yuron 23:1e4d7540715f 565 //壁に当たるまで前進
yuron 23:1e4d7540715f 566 case 29:
yuron 21:89db2a19e52e 567 counter.reset();
yuron 22:5682246f9409 568 if(front_limit == 3) {
yuron 23:1e4d7540715f 569 phase = 30;
yuron 23:1e4d7540715f 570 }
yuron 22:5682246f9409 571 else if(front_limit != 3){
yuron 22:5682246f9409 572 true_migimae_data[0] = 0xC0;
yuron 22:5682246f9409 573 true_migiusiro_data[0] = 0xC0;
yuron 22:5682246f9409 574 true_hidarimae_data[0] = 0xC0;
yuron 22:5682246f9409 575 true_hidariusiro_data[0] = 0xC0;
yuron 22:5682246f9409 576 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 577 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 578 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 579 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 580 wait_us(20);
yuron 22:5682246f9409 581 }
yuron 22:5682246f9409 582 break;
yuron 23:1e4d7540715f 583
yuron 22:5682246f9409 584 //1秒停止
yuron 23:1e4d7540715f 585 case 30:
yuron 22:5682246f9409 586 stop();
yuron 22:5682246f9409 587 counter.start();
yuron 22:5682246f9409 588 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 589 phase = 31;
yuron 22:5682246f9409 590 wheel_reset();
yuron 22:5682246f9409 591 }
yuron 22:5682246f9409 592 break;
yuron 23:1e4d7540715f 593
yuron 22:5682246f9409 594 //掛けるところまで後進
yuron 23:1e4d7540715f 595 case 31:
yuron 22:5682246f9409 596 counter.reset();
yuron 22:5682246f9409 597 back(-10000);
yuron 22:5682246f9409 598 if((y_pulse1*-1 > 10000) || (y_pulse2*-1 > 10000)) {
yuron 23:1e4d7540715f 599 phase = 32;
yuron 22:5682246f9409 600 counter.start();
yuron 22:5682246f9409 601 }
yuron 22:5682246f9409 602 break;
yuron 23:1e4d7540715f 603
yuron 22:5682246f9409 604 //1秒停止
yuron 23:1e4d7540715f 605 case 32:
yuron 22:5682246f9409 606 stop();
yuron 22:5682246f9409 607 counter.start();
yuron 22:5682246f9409 608 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 609 phase = 33;
yuron 22:5682246f9409 610 wheel_reset();
yuron 22:5682246f9409 611 }
yuron 22:5682246f9409 612 break;
yuron 22:5682246f9409 613
yuron 22:5682246f9409 614 //妨害防止の右旋回
yuron 23:1e4d7540715f 615 case 33:
yuron 22:5682246f9409 616 counter.reset();
yuron 22:5682246f9409 617 turn_right(30);
yuron 22:5682246f9409 618 if(sum_pulse > 30) {
yuron 23:1e4d7540715f 619 phase = 34;
yuron 22:5682246f9409 620 }
yuron 22:5682246f9409 621 break;
yuron 22:5682246f9409 622
yuron 22:5682246f9409 623 //1秒停止
yuron 23:1e4d7540715f 624 case 34:
yuron 22:5682246f9409 625 stop();
yuron 22:5682246f9409 626 counter.start();
yuron 22:5682246f9409 627 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 628 phase = 35;
yuron 22:5682246f9409 629 wheel_reset();
yuron 22:5682246f9409 630 }
yuron 22:5682246f9409 631 break;
yuron 22:5682246f9409 632
yuron 22:5682246f9409 633 //カウンターリセット
yuron 23:1e4d7540715f 634 case 35:
yuron 22:5682246f9409 635 counter.reset();
yuron 22:5682246f9409 636 counter.start();
yuron 23:1e4d7540715f 637 phase = 36;
yuron 22:5682246f9409 638 break;
yuron 23:1e4d7540715f 639
yuron 20:ac4954be1fe0 640 //アームアップ
yuron 23:1e4d7540715f 641 case 36:
yuron 18:851f783ec516 642 stop();
yuron 22:5682246f9409 643 //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
yuron 23:1e4d7540715f 644 if(counter.read() < 1.0f) {
yuron 21:89db2a19e52e 645 right_arm_data[0] = 0xFF;
yuron 21:89db2a19e52e 646 left_arm_data[0] = 0xFF;
yuron 21:89db2a19e52e 647 i2c.write(0x22, right_arm_data, 1);
yuron 21:89db2a19e52e 648 i2c.write(0x24, left_arm_data, 1);
yuron 21:89db2a19e52e 649 wait_us(20);
yuron 21:89db2a19e52e 650 } else {
yuron 23:1e4d7540715f 651 arm_up(37);
yuron 21:89db2a19e52e 652 }
yuron 21:89db2a19e52e 653 break;
yuron 23:1e4d7540715f 654
yuron 21:89db2a19e52e 655 //カウンターリセット
yuron 23:1e4d7540715f 656 case 37:
yuron 19:f17d2e585973 657 counter.reset();
yuron 23:1e4d7540715f 658 phase = 38;
yuron 22:5682246f9409 659 break;
yuron 23:1e4d7540715f 660
yuron 19:f17d2e585973 661 //シーツを掛ける
yuron 23:1e4d7540715f 662 case 38:
yuron 19:f17d2e585973 663 counter.start();
yuron 23:1e4d7540715f 664
yuron 21:89db2a19e52e 665 //1秒間ファン送風
yuron 23:1e4d7540715f 666 if(counter.read() <= 2.0f) {
yuron 19:f17d2e585973 667 fan_data[0] = 0xFF;
yuron 19:f17d2e585973 668 i2c.write(0x26, fan_data, 1);
yuron 21:89db2a19e52e 669 i2c.write(0x28, fan_data, 1);
yuron 19:f17d2e585973 670 servo_data[0] = 0x04;
yuron 19:f17d2e585973 671 i2c.write(0x30, servo_data, 1);
yuron 19:f17d2e585973 672 }
yuron 22:5682246f9409 673 //1~3秒の間でサーボを開放
yuron 23:1e4d7540715f 674 else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
yuron 19:f17d2e585973 675 fan_data[0] = 0xFF;
yuron 19:f17d2e585973 676 i2c.write(0x26, fan_data, 1);
yuron 21:89db2a19e52e 677 i2c.write(0x28, fan_data, 1);
yuron 19:f17d2e585973 678 servo_data[0] = 0x03;
yuron 19:f17d2e585973 679 i2c.write(0x30, servo_data, 1);
yuron 19:f17d2e585973 680 }
yuron 21:89db2a19e52e 681 //3秒過ぎたら終わり
yuron 23:1e4d7540715f 682 else if(counter.read() > 4.0f) {
yuron 19:f17d2e585973 683 fan_data[0] = 0x80;
yuron 19:f17d2e585973 684 i2c.write(0x26, fan_data, 1);
yuron 21:89db2a19e52e 685 i2c.write(0x28, fan_data, 1);
yuron 19:f17d2e585973 686 servo_data[0] = 0x04;
yuron 19:f17d2e585973 687 i2c.write(0x30, servo_data, 1);
yuron 23:1e4d7540715f 688 phase = 39;
yuron 19:f17d2e585973 689 }
yuron 19:f17d2e585973 690 break;
yuron 20:ac4954be1fe0 691
yuron 19:f17d2e585973 692 //終了っ！(守衛さん風)
yuron 23:1e4d7540715f 693 case 39:
yuron 18:851f783ec516 694 default:
yuron 19:f17d2e585973 695 //駆動系統OFF
yuron 22:5682246f9409 696 all_stop();
yuron 18:851f783ec516 697 break;
yuron 18:851f783ec516 698 }
yuron 16:05b26003da50 699 }
yuron 21:89db2a19e52e 700
yuron 21:89db2a19e52e 701 //REDゾーン
yuron 21:89db2a19e52e 702 else if(zone == RED) {
yuron 21:89db2a19e52e 703 GREEN_LED = 0;
yuron 21:89db2a19e52e 704 RED_LED = 1;
yuron 22:5682246f9409 705
yuron 22:5682246f9409 706 switch(phase) {
yuron 22:5682246f9409 707
yuron 22:5682246f9409 708 //スタート位置へセット
yuron 22:5682246f9409 709 case 0:
yuron 22:5682246f9409 710 //リミットが洗濯物台に触れているか
yuron 22:5682246f9409 711 if(right_limit == 3) {
yuron 22:5682246f9409 712 USR_LED1 = 1;
yuron 22:5682246f9409 713 //スタートスイッチが押されたか
yuron 22:5682246f9409 714 if(start_switch == 1) {
yuron 22:5682246f9409 715 wheel_reset();
yuron 22:5682246f9409 716 phase = 1;
yuron 22:5682246f9409 717 }
yuron 22:5682246f9409 718 } else {
yuron 22:5682246f9409 719 USR_LED1 = 0;
yuron 22:5682246f9409 720 }
yuron 22:5682246f9409 721 break;
yuron 22:5682246f9409 722
yuron 23:1e4d7540715f 723 //回収アームを伸ばす
yuron 22:5682246f9409 724 case 1:
yuron 23:1e4d7540715f 725 counter.reset();
yuron 23:1e4d7540715f 726 //kaisyu(arm_enc.getPulses(), 2);
yuron 23:1e4d7540715f 727 kaisyu_nobasu(arm_pulse, 2);
yuron 23:1e4d7540715f 728 //サーボを開いておく
yuron 22:5682246f9409 729 servo_data[0] = 0x03;
yuron 22:5682246f9409 730 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 731 break;
yuron 22:5682246f9409 732
yuron 22:5682246f9409 733 //1秒停止
yuron 22:5682246f9409 734 case 2:
yuron 22:5682246f9409 735 stop();
yuron 22:5682246f9409 736 servo_data[0] = 0x04;
yuron 22:5682246f9409 737 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 738 counter.start();
yuron 22:5682246f9409 739 if(counter.read() > 1.0f) {
yuron 22:5682246f9409 740 phase = 3;
yuron 22:5682246f9409 741 wheel_reset();
yuron 22:5682246f9409 742 }
yuron 22:5682246f9409 743 break;
yuron 22:5682246f9409 744
yuron 23:1e4d7540715f 745 //ちょっと前進
yuron 22:5682246f9409 746 case 3:
yuron 22:5682246f9409 747 counter.reset();
yuron 23:1e4d7540715f 748 front(800);
yuron 23:1e4d7540715f 749 if((y_pulse1 > 800) || (y_pulse2 > 800)) {
yuron 22:5682246f9409 750 phase = 4;
yuron 22:5682246f9409 751 }
yuron 22:5682246f9409 752 break;
yuron 23:1e4d7540715f 753
yuron 22:5682246f9409 754 //1秒停止
yuron 22:5682246f9409 755 case 4:
yuron 22:5682246f9409 756 stop();
yuron 22:5682246f9409 757 counter.start();
yuron 22:5682246f9409 758 if(counter.read() > 1.0f) {
yuron 22:5682246f9409 759 phase = 5;
yuron 22:5682246f9409 760 wheel_reset();
yuron 22:5682246f9409 761 }
yuron 22:5682246f9409 762 break;
yuron 23:1e4d7540715f 763
yuron 23:1e4d7540715f 764 //回収アーム引っ込める
yuron 22:5682246f9409 765 case 5:
yuron 22:5682246f9409 766 counter.reset();
yuron 23:1e4d7540715f 767 kaisyu_hiku(arm_pulse, 6);
yuron 22:5682246f9409 768 break;
yuron 23:1e4d7540715f 769
yuron 22:5682246f9409 770 //1秒停止
yuron 22:5682246f9409 771 case 6:
yuron 22:5682246f9409 772 stop();
yuron 22:5682246f9409 773 counter.start();
yuron 22:5682246f9409 774 if(counter.read() > 1.0f) {
yuron 22:5682246f9409 775 phase = 7;
yuron 22:5682246f9409 776 wheel_reset();
yuron 22:5682246f9409 777 }
yuron 23:1e4d7540715f 778 break;
yuron 23:1e4d7540715f 779
yuron 23:1e4d7540715f 780 //左移動
yuron 23:1e4d7540715f 781 case 7:
yuron 23:1e4d7540715f 782 counter.reset();
yuron 23:1e4d7540715f 783 left(11500);
yuron 23:1e4d7540715f 784 if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
yuron 23:1e4d7540715f 785 phase = 8;
yuron 23:1e4d7540715f 786 }
yuron 23:1e4d7540715f 787 break;
yuron 23:1e4d7540715f 788
yuron 23:1e4d7540715f 789 //1秒停止
yuron 23:1e4d7540715f 790 case 8:
yuron 23:1e4d7540715f 791 stop();
yuron 23:1e4d7540715f 792 counter.start();
yuron 23:1e4d7540715f 793 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 794 phase = 9;
yuron 23:1e4d7540715f 795 wheel_reset();
yuron 23:1e4d7540715f 796 }
yuron 23:1e4d7540715f 797 break;
yuron 23:1e4d7540715f 798
yuron 23:1e4d7540715f 799 //右旋回(180°)
yuron 23:1e4d7540715f 800 case 9:
yuron 23:1e4d7540715f 801 counter.reset();
yuron 23:1e4d7540715f 802 turn_right(975);
yuron 23:1e4d7540715f 803 if(sum_pulse > 975) {
yuron 23:1e4d7540715f 804 phase = 10;
yuron 23:1e4d7540715f 805 }
yuron 23:1e4d7540715f 806 break;
yuron 23:1e4d7540715f 807
yuron 23:1e4d7540715f 808 //1秒停止
yuron 23:1e4d7540715f 809 case 10:
yuron 23:1e4d7540715f 810 stop();
yuron 23:1e4d7540715f 811 counter.start();
yuron 23:1e4d7540715f 812 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 813 phase = 11;
yuron 23:1e4d7540715f 814 wheel_reset();
yuron 23:1e4d7540715f 815 }
yuron 23:1e4d7540715f 816 break;
yuron 23:1e4d7540715f 817
yuron 23:1e4d7540715f 818 //壁に当たるまで前進
yuron 23:1e4d7540715f 819 case 11:
yuron 23:1e4d7540715f 820 counter.reset();
yuron 23:1e4d7540715f 821
yuron 23:1e4d7540715f 822 if(front_limit == 3) {
yuron 23:1e4d7540715f 823 phase = 12;
yuron 23:1e4d7540715f 824 }
yuron 23:1e4d7540715f 825 else if(front_limit != 3){
yuron 23:1e4d7540715f 826 true_migimae_data[0] = 0xC0;
yuron 23:1e4d7540715f 827 true_migiusiro_data[0] = 0xC0;
yuron 23:1e4d7540715f 828 true_hidarimae_data[0] = 0xC0;
yuron 23:1e4d7540715f 829 true_hidariusiro_data[0] = 0xC0;
yuron 23:1e4d7540715f 830 i2c.write(0x10, true_migimae_data, 1, false);
yuron 23:1e4d7540715f 831 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 23:1e4d7540715f 832 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 23:1e4d7540715f 833 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 23:1e4d7540715f 834 wait_us(20);
yuron 23:1e4d7540715f 835 }
yuron 23:1e4d7540715f 836 break;
yuron 23:1e4d7540715f 837
yuron 23:1e4d7540715f 838 //1秒停止
yuron 23:1e4d7540715f 839 case 12:
yuron 23:1e4d7540715f 840 stop();
yuron 23:1e4d7540715f 841 counter.start();
yuron 23:1e4d7540715f 842 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 843 phase = 13;
yuron 23:1e4d7540715f 844 wheel_reset();
yuron 23:1e4d7540715f 845 }
yuron 22:5682246f9409 846 break;
yuron 22:5682246f9409 847
yuron 22:5682246f9409 848 //壁に当たるまで右移動
yuron 23:1e4d7540715f 849 case 13:
yuron 22:5682246f9409 850 counter.reset();
yuron 22:5682246f9409 851
yuron 22:5682246f9409 852 if(right_limit == 3) {
yuron 23:1e4d7540715f 853 phase = 14;
yuron 22:5682246f9409 854 }
yuron 22:5682246f9409 855 else if(right_limit != 3) {
yuron 22:5682246f9409 856 true_migimae_data[0] = 0x40;
yuron 22:5682246f9409 857 true_migiusiro_data[0] = 0xBF;
yuron 22:5682246f9409 858 true_hidarimae_data[0] = 0xBF;
yuron 22:5682246f9409 859 true_hidariusiro_data[0] = 0x40;
yuron 22:5682246f9409 860 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 861 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 862 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 863 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 864 wait_us(20);
yuron 22:5682246f9409 865 }
yuron 22:5682246f9409 866 break;
yuron 22:5682246f9409 867
yuron 22:5682246f9409 868 //1秒停止
yuron 23:1e4d7540715f 869 case 14:
yuron 22:5682246f9409 870 stop();
yuron 22:5682246f9409 871 counter.start();
yuron 22:5682246f9409 872 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 873 phase = 15;
yuron 22:5682246f9409 874 wheel_reset();
yuron 22:5682246f9409 875 }
yuron 22:5682246f9409 876 break;
yuron 22:5682246f9409 877
yuron 22:5682246f9409 878 //排出
yuron 23:1e4d7540715f 879 case 15:
yuron 22:5682246f9409 880 counter.reset();
yuron 23:1e4d7540715f 881 tyokudo(arm_enc.getPulses(), 16);
yuron 22:5682246f9409 882 break;
yuron 22:5682246f9409 883
yuron 22:5682246f9409 884 //1秒停止
yuron 23:1e4d7540715f 885 case 16:
yuron 22:5682246f9409 886 stop();
yuron 22:5682246f9409 887 counter.start();
yuron 22:5682246f9409 888 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 889 phase = 17;
yuron 22:5682246f9409 890 wheel_reset();
yuron 22:5682246f9409 891 }
yuron 22:5682246f9409 892 break;
yuron 22:5682246f9409 893
yuron 22:5682246f9409 894 //後進
yuron 23:1e4d7540715f 895 case 17:
yuron 22:5682246f9409 896 counter.reset();
yuron 22:5682246f9409 897 back(-5000);
yuron 22:5682246f9409 898 if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
yuron 23:1e4d7540715f 899 phase = 18;
yuron 22:5682246f9409 900 }
yuron 22:5682246f9409 901 break;
yuron 22:5682246f9409 902
yuron 22:5682246f9409 903 //1秒停止
yuron 23:1e4d7540715f 904 case 18:
yuron 22:5682246f9409 905 stop();
yuron 22:5682246f9409 906 counter.start();
yuron 22:5682246f9409 907 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 908 phase = 19;
yuron 22:5682246f9409 909 wheel_reset();
yuron 22:5682246f9409 910 }
yuron 22:5682246f9409 911 break;
yuron 22:5682246f9409 912
yuron 22:5682246f9409 913 //壁に当たるまで右移動
yuron 23:1e4d7540715f 914 case 19:
yuron 22:5682246f9409 915 counter.reset();
yuron 22:5682246f9409 916
yuron 22:5682246f9409 917 if(right_limit == 3) {
yuron 23:1e4d7540715f 918 phase = 20;
yuron 22:5682246f9409 919 }
yuron 22:5682246f9409 920 else if(right_limit != 3) {
yuron 22:5682246f9409 921 true_migimae_data[0] = 0x40;
yuron 22:5682246f9409 922 true_migiusiro_data[0] = 0xBF;
yuron 22:5682246f9409 923 true_hidarimae_data[0] = 0xBF;
yuron 22:5682246f9409 924 true_hidariusiro_data[0] = 0x40;
yuron 22:5682246f9409 925 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 926 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 927 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 928 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 929 wait_us(20);
yuron 22:5682246f9409 930 }
yuron 22:5682246f9409 931 break;
yuron 22:5682246f9409 932
yuron 22:5682246f9409 933 //1秒停止
yuron 23:1e4d7540715f 934 case 20:
yuron 22:5682246f9409 935 stop();
yuron 22:5682246f9409 936 counter.start();
yuron 22:5682246f9409 937 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 938 phase = 21;
yuron 22:5682246f9409 939 wheel_reset();
yuron 22:5682246f9409 940 }
yuron 22:5682246f9409 941 break;
yuron 22:5682246f9409 942
yuron 22:5682246f9409 943 //壁に当たるまで前進
yuron 23:1e4d7540715f 944 case 21:
yuron 22:5682246f9409 945 counter.reset();
yuron 22:5682246f9409 946
yuron 22:5682246f9409 947 if(front_limit == 3) {
yuron 23:1e4d7540715f 948 phase = 22;
yuron 22:5682246f9409 949 }
yuron 22:5682246f9409 950 else if(front_limit != 3){
yuron 22:5682246f9409 951 true_migimae_data[0] = 0xC0;
yuron 22:5682246f9409 952 true_migiusiro_data[0] = 0xC0;
yuron 22:5682246f9409 953 true_hidarimae_data[0] = 0xC0;
yuron 22:5682246f9409 954 true_hidariusiro_data[0] = 0xC0;
yuron 22:5682246f9409 955 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 956 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 957 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 958 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 959 wait_us(20);
yuron 22:5682246f9409 960 }
yuron 22:5682246f9409 961 break;
yuron 22:5682246f9409 962
yuron 22:5682246f9409 963 //シーツ装填
yuron 23:1e4d7540715f 964 case 22:
yuron 22:5682246f9409 965 if(start_switch == 1) {
yuron 22:5682246f9409 966 wheel_reset();
yuron 23:1e4d7540715f 967 phase = 23;
yuron 22:5682246f9409 968 } else {
yuron 22:5682246f9409 969 stop();
yuron 22:5682246f9409 970 }
yuron 22:5682246f9409 971 break;
yuron 22:5682246f9409 972
yuron 22:5682246f9409 973 //竿のラインまで後進
yuron 23:1e4d7540715f 974 case 23:
yuron 22:5682246f9409 975 counter.reset();
yuron 23:1e4d7540715f 976 back(-20500);
yuron 23:1e4d7540715f 977 if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
yuron 23:1e4d7540715f 978 phase = 24;
yuron 22:5682246f9409 979 }
yuron 22:5682246f9409 980 break;
yuron 22:5682246f9409 981
yuron 22:5682246f9409 982 //1秒停止
yuron 23:1e4d7540715f 983 case 24:
yuron 22:5682246f9409 984 stop();
yuron 22:5682246f9409 985 counter.start();
yuron 22:5682246f9409 986 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 987 phase = 25;
yuron 22:5682246f9409 988 wheel_reset();
yuron 22:5682246f9409 989 }
yuron 22:5682246f9409 990 break;
yuron 22:5682246f9409 991
yuron 22:5682246f9409 992 //ちょっと左移動
yuron 23:1e4d7540715f 993 case 25:
yuron 22:5682246f9409 994 counter.reset();
yuron 22:5682246f9409 995 left(500);
yuron 22:5682246f9409 996 if((x_pulse1 > 500) || (x_pulse2 > 500)) {
yuron 23:1e4d7540715f 997 phase = 26;
yuron 22:5682246f9409 998 }
yuron 22:5682246f9409 999 break;
yuron 22:5682246f9409 1000
yuron 22:5682246f9409 1001 //1秒停止
yuron 23:1e4d7540715f 1002 case 26:
yuron 22:5682246f9409 1003 stop();
yuron 22:5682246f9409 1004 counter.start();
yuron 22:5682246f9409 1005 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1006 phase = 27;
yuron 22:5682246f9409 1007 wheel_reset();
yuron 22:5682246f9409 1008 }
yuron 22:5682246f9409 1009 break;
yuron 22:5682246f9409 1010
yuron 22:5682246f9409 1011 //90°左旋回
yuron 23:1e4d7540715f 1012 case 27:
yuron 22:5682246f9409 1013 counter.reset();
yuron 23:1e4d7540715f 1014 //turn_left(465);
yuron 23:1e4d7540715f 1015 turn_left(485);
yuron 23:1e4d7540715f 1016 if(sum_pulse > 485) {
yuron 23:1e4d7540715f 1017 phase = 28;
yuron 22:5682246f9409 1018 }
yuron 22:5682246f9409 1019 break;
yuron 22:5682246f9409 1020
yuron 22:5682246f9409 1021 //1秒停止
yuron 23:1e4d7540715f 1022 case 28:
yuron 22:5682246f9409 1023 stop();
yuron 22:5682246f9409 1024 counter.start();
yuron 22:5682246f9409 1025 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1026 phase = 29;
yuron 22:5682246f9409 1027 wheel_reset();
yuron 22:5682246f9409 1028 }
yuron 22:5682246f9409 1029 break;
yuron 22:5682246f9409 1030
yuron 23:1e4d7540715f 1031 //壁に当たるまで後進
yuron 23:1e4d7540715f 1032 case 29:
yuron 22:5682246f9409 1033 counter.reset();
yuron 22:5682246f9409 1034
yuron 22:5682246f9409 1035 if(back_limit == 3) {
yuron 23:1e4d7540715f 1036 phase = 30;
yuron 22:5682246f9409 1037 }
yuron 22:5682246f9409 1038 else if(back_limit != 3){
yuron 22:5682246f9409 1039 true_migimae_data[0] = 0x50;
yuron 22:5682246f9409 1040 true_migiusiro_data[0] = 0x50;
yuron 22:5682246f9409 1041 true_hidarimae_data[0] = 0x50;
yuron 22:5682246f9409 1042 true_hidariusiro_data[0] = 0x50;
yuron 22:5682246f9409 1043 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 1044 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 1045 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 1046 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 1047 wait_us(20);
yuron 22:5682246f9409 1048 }
yuron 22:5682246f9409 1049 break;
yuron 22:5682246f9409 1050
yuron 22:5682246f9409 1051 //1秒停止
yuron 23:1e4d7540715f 1052 case 30:
yuron 22:5682246f9409 1053 stop();
yuron 22:5682246f9409 1054 counter.start();
yuron 22:5682246f9409 1055 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1056 phase = 31;
yuron 22:5682246f9409 1057 wheel_reset();
yuron 22:5682246f9409 1058 }
yuron 22:5682246f9409 1059 break;
yuron 22:5682246f9409 1060
yuron 22:5682246f9409 1061 //掛けるところまで前進
yuron 23:1e4d7540715f 1062 case 31:
yuron 22:5682246f9409 1063 counter.reset();
yuron 22:5682246f9409 1064 front(10000);
yuron 22:5682246f9409 1065 if((y_pulse1 > 10000) || (y_pulse2 > 10000)) {
yuron 23:1e4d7540715f 1066 phase = 32;
yuron 22:5682246f9409 1067 counter.start();
yuron 22:5682246f9409 1068 }
yuron 22:5682246f9409 1069 break;
yuron 22:5682246f9409 1070
yuron 22:5682246f9409 1071 //1秒停止
yuron 23:1e4d7540715f 1072 case 32:
yuron 22:5682246f9409 1073 stop();
yuron 22:5682246f9409 1074 counter.start();
yuron 22:5682246f9409 1075 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1076 phase = 33;
yuron 22:5682246f9409 1077 wheel_reset();
yuron 22:5682246f9409 1078 }
yuron 22:5682246f9409 1079 break;
yuron 22:5682246f9409 1080
yuron 22:5682246f9409 1081 //妨害防止の左旋回
yuron 23:1e4d7540715f 1082 case 33:
yuron 22:5682246f9409 1083 counter.reset();
yuron 22:5682246f9409 1084 turn_left(30);
yuron 22:5682246f9409 1085 if(sum_pulse > 30) {
yuron 23:1e4d7540715f 1086 phase = 34;
yuron 22:5682246f9409 1087 }
yuron 22:5682246f9409 1088 break;
yuron 22:5682246f9409 1089
yuron 22:5682246f9409 1090 //1秒停止
yuron 23:1e4d7540715f 1091 case 34:
yuron 22:5682246f9409 1092 stop();
yuron 22:5682246f9409 1093 counter.start();
yuron 22:5682246f9409 1094 if(counter.read() > 1.0f) {
yuron 23:1e4d7540715f 1095 phase = 35;
yuron 22:5682246f9409 1096 wheel_reset();
yuron 22:5682246f9409 1097 }
yuron 22:5682246f9409 1098 break;
yuron 22:5682246f9409 1099
yuron 22:5682246f9409 1100 //カウンターリセット
yuron 23:1e4d7540715f 1101 case 35:
yuron 22:5682246f9409 1102 counter.reset();
yuron 22:5682246f9409 1103 counter.start();
yuron 23:1e4d7540715f 1104 phase = 36;
yuron 22:5682246f9409 1105 break;
yuron 22:5682246f9409 1106
yuron 22:5682246f9409 1107 //アームアップ
yuron 23:1e4d7540715f 1108 case 36:
yuron 22:5682246f9409 1109 stop();
yuron 22:5682246f9409 1110 //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
yuron 23:1e4d7540715f 1111 if(counter.read() < 1.0f) {
yuron 22:5682246f9409 1112 right_arm_data[0] = 0xFF;
yuron 22:5682246f9409 1113 left_arm_data[0] = 0xFF;
yuron 22:5682246f9409 1114 i2c.write(0x22, right_arm_data, 1);
yuron 22:5682246f9409 1115 i2c.write(0x24, left_arm_data, 1);
yuron 22:5682246f9409 1116 wait_us(20);
yuron 22:5682246f9409 1117 } else {
yuron 23:1e4d7540715f 1118 arm_up(37);
yuron 22:5682246f9409 1119 }
yuron 22:5682246f9409 1120 break;
yuron 22:5682246f9409 1121
yuron 22:5682246f9409 1122 //カウンターリセット
yuron 23:1e4d7540715f 1123 case 37:
yuron 22:5682246f9409 1124 counter.reset();
yuron 23:1e4d7540715f 1125 phase = 38;
yuron 22:5682246f9409 1126 break;
yuron 22:5682246f9409 1127
yuron 22:5682246f9409 1128 //シーツを掛ける
yuron 23:1e4d7540715f 1129 case 38:
yuron 22:5682246f9409 1130 counter.start();
yuron 22:5682246f9409 1131
yuron 22:5682246f9409 1132 //1秒間ファン送風
yuron 23:1e4d7540715f 1133 if(counter.read() <= 2.0f) {
yuron 22:5682246f9409 1134 fan_data[0] = 0xFF;
yuron 22:5682246f9409 1135 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1136 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1137 servo_data[0] = 0x04;
yuron 22:5682246f9409 1138 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 1139 }
yuron 22:5682246f9409 1140 //1~3秒の間でサーボを開放
yuron 23:1e4d7540715f 1141 else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
yuron 22:5682246f9409 1142 fan_data[0] = 0xFF;
yuron 22:5682246f9409 1143 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1144 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1145 servo_data[0] = 0x03;
yuron 22:5682246f9409 1146 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 1147 }
yuron 22:5682246f9409 1148 //3秒過ぎたら終わり
yuron 23:1e4d7540715f 1149 else if(counter.read() > 4.0f) {
yuron 22:5682246f9409 1150 fan_data[0] = 0x80;
yuron 22:5682246f9409 1151 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1152 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1153 servo_data[0] = 0x04;
yuron 22:5682246f9409 1154 i2c.write(0x30, servo_data, 1);
yuron 23:1e4d7540715f 1155 phase = 39;
yuron 22:5682246f9409 1156 }
yuron 22:5682246f9409 1157 break;
yuron 22:5682246f9409 1158
yuron 22:5682246f9409 1159 //終了っ！(守衛さん風)
yuron 23:1e4d7540715f 1160 case 39:
yuron 22:5682246f9409 1161 default:
yuron 22:5682246f9409 1162 //駆動系統OFF
yuron 22:5682246f9409 1163 all_stop();
yuron 22:5682246f9409 1164 break;
yuron 22:5682246f9409 1165 }
yuron 21:89db2a19e52e 1166 }
yuron 16:05b26003da50 1167 }
yuron 16:05b26003da50 1168 }
yuron 17:de3bc1999ae7 1169
yuron 14:ab89b6cd9719 1170 void init(void) {
yuron 10:b672aa81b226 1171
yuron 14:ab89b6cd9719 1172 //通信ボーレートの設定
yuron 16:05b26003da50 1173 pc.baud(460800);
yuron 20:ac4954be1fe0 1174
yuron 18:851f783ec516 1175 limit_serial.baud(115200);
yuron 20:ac4954be1fe0 1176
yuron 16:05b26003da50 1177 start_switch.mode(PullUp);
yuron 21:89db2a19e52e 1178 zone_switch.mode(PullDown);
yuron 20:ac4954be1fe0 1179
yuron 17:de3bc1999ae7 1180 //非常停止関連
yuron 17:de3bc1999ae7 1181 pic.baud(19200);
yuron 17:de3bc1999ae7 1182 pic.format(8, Serial::None, 1);
yuron 17:de3bc1999ae7 1183 pic.attach(get, Serial::RxIrq);
yuron 20:ac4954be1fe0 1184
yuron 22:5682246f9409 1185 x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; sum_pulse = 0;
yuron 14:ab89b6cd9719 1186 migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 1187 true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80;
yuron 19:f17d2e585973 1188 fan_data[0] = 0x80;
yuron 19:f17d2e585973 1189 servo_data[0] = 0x80;
yuron 21:89db2a19e52e 1190 arm_motor[0] = 0x80; drop_motor[0] = 0x80;
yuron 20:ac4954be1fe0 1191 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
yuron 14:ab89b6cd9719 1192 }
yuron 5:167327a82430 1193
yuron 14:ab89b6cd9719 1194 void init_send(void) {
yuron 20:ac4954be1fe0 1195
yuron 14:ab89b6cd9719 1196 init_send_data[0] = 0x80;
yuron 14:ab89b6cd9719 1197 i2c.write(0x10, init_send_data, 1);
yuron 14:ab89b6cd9719 1198 i2c.write(0x12, init_send_data, 1);
yuron 14:ab89b6cd9719 1199 i2c.write(0x14, init_send_data, 1);
yuron 14:ab89b6cd9719 1200 i2c.write(0x16, init_send_data, 1);
yuron 20:ac4954be1fe0 1201 i2c.write(0x18, init_send_data, 1);
yuron 20:ac4954be1fe0 1202 i2c.write(0x20, init_send_data, 1);
yuron 20:ac4954be1fe0 1203 i2c.write(0x22, init_send_data, 1);
yuron 20:ac4954be1fe0 1204 i2c.write(0x24, init_send_data, 1);
yuron 20:ac4954be1fe0 1205 i2c.write(0x30, init_send_data, 1);
yuron 14:ab89b6cd9719 1206 wait(0.1);
yuron 14:ab89b6cd9719 1207 }
yuron 0:f73c1b076ae4 1208
yuron 17:de3bc1999ae7 1209 void get(void) {
yuron 20:ac4954be1fe0 1210
yuron 20:ac4954be1fe0 1211 baff = pic.getc();
yuron 20:ac4954be1fe0 1212
yuron 17:de3bc1999ae7 1213 for(; flug; flug--)
yuron 17:de3bc1999ae7 1214 RDATA = baff;
yuron 20:ac4954be1fe0 1215
yuron 17:de3bc1999ae7 1216 if(baff == ':')
yuron 17:de3bc1999ae7 1217 flug = 1;
yuron 17:de3bc1999ae7 1218 }
yuron 17:de3bc1999ae7 1219
yuron 14:ab89b6cd9719 1220 void get_pulses(void) {
yuron 20:ac4954be1fe0 1221
yuron 14:ab89b6cd9719 1222 x_pulse1 = wheel_x1.getPulses();
yuron 14:ab89b6cd9719 1223 x_pulse2 = wheel_x2.getPulses();
yuron 14:ab89b6cd9719 1224 y_pulse1 = wheel_y1.getPulses();
yuron 14:ab89b6cd9719 1225 y_pulse2 = wheel_y2.getPulses();
yuron 22:5682246f9409 1226 sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4;
yuron 23:1e4d7540715f 1227 arm_pulse = arm_enc.getPulses();
yuron 14:ab89b6cd9719 1228 }
yuron 0:f73c1b076ae4 1229
yuron 14:ab89b6cd9719 1230 void print_pulses(void) {
yuron 23:1e4d7540715f 1231 //pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
yuron 23:1e4d7540715f 1232 //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 1233 //pc.printf("f: %d, b: %d, r: %d, phase: %d\n\r", front_limit, back_limit, right_limit, phase);
yuron 21:89db2a19e52e 1234 //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
yuron 19:f17d2e585973 1235 //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
yuron 18:851f783ec516 1236 //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 1237 //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 1238 }
yuron 4:df334779a69e 1239
yuron 17:de3bc1999ae7 1240 void get_emergency(void) {
yuron 20:ac4954be1fe0 1241
yuron 17:de3bc1999ae7 1242 if(RDATA == '1') {
yuron 17:de3bc1999ae7 1243 myled = 1;
yuron 17:de3bc1999ae7 1244 emergency = 1;
yuron 17:de3bc1999ae7 1245 }
yuron 17:de3bc1999ae7 1246 else if(RDATA == '9'){
yuron 17:de3bc1999ae7 1247 myled = 0.2;
yuron 17:de3bc1999ae7 1248 emergency = 0;
yuron 23:1e4d7540715f 1249
yuron 23:1e4d7540715f 1250 //終了phaseで駆動系統OFF
yuron 23:1e4d7540715f 1251 if(phase >= 39) {
yuron 23:1e4d7540715f 1252 emergency = 1;
yuron 23:1e4d7540715f 1253 }
yuron 23:1e4d7540715f 1254 else if(phase < 39) {
yuron 23:1e4d7540715f 1255 emergency = 0;
yuron 23:1e4d7540715f 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 23:1e4d7540715f 1587 phase = next_phase;
yuron 23:1e4d7540715f 1588 kaisyu_phase = 2;
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 21:89db2a19e52e 1674 arm_motor[0] = 0x40;
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 21:89db2a19e52e 1681 //後進->減速
yuron 21:89db2a19e52e 1682 //リミットが押されたら強制停止
yuron 21:89db2a19e52e 1683 if(tyokudo_usiro_limit == 1) {
yuron 21:89db2a19e52e 1684 arm_motor[0] = 0x80;
yuron 21:89db2a19e52e 1685 drop_motor[0] = 0x80;
yuron 21:89db2a19e52e 1686 tyokudo_phase = 2;
yuron 21:89db2a19e52e 1687 phase = next_phase;
yuron 19:f17d2e585973 1688 }
yuron 19:f17d2e585973 1689 break;
yuron 20:ac4954be1fe0 1690
yuron 19:f17d2e585973 1691 default:
yuron 21:89db2a19e52e 1692 arm_motor[0] = 0x80;
yuron 21:89db2a19e52e 1693 drop_motor[0] = 0x80;
yuron 19:f17d2e585973 1694 break;
yuron 19:f17d2e585973 1695 }
yuron 19:f17d2e585973 1696
yuron 21:89db2a19e52e 1697 i2c.write(0x18, arm_motor, 1);
yuron 21:89db2a19e52e 1698 i2c.write(0x20, drop_motor, 1);
yuron 18:851f783ec516 1699 }
yuron 18:851f783ec516 1700
yuron 21:89db2a19e52e 1701 void arm_up(int next_phase) {
yuron 20:ac4954be1fe0 1702
yuron 20:ac4954be1fe0 1703 //両腕、上限リミットが押されてなかったら上昇
yuron 20:ac4954be1fe0 1704 if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
yuron 20:ac4954be1fe0 1705 right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF;
yuron 20:ac4954be1fe0 1706 }
yuron 20:ac4954be1fe0 1707 //右腕のみリミットが押されたら左腕のみ上昇
yuron 20:ac4954be1fe0 1708 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
yuron 20:ac4954be1fe0 1709 right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF;
yuron 20:ac4954be1fe0 1710 }
yuron 20:ac4954be1fe0 1711 //左腕のみリミットが押されたら右腕のみ上昇
yuron 20:ac4954be1fe0 1712 else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
yuron 20:ac4954be1fe0 1713 right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80;
yuron 20:ac4954be1fe0 1714 }
yuron 20:ac4954be1fe0 1715 //両腕、上限リミットが押されたら停止
yuron 20:ac4954be1fe0 1716 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
yuron 20:ac4954be1fe0 1717 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80;
yuron 21:89db2a19e52e 1718 phase = next_phase;
yuron 20:ac4954be1fe0 1719 }
yuron 20:ac4954be1fe0 1720
yuron 20:ac4954be1fe0 1721 i2c.write(0x22, right_arm_data, 1);
yuron 20:ac4954be1fe0 1722 i2c.write(0x24, left_arm_data, 1);
yuron 21:89db2a19e52e 1723 wait_us(20);
yuron 20:ac4954be1fe0 1724 }
yuron 20:ac4954be1fe0 1725
yuron 17:de3bc1999ae7 1726 void front(int target) {
yuron 20:ac4954be1fe0 1727
yuron 14:ab89b6cd9719 1728 front_PID(target);
yuron 14:ab89b6cd9719 1729 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1730 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1731 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1732 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1733 wait_us(20);
yuron 14:ab89b6cd9719 1734 }
yuron 4:df334779a69e 1735
yuron 17:de3bc1999ae7 1736 void back(int target) {
yuron 20:ac4954be1fe0 1737
yuron 14:ab89b6cd9719 1738 back_PID(target);
yuron 14:ab89b6cd9719 1739 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1740 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1741 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1742 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1743 wait_us(20);
yuron 14:ab89b6cd9719 1744 }
yuron 5:167327a82430 1745
yuron 17:de3bc1999ae7 1746 void right(int target) {
yuron 20:ac4954be1fe0 1747
yuron 14:ab89b6cd9719 1748 right_PID(target);
yuron 14:ab89b6cd9719 1749 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1750 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1751 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1752 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1753 wait_us(20);
yuron 14:ab89b6cd9719 1754 }
yuron 5:167327a82430 1755
yuron 17:de3bc1999ae7 1756 void left(int target) {
yuron 20:ac4954be1fe0 1757
yuron 14:ab89b6cd9719 1758 left_PID(target);
yuron 14:ab89b6cd9719 1759 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1760 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1761 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1762 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1763 wait_us(20);
yuron 14:ab89b6cd9719 1764 }
yuron 4:df334779a69e 1765
yuron 17:de3bc1999ae7 1766 void turn_right(int target) {
yuron 20:ac4954be1fe0 1767
yuron 14:ab89b6cd9719 1768 turn_right_PID(target);
yuron 14:ab89b6cd9719 1769 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1770 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1771 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1772 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1773 wait_us(20);
yuron 14:ab89b6cd9719 1774 }
yuron 4:df334779a69e 1775
yuron 17:de3bc1999ae7 1776 void turn_left(int target) {
yuron 20:ac4954be1fe0 1777
yuron 14:ab89b6cd9719 1778 turn_left_PID(target);
yuron 14:ab89b6cd9719 1779 i2c.write(0x10, true_migimae_data, 1, false);
yuron 14:ab89b6cd9719 1780 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 14:ab89b6cd9719 1781 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 14:ab89b6cd9719 1782 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 14:ab89b6cd9719 1783 wait_us(20);
yuron 14:ab89b6cd9719 1784 }
yuron 5:167327a82430 1785
yuron 18:851f783ec516 1786 void stop(void) {
yuron 20:ac4954be1fe0 1787
yuron 18:851f783ec516 1788 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 1789 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 1790 true_hidarimae_data[0] = 0x80;
yuron 20:ac4954be1fe0 1791 true_hidariusiro_data[0] = 0x80;
yuron 20:ac4954be1fe0 1792
yuron 18:851f783ec516 1793 i2c.write(0x10, true_migimae_data, 1, false);
yuron 18:851f783ec516 1794 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 18:851f783ec516 1795 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 18:851f783ec516 1796 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 18:851f783ec516 1797 wait_us(20);
yuron 18:851f783ec516 1798 }
yuron 18:851f783ec516 1799
yuron 22:5682246f9409 1800 void all_stop(void) {
yuron 22:5682246f9409 1801
yuron 22:5682246f9409 1802 true_migimae_data[0] = 0x80;
yuron 22:5682246f9409 1803 true_migiusiro_data[0] = 0x80;
yuron 22:5682246f9409 1804 true_hidarimae_data[0] = 0x80;
yuron 22:5682246f9409 1805 true_hidariusiro_data[0] = 0x80;
yuron 22:5682246f9409 1806 arm_motor[0] = 0x80;
yuron 22:5682246f9409 1807 drop_motor[0] = 0x80;
yuron 22:5682246f9409 1808 right_arm_data[0] = 0x80;
yuron 22:5682246f9409 1809 left_arm_data[0] = 0x80;
yuron 22:5682246f9409 1810 fan_data[0] = 0x80;
yuron 22:5682246f9409 1811 servo_data[0] = 0x04;
yuron 22:5682246f9409 1812
yuron 22:5682246f9409 1813 i2c.write(0x10, true_migimae_data, 1, false);
yuron 22:5682246f9409 1814 i2c.write(0x12, true_migiusiro_data, 1, false);
yuron 22:5682246f9409 1815 i2c.write(0x14, true_hidarimae_data, 1, false);
yuron 22:5682246f9409 1816 i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron 22:5682246f9409 1817 i2c.write(0x18, arm_motor, 1);
yuron 22:5682246f9409 1818 i2c.write(0x20, drop_motor, 1);
yuron 22:5682246f9409 1819 i2c.write(0x22, right_arm_data, 1);
yuron 22:5682246f9409 1820 i2c.write(0x24, left_arm_data, 1);
yuron 22:5682246f9409 1821 i2c.write(0x26, fan_data, 1);
yuron 22:5682246f9409 1822 i2c.write(0x28, fan_data, 1);
yuron 22:5682246f9409 1823 i2c.write(0x30, servo_data, 1);
yuron 22:5682246f9409 1824 wait_us(20);
yuron 22:5682246f9409 1825 }
yuron 22:5682246f9409 1826
yuron 17:de3bc1999ae7 1827 void front_PID(int target) {
yuron 5:167327a82430 1828
yuron 14:ab89b6cd9719 1829 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 1830 front_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1831 front_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1832 front_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1833 front_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 5:167327a82430 1834
yuron 14:ab89b6cd9719 1835 //制御量の最小、最大
yuron 14:ab89b6cd9719 1836 //正転(目標に達してない)
yuron 19:f17d2e585973 1837 if((y_pulse1 < target) && (y_pulse2 < target)) {
yuron 22:5682246f9409 1838 front_migimae.setOutputLimits(0x84, 0xF5);
yuron 22:5682246f9409 1839 front_migiusiro.setOutputLimits(0x84, 0xF5);
yuron 16:05b26003da50 1840 front_hidarimae.setOutputLimits(0x84, 0xFF);
yuron 16:05b26003da50 1841 front_hidariusiro.setOutputLimits(0x84, 0xFF);
yuron 14:ab89b6cd9719 1842 }
yuron 18:851f783ec516 1843 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1844 else if((y_pulse1 > target) && (y_pulse2 > target)) {
yuron 18:851f783ec516 1845 front_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1846 front_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1847 front_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1848 front_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 14:ab89b6cd9719 1849 }
yuron 5:167327a82430 1850
yuron 14:ab89b6cd9719 1851 //よくわからんやつ
yuron 16:05b26003da50 1852 front_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1853 front_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 1854 front_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1855 front_hidariusiro.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 1856
yuron 14:ab89b6cd9719 1857 //目標値
yuron 16:05b26003da50 1858 front_migimae.setSetPoint(target);
yuron 16:05b26003da50 1859 front_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 1860 front_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 1861 front_hidariusiro.setSetPoint(target);
yuron 5:167327a82430 1862
yuron 14:ab89b6cd9719 1863 //センサ出力
yuron 16:05b26003da50 1864 front_migimae.setProcessValue(y_pulse1);
yuron 16:05b26003da50 1865 front_migiusiro.setProcessValue(y_pulse1);
yuron 16:05b26003da50 1866 front_hidarimae.setProcessValue(y_pulse2);
yuron 16:05b26003da50 1867 front_hidariusiro.setProcessValue(y_pulse2);
yuron 5:167327a82430 1868
yuron 14:ab89b6cd9719 1869 //制御量(計算結果)
yuron 16:05b26003da50 1870 migimae_data[0] = front_migimae.compute();
yuron 16:05b26003da50 1871 migiusiro_data[0] = front_migiusiro.compute();
yuron 16:05b26003da50 1872 hidarimae_data[0] = front_hidarimae.compute();
yuron 16:05b26003da50 1873 hidariusiro_data[0] = front_hidariusiro.compute();
yuron 4:df334779a69e 1874
yuron 14:ab89b6cd9719 1875 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 1876 //正転(目標に達してない)
yuron 19:f17d2e585973 1877 if((y_pulse1 < target) && (y_pulse2 < target)) {
yuron 14:ab89b6cd9719 1878 true_migimae_data[0] = migimae_data[0];
yuron 14:ab89b6cd9719 1879 true_migiusiro_data[0] = migiusiro_data[0];
yuron 14:ab89b6cd9719 1880 true_hidarimae_data[0] = hidarimae_data[0];
yuron 14:ab89b6cd9719 1881 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 14:ab89b6cd9719 1882 }
yuron 18:851f783ec516 1883 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1884 else if((y_pulse1 > target) && (y_pulse2 > target)) {
yuron 18:851f783ec516 1885 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 1886 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 1887 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 1888 true_hidariusiro_data[0] = 0x80;
yuron 17:de3bc1999ae7 1889 }
yuron 5:167327a82430 1890 }
yuron 5:167327a82430 1891
yuron 17:de3bc1999ae7 1892 void back_PID(int target) {
yuron 20:ac4954be1fe0 1893
yuron 14:ab89b6cd9719 1894 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 1895 back_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1896 back_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1897 back_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1898 back_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 5:167327a82430 1899
yuron 14:ab89b6cd9719 1900 //制御量の最小、最大
yuron 14:ab89b6cd9719 1901 //逆転(目標に達してない)
yuron 19:f17d2e585973 1902 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
yuron 16:05b26003da50 1903 back_migimae.setOutputLimits(0x00, 0x7B);
yuron 16:05b26003da50 1904 back_migiusiro.setOutputLimits(0x00, 0x7B);
yuron 22:5682246f9409 1905 back_hidarimae.setOutputLimits(0x00, 0x70);
yuron 22:5682246f9409 1906 back_hidariusiro.setOutputLimits(0x00, 0x70);
yuron 22:5682246f9409 1907 //back_hidarimae.setOutputLimits(0x00, 0x7B);
yuron 22:5682246f9409 1908 //back_hidariusiro.setOutputLimits(0x00, 0x7B);
yuron 14:ab89b6cd9719 1909 }
yuron 18:851f783ec516 1910 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1911 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 1912 back_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1913 back_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1914 back_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1915 back_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 4:df334779a69e 1916 }
yuron 5:167327a82430 1917
yuron 14:ab89b6cd9719 1918 //よくわからんやつ
yuron 16:05b26003da50 1919 back_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1920 back_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 1921 back_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1922 back_hidariusiro.setMode(AUTO_MODE);
yuron 14:ab89b6cd9719 1923
yuron 14:ab89b6cd9719 1924 //目標値
yuron 17:de3bc1999ae7 1925 back_migimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1926 back_migiusiro.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1927 back_hidarimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1928 back_hidariusiro.setSetPoint(target*-1);
yuron 14:ab89b6cd9719 1929
yuron 14:ab89b6cd9719 1930 //センサ出力
yuron 17:de3bc1999ae7 1931 back_migimae.setProcessValue(y_pulse1*-1);
yuron 17:de3bc1999ae7 1932 back_migiusiro.setProcessValue(y_pulse1*-1);
yuron 17:de3bc1999ae7 1933 back_hidarimae.setProcessValue(y_pulse2*-1);
yuron 17:de3bc1999ae7 1934 back_hidariusiro.setProcessValue(y_pulse2*-1);
yuron 14:ab89b6cd9719 1935
yuron 14:ab89b6cd9719 1936 //制御量(計算結果)
yuron 16:05b26003da50 1937 migimae_data[0] = back_migimae.compute();
yuron 16:05b26003da50 1938 migiusiro_data[0] = back_migiusiro.compute();
yuron 16:05b26003da50 1939 hidarimae_data[0] = back_hidarimae.compute();
yuron 16:05b26003da50 1940 hidariusiro_data[0] = back_hidariusiro.compute();
yuron 14:ab89b6cd9719 1941
yuron 14:ab89b6cd9719 1942 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 1943 //逆転(目標に達してない)
yuron 19:f17d2e585973 1944 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
yuron 14:ab89b6cd9719 1945 true_migimae_data[0] = 0x7B - migimae_data[0];
yuron 14:ab89b6cd9719 1946 true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
yuron 14:ab89b6cd9719 1947 true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
yuron 14:ab89b6cd9719 1948 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
yuron 5:167327a82430 1949 }
yuron 18:851f783ec516 1950 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1951 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 1952 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 1953 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 1954 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 1955 true_hidariusiro_data[0] = 0x80;
yuron 17:de3bc1999ae7 1956 }
yuron 14:ab89b6cd9719 1957 }
yuron 14:ab89b6cd9719 1958
yuron 17:de3bc1999ae7 1959 void right_PID(int target) {
yuron 14:ab89b6cd9719 1960
yuron 14:ab89b6cd9719 1961 //センサ出力値の最小、最大
yuron 16:05b26003da50 1962 right_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1963 right_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1964 right_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 1965 right_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 14:ab89b6cd9719 1966
yuron 14:ab89b6cd9719 1967 //制御量の最小、最大
yuron 14:ab89b6cd9719 1968 //右進(目標まで達していない)
yuron 19:f17d2e585973 1969 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
yuron 22:5682246f9409 1970 right_migimae.setOutputLimits(0x6A, 0x6C);
yuron 22:5682246f9409 1971 //right_migimae.setOutputLimits(0x7A, 0x7B);
yuron 21:89db2a19e52e 1972 right_migiusiro.setOutputLimits(0xFE, 0xFF);
yuron 22:5682246f9409 1973 right_hidarimae.setOutputLimits(0xEF, 0xF0);
yuron 22:5682246f9409 1974 //right_hidarimae.setOutputLimits(0xFE, 0xFF);
yuron 21:89db2a19e52e 1975 right_hidariusiro.setOutputLimits(0x7A, 0x7B);
yuron 17:de3bc1999ae7 1976
yuron 17:de3bc1999ae7 1977 }
yuron 18:851f783ec516 1978 //停止(目標より行き過ぎ)
yuron 19:f17d2e585973 1979 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 1980 right_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1981 right_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1982 right_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 1983 right_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 5:167327a82430 1984 }
yuron 5:167327a82430 1985
yuron 14:ab89b6cd9719 1986 //よくわからんやつ
yuron 16:05b26003da50 1987 right_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1988 right_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 1989 right_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 1990 right_hidariusiro.setMode(AUTO_MODE);
yuron 14:ab89b6cd9719 1991
yuron 14:ab89b6cd9719 1992 //目標値
yuron 17:de3bc1999ae7 1993 right_migimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1994 right_migiusiro.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1995 right_hidarimae.setSetPoint(target*-1);
yuron 17:de3bc1999ae7 1996 right_hidariusiro.setSetPoint(target*-1);
yuron 8:3df97287c825 1997
yuron 14:ab89b6cd9719 1998 //センサ出力
yuron 17:de3bc1999ae7 1999 right_migimae.setProcessValue(x_pulse1*-1);
yuron 17:de3bc1999ae7 2000 right_migiusiro.setProcessValue(x_pulse2*-1);
yuron 17:de3bc1999ae7 2001 right_hidarimae.setProcessValue(x_pulse1*-1);
yuron 17:de3bc1999ae7 2002 right_hidariusiro.setProcessValue(x_pulse2*-1);
yuron 14:ab89b6cd9719 2003
yuron 14:ab89b6cd9719 2004 //制御量(計算結果)
yuron 16:05b26003da50 2005 migimae_data[0] = right_migimae.compute();
yuron 16:05b26003da50 2006 migiusiro_data[0] = right_migiusiro.compute();
yuron 16:05b26003da50 2007 hidarimae_data[0] = right_hidarimae.compute();
yuron 16:05b26003da50 2008 hidariusiro_data[0] = right_hidariusiro.compute();
yuron 8:3df97287c825 2009
yuron 14:ab89b6cd9719 2010 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 2011 //右進(目標まで達していない)
yuron 19:f17d2e585973 2012 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
yuron 14:ab89b6cd9719 2013 true_migimae_data[0] = 0x7B - migimae_data[0];
yuron 14:ab89b6cd9719 2014 true_migiusiro_data[0] = migiusiro_data[0];
yuron 14:ab89b6cd9719 2015 true_hidarimae_data[0] = hidarimae_data[0];
yuron 14:ab89b6cd9719 2016 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
yuron 9:1359f0c813b1 2017 }
yuron 17:de3bc1999ae7 2018 //左進(目標より行き過ぎ)
yuron 19:f17d2e585973 2019 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
yuron 18:851f783ec516 2020 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2021 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2022 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2023 true_hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 2024 }
yuron 14:ab89b6cd9719 2025 }
yuron 9:1359f0c813b1 2026
yuron 17:de3bc1999ae7 2027 void left_PID(int target) {
yuron 20:ac4954be1fe0 2028
yuron 14:ab89b6cd9719 2029 //センサ出力値の最小、最大
yuron 16:05b26003da50 2030 left_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2031 left_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2032 left_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2033 left_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 9:1359f0c813b1 2034
yuron 14:ab89b6cd9719 2035 //制御量の最小、最大
yuron 14:ab89b6cd9719 2036 //左進(目標まで達していない)
yuron 22:5682246f9409 2037 if((x_pulse1 < target) && (x_pulse2 < target)) {
yuron 23:1e4d7540715f 2038 left_migimae.setOutputLimits(0xEC, 0xED); //0xFFに近いほうが速い
yuron 23:1e4d7540715f 2039 left_migiusiro.setOutputLimits(0x7A, 0x7B); //0x7Bに近いほうが速い
yuron 23:1e4d7540715f 2040 //left_hidarimae.setOutputLimits(0x6B, 0x6C); //0x7Bに近いほうが速い
yuron 23:1e4d7540715f 2041 left_hidarimae.setOutputLimits(0x6E, 0x6F);
yuron 23:1e4d7540715f 2042 left_hidariusiro.setOutputLimits(0xFE, 0xFF); //0xFFに近いほうが速い
yuron 17:de3bc1999ae7 2043 }
yuron 18:851f783ec516 2044 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2045 else if((x_pulse1 > target) && (x_pulse2 > target)) {
yuron 18:851f783ec516 2046 left_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2047 left_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2048 left_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2049 left_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 8:3df97287c825 2050 }
yuron 20:ac4954be1fe0 2051
yuron 14:ab89b6cd9719 2052 //よくわからんやつ
yuron 16:05b26003da50 2053 left_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2054 left_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2055 left_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2056 left_hidariusiro.setMode(AUTO_MODE);
yuron 10:b672aa81b226 2057
yuron 14:ab89b6cd9719 2058 //目標値
yuron 16:05b26003da50 2059 left_migimae.setSetPoint(target);
yuron 16:05b26003da50 2060 left_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 2061 left_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 2062 left_hidariusiro.setSetPoint(target);
yuron 9:1359f0c813b1 2063
yuron 14:ab89b6cd9719 2064 //センサ出力
yuron 16:05b26003da50 2065 left_migimae.setProcessValue(x_pulse1);
yuron 16:05b26003da50 2066 left_migiusiro.setProcessValue(x_pulse2);
yuron 16:05b26003da50 2067 left_hidarimae.setProcessValue(x_pulse1);
yuron 16:05b26003da50 2068 left_hidariusiro.setProcessValue(x_pulse2);
yuron 8:3df97287c825 2069
yuron 14:ab89b6cd9719 2070 //制御量(計算結果)
yuron 16:05b26003da50 2071 migimae_data[0] = left_migimae.compute();
yuron 16:05b26003da50 2072 migiusiro_data[0] = left_migiusiro.compute();
yuron 16:05b26003da50 2073 hidarimae_data[0] = left_hidarimae.compute();
yuron 16:05b26003da50 2074 hidariusiro_data[0] = left_hidariusiro.compute();
yuron 8:3df97287c825 2075
yuron 14:ab89b6cd9719 2076 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 2077 //左進(目標まで達していない)
yuron 22:5682246f9409 2078 if((x_pulse1 < target) && (x_pulse2 < target)) {
yuron 14:ab89b6cd9719 2079 true_migimae_data[0] = migimae_data[0];
yuron 14:ab89b6cd9719 2080 true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
yuron 14:ab89b6cd9719 2081 true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
yuron 14:ab89b6cd9719 2082 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 14:ab89b6cd9719 2083 }
yuron 18:851f783ec516 2084 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2085 else if((x_pulse1 > target) && (x_pulse2 > target)) {
yuron 18:851f783ec516 2086 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2087 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2088 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2089 true_hidariusiro_data[0] = 0x80;
yuron 17:de3bc1999ae7 2090 }
yuron 14:ab89b6cd9719 2091 }
yuron 12:1a22b9797004 2092
yuron 17:de3bc1999ae7 2093 void turn_right_PID(int target) {
yuron 14:ab89b6cd9719 2094
yuron 14:ab89b6cd9719 2095 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 2096 turn_right_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2097 turn_right_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2098 turn_right_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2099 turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 9:1359f0c813b1 2100
yuron 14:ab89b6cd9719 2101 //制御量の最小、最大
yuron 14:ab89b6cd9719 2102 //右旋回(目標に達してない)
yuron 22:5682246f9409 2103 if(sum_pulse < target) {
yuron 17:de3bc1999ae7 2104 turn_right_migimae.setOutputLimits(0x10, 0x7B);
yuron 17:de3bc1999ae7 2105 turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
yuron 17:de3bc1999ae7 2106 turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
yuron 17:de3bc1999ae7 2107 turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
yuron 8:3df97287c825 2108 }
yuron 18:851f783ec516 2109 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2110 else if(sum_pulse > target) {
yuron 18:851f783ec516 2111 turn_right_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2112 turn_right_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2113 turn_right_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2114 turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 4:df334779a69e 2115 }
yuron 8:3df97287c825 2116
yuron 14:ab89b6cd9719 2117 //よくわからんやつ
yuron 16:05b26003da50 2118 turn_right_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2119 turn_right_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2120 turn_right_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2121 turn_right_hidariusiro.setMode(AUTO_MODE);
yuron 8:3df97287c825 2122
yuron 14:ab89b6cd9719 2123 //目標値
yuron 16:05b26003da50 2124 turn_right_migimae.setSetPoint(target);
yuron 16:05b26003da50 2125 turn_right_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 2126 turn_right_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 2127 turn_right_hidariusiro.setSetPoint(target);
yuron 5:167327a82430 2128
yuron 14:ab89b6cd9719 2129 //センサ出力
yuron 22:5682246f9409 2130 turn_right_migimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2131 turn_right_migiusiro.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2132 turn_right_hidarimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2133 turn_right_hidariusiro.setProcessValue(sum_pulse);
yuron 5:167327a82430 2134
yuron 14:ab89b6cd9719 2135 //制御量(計算結果)
yuron 16:05b26003da50 2136 migimae_data[0] = turn_right_migimae.compute();
yuron 16:05b26003da50 2137 migiusiro_data[0] = turn_right_migiusiro.compute();
yuron 16:05b26003da50 2138 hidarimae_data[0] = turn_right_hidarimae.compute();
yuron 16:05b26003da50 2139 hidariusiro_data[0] = turn_right_hidariusiro.compute();
yuron 8:3df97287c825 2140
yuron 14:ab89b6cd9719 2141 //制御量をPWM値に変換
yuron 14:ab89b6cd9719 2142 //右旋回(目標に達してない)
yuron 22:5682246f9409 2143 if(sum_pulse < target) {
yuron 14:ab89b6cd9719 2144 true_migimae_data[0] = 0x7B - migimae_data[0];
yuron 14:ab89b6cd9719 2145 true_migiusiro_data[0] = 0x7B - migiusiro_data[0];
yuron 14:ab89b6cd9719 2146 true_hidarimae_data[0] = hidarimae_data[0];
yuron 14:ab89b6cd9719 2147 true_hidariusiro_data[0] = hidariusiro_data[0];
yuron 14:ab89b6cd9719 2148 }
yuron 18:851f783ec516 2149 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2150 else if(sum_pulse > target) {
yuron 18:851f783ec516 2151 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2152 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2153 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2154 true_hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 2155 }
yuron 14:ab89b6cd9719 2156 }
yuron 8:3df97287c825 2157
yuron 17:de3bc1999ae7 2158 void turn_left_PID(int target) {
yuron 20:ac4954be1fe0 2159
yuron 14:ab89b6cd9719 2160 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
yuron 16:05b26003da50 2161 turn_left_migimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2162 turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2163 turn_left_hidarimae.setInputLimits(-2147483648, 2147483647);
yuron 16:05b26003da50 2164 turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
yuron 9:1359f0c813b1 2165
yuron 14:ab89b6cd9719 2166 //制御量の最小、最大
yuron 18:851f783ec516 2167 //左旋回(目標に達してない)
yuron 22:5682246f9409 2168 if(sum_pulse < target) {
yuron 17:de3bc1999ae7 2169 turn_left_migimae.setOutputLimits(0x94, 0xFF);
yuron 17:de3bc1999ae7 2170 turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
yuron 17:de3bc1999ae7 2171 turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
yuron 17:de3bc1999ae7 2172 turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
yuron 14:ab89b6cd9719 2173 }
yuron 18:851f783ec516 2174 //停止(目標より行き過ぎ)
yuron 22:5682246f9409 2175 else if(sum_pulse > target) {
yuron 18:851f783ec516 2176 turn_left_migimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2177 turn_left_migiusiro.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2178 turn_left_hidarimae.setOutputLimits(0x7C, 0x83);
yuron 18:851f783ec516 2179 turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
yuron 7:7f16fb8b0192 2180 }
yuron 8:3df97287c825 2181
yuron 14:ab89b6cd9719 2182 //よくわからんやつ
yuron 16:05b26003da50 2183 turn_left_migimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2184 turn_left_migiusiro.setMode(AUTO_MODE);
yuron 16:05b26003da50 2185 turn_left_hidarimae.setMode(AUTO_MODE);
yuron 16:05b26003da50 2186 turn_left_hidariusiro.setMode(AUTO_MODE);
yuron 5:167327a82430 2187
yuron 14:ab89b6cd9719 2188 //目標値
yuron 16:05b26003da50 2189 turn_left_migimae.setSetPoint(target);
yuron 16:05b26003da50 2190 turn_left_migiusiro.setSetPoint(target);
yuron 16:05b26003da50 2191 turn_left_hidarimae.setSetPoint(target);
yuron 16:05b26003da50 2192 turn_left_hidariusiro.setSetPoint(target);
yuron 8:3df97287c825 2193
yuron 14:ab89b6cd9719 2194 //センサ出力
yuron 22:5682246f9409 2195 turn_left_migimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2196 turn_left_migiusiro.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2197 turn_left_hidarimae.setProcessValue(sum_pulse);
yuron 22:5682246f9409 2198 turn_left_hidariusiro.setProcessValue(sum_pulse);
yuron 5:167327a82430 2199
yuron 14:ab89b6cd9719 2200 //制御量(計算結果)
yuron 16:05b26003da50 2201 migimae_data[0] = turn_left_migimae.compute();
yuron 16:05b26003da50 2202 migiusiro_data[0] = turn_left_migiusiro.compute();
yuron 16:05b26003da50 2203 hidarimae_data[0] = turn_left_hidarimae.compute();
yuron 16:05b26003da50 2204 hidariusiro_data[0] = turn_left_hidariusiro.compute();
yuron 5:167327a82430 2205
yuron 14:ab89b6cd9719 2206 //制御量をPWM値に変換
yuron 18:851f783ec516 2207 //左旋回(目標に達してない)
yuron 22:5682246f9409 2208 if(sum_pulse < target) {
yuron 14:ab89b6cd9719 2209 true_migimae_data[0] = migimae_data[0];
yuron 14:ab89b6cd9719 2210 true_migiusiro_data[0] = migiusiro_data[0];
yuron 14:ab89b6cd9719 2211 true_hidarimae_data[0] = 0x7B - hidarimae_data[0];
yuron 14:ab89b6cd9719 2212 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
yuron 14:ab89b6cd9719 2213 }
yuron 14:ab89b6cd9719 2214 //左旋回(目標より行き過ぎ)
yuron 22:5682246f9409 2215 else if(sum_pulse > target) {
yuron 18:851f783ec516 2216 true_migimae_data[0] = 0x80;
yuron 18:851f783ec516 2217 true_migiusiro_data[0] = 0x80;
yuron 18:851f783ec516 2218 true_hidarimae_data[0] = 0x80;
yuron 18:851f783ec516 2219 true_hidariusiro_data[0] = 0x80;
yuron 14:ab89b6cd9719 2220 }
yuron 14:ab89b6cd9719 2221 }