2019NHK_teamA / Mbed 2 deprecated 2019_A_ver7-5

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

Dependencies:   mbed QEI PID

main.cpp@22:5682246f9409, 2019-09-25 (annotated)

Committer:
yuron
Date:
Wed Sep 25 02:07:26 2019 +0000
Revision:
22:5682246f9409
Parent:
21:89db2a19e52e
Child:
23:1e4d7540715f
aaaaaaa;

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