2019NHK_teamA / Mbed 2 deprecated 2019_A_ver7-4

2019年9月30日AM02:00現在のもの

Dependencies:   mbed QEI PID

main.cpp@19:f17d2e585973, 2019-09-17 (annotated)

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