2019NHK_teamA / Mbed 2 deprecated 2019_A_ver6-2_fixed_phase1

ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数

Dependencies:   mbed QEI PID

main.cpp@20:ac4954be1fe0, 2019-09-18 (annotated)

Committer:
yuron
Date:
Wed Sep 18 03:36:25 2019 +0000
Revision:
20:ac4954be1fe0
Parent:
19:f17d2e585973
Child:
21:123520676121
aaa

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