2019NHK_teamA
/
2019_A_ver6-2_fixed_phase1
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
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?
User | Revision | Line number | New 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 | } |