2019NHK_teamA / Mbed 2 deprecated 2019_A_ver7-4

Dependencies:   mbed QEI PID

main.cpp@21:89db2a19e52e, 2019-09-21 (annotated)

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