2019NHK_teamA / Mbed 2 deprecated 2019_A_ver6-2_fixed_phase1

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

Dependencies:   mbed QEI PID

main.cpp@21:123520676121, 2019-09-19 (annotated)

Committer:
st17099ng
Date:
Thu Sep 19 07:35:49 2019 +0000
Revision:
21:123520676121
Parent:
20:ac4954be1fe0 
2019/09/18fixed; change point; phase1; kaisyu; tyokudo

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