2019NHK_teamA
/
NHK2019robokon_11_19
NHK2019 manual program(回収機構ストール問題解決)
main.cpp@5:29971215ef50, 2019-10-26 (annotated)
- Committer:
- shina
- Date:
- Sat Oct 26 04:30:54 2019 +0000
- Revision:
- 5:29971215ef50
- Parent:
- 4:4d9c0999a90a
- Child:
- 7:c423a6579c3c
NHK 2019 manual program 10/26 version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shina | 0:c7e17c2fd542 | 1 | ///////////////////////////////////// |
shina | 0:c7e17c2fd542 | 2 | /*NHK2019*/ |
shina | 1:99294241f2ba | 3 | //Aチーム手動機プログラム |
shina | 0:c7e17c2fd542 | 4 | /* |
shina | 0:c7e17c2fd542 | 5 | アドレス |
shina | 0:c7e17c2fd542 | 6 | 0x10:右前 |
shina | 1:99294241f2ba | 7 | 0x12:左後ろ |
shina | 0:c7e17c2fd542 | 8 | 0x14:右後ろ |
shina | 1:99294241f2ba | 9 | 0x16:左前 |
shina | 0:c7e17c2fd542 | 10 | 0x18:サーボ制御回路 |
shina | 0:c7e17c2fd542 | 11 | 0x20:右ラック |
shina | 4:4d9c0999a90a | 12 | 0x22:左ラック. |
shina | 0:c7e17c2fd542 | 13 | 0x24:回収機構 |
shina | 0:c7e17c2fd542 | 14 | 0x26:右ファン |
shina | 0:c7e17c2fd542 | 15 | 0x28:左ファン |
shina | 1:99294241f2ba | 16 | 0x30:吐き出し機構 |
shina | 0:c7e17c2fd542 | 17 | */ |
shina | 0:c7e17c2fd542 | 18 | ///////////////////////////////////// |
shina | 0:c7e17c2fd542 | 19 | |
shina | 0:c7e17c2fd542 | 20 | //宣言 |
shina | 0:c7e17c2fd542 | 21 | #include "mbed.h" |
shina | 0:c7e17c2fd542 | 22 | #include "PS3.h" |
shina | 0:c7e17c2fd542 | 23 | I2C i2c(D14,D15); |
shina | 0:c7e17c2fd542 | 24 | Serial pc(USBTX,USBRX); |
shina | 0:c7e17c2fd542 | 25 | Serial slave(PC_6,PC_7); |
shina | 0:c7e17c2fd542 | 26 | PS3 ps3(D8,D2); |
shina | 0:c7e17c2fd542 | 27 | DigitalOut led(D5);//電源確認 |
shina | 0:c7e17c2fd542 | 28 | DigitalOut tsushin(D6);//通信確認 |
shina | 0:c7e17c2fd542 | 29 | DigitalOut data_check(D7); |
shina | 0:c7e17c2fd542 | 30 | DigitalOut pwm(D4); |
shina | 0:c7e17c2fd542 | 31 | DigitalOut stop(D10); |
shina | 0:c7e17c2fd542 | 32 | DigitalOut led1(LED1); |
shina | 5:29971215ef50 | 33 | //DigitalOut aaaaa(D9); |
shina | 4:4d9c0999a90a | 34 | Timer timer; |
shina | 4:4d9c0999a90a | 35 | Timer timer2; |
shina | 1:99294241f2ba | 36 | |
shina | 0:c7e17c2fd542 | 37 | //変数 |
shina | 0:c7e17c2fd542 | 38 | char data1;//右上 |
shina | 0:c7e17c2fd542 | 39 | char data2;//左上 |
shina | 0:c7e17c2fd542 | 40 | char data3;//右下 |
shina | 0:c7e17c2fd542 | 41 | char data4;//左下 |
shina | 0:c7e17c2fd542 | 42 | char data_servo; |
shina | 0:c7e17c2fd542 | 43 | char data5;//ラック直動右 |
shina | 0:c7e17c2fd542 | 44 | char data6;//ラック直動左 |
shina | 0:c7e17c2fd542 | 45 | char data7;//回収機構 |
shina | 0:c7e17c2fd542 | 46 | char data8;//右ファン |
shina | 0:c7e17c2fd542 | 47 | char data9;//左ファン |
shina | 1:99294241f2ba | 48 | char data10;//パトランプ |
shina | 1:99294241f2ba | 49 | char data11;//吐き出し機構 |
yosino_adati | 2:e462c8257384 | 50 | char get_data_rs232=0x00; |
shina | 0:c7e17c2fd542 | 51 | int Ry; |
shina | 0:c7e17c2fd542 | 52 | int Rx; |
shina | 1:99294241f2ba | 53 | int Ly; |
shina | 5:29971215ef50 | 54 | bool right1; |
shina | 5:29971215ef50 | 55 | bool right2; |
shina | 5:29971215ef50 | 56 | bool left1; |
shina | 5:29971215ef50 | 57 | bool left2; |
shina | 5:29971215ef50 | 58 | bool select; |
shina | 5:29971215ef50 | 59 | bool start; |
shina | 5:29971215ef50 | 60 | bool circle; |
shina | 5:29971215ef50 | 61 | bool cross; |
shina | 5:29971215ef50 | 62 | bool triangle; |
shina | 5:29971215ef50 | 63 | bool square; |
shina | 5:29971215ef50 | 64 | bool ue1; |
shina | 5:29971215ef50 | 65 | bool shita; |
shina | 5:29971215ef50 | 66 | bool migi1; |
shina | 5:29971215ef50 | 67 | bool hidari1; |
shina | 0:c7e17c2fd542 | 68 | int tushin_check; |
shina | 0:c7e17c2fd542 | 69 | int old_select=0; |
shina | 0:c7e17c2fd542 | 70 | int i=1; |
shina | 0:c7e17c2fd542 | 71 | int old_start=0; |
shina | 0:c7e17c2fd542 | 72 | int j=1; |
shina | 0:c7e17c2fd542 | 73 | int old_circle=0; |
shina | 0:c7e17c2fd542 | 74 | int k=1; |
shina | 1:99294241f2ba | 75 | int old_square=0; |
shina | 1:99294241f2ba | 76 | int old_migi1=0; |
shina | 1:99294241f2ba | 77 | int old_hidari1=0; |
shina | 1:99294241f2ba | 78 | int n=1; |
shina | 1:99294241f2ba | 79 | int old_right2=0; |
shina | 1:99294241f2ba | 80 | int o=1; |
shina | 1:99294241f2ba | 81 | int old_right1=0; |
shina | 1:99294241f2ba | 82 | int old_left1=0; |
shina | 4:4d9c0999a90a | 83 | int p=5; |
shina | 1:99294241f2ba | 84 | int q=0; |
shina | 3:8c4e42cba9cb | 85 | int old_ue1=0; |
shina | 3:8c4e42cba9cb | 86 | int old_shita=0; |
shina | 3:8c4e42cba9cb | 87 | int old_left2=0; |
shina | 3:8c4e42cba9cb | 88 | int old_triangle=0; |
shina | 3:8c4e42cba9cb | 89 | int old_cross=0; |
shina | 4:4d9c0999a90a | 90 | int r=6; |
shina | 4:4d9c0999a90a | 91 | int s=5; |
shina | 4:4d9c0999a90a | 92 | bool x=0; |
shina | 4:4d9c0999a90a | 93 | bool y=0; |
shina | 5:29971215ef50 | 94 | bool kaisyu_arm=0; |
shina | 5:29971215ef50 | 95 | char get_data_ps3[8]; |
shina | 5:29971215ef50 | 96 | |
shina | 0:c7e17c2fd542 | 97 | |
shina | 0:c7e17c2fd542 | 98 | |
shina | 0:c7e17c2fd542 | 99 | //関数プロトタイプ宣言 |
shina | 1:99294241f2ba | 100 | void initialization(); |
shina | 0:c7e17c2fd542 | 101 | void get_data(); |
shina | 0:c7e17c2fd542 | 102 | void change_data(); |
shina | 0:c7e17c2fd542 | 103 | void change_pwm(); |
shina | 0:c7e17c2fd542 | 104 | void change_servo(); |
shina | 0:c7e17c2fd542 | 105 | void change_fan(); |
shina | 0:c7e17c2fd542 | 106 | void send_data(char address,char data); |
shina | 0:c7e17c2fd542 | 107 | void emergency(); |
shina | 1:99294241f2ba | 108 | void change_rack_2(); |
shina | 4:4d9c0999a90a | 109 | //void sequence_kaisyu(); |
shina | 4:4d9c0999a90a | 110 | void kaisyu(); |
shina | 4:4d9c0999a90a | 111 | //void sequence_kaisyu_2(); |
shina | 4:4d9c0999a90a | 112 | void hakidashi(); |
shina | 3:8c4e42cba9cb | 113 | void mode_change(); |
shina | 1:99294241f2ba | 114 | |
shina | 1:99294241f2ba | 115 | |
shina | 0:c7e17c2fd542 | 116 | |
shina | 0:c7e17c2fd542 | 117 | //メイン関数 |
yosino_adati | 2:e462c8257384 | 118 | int main() |
shina | 5:29971215ef50 | 119 | { //pc.printf("hi\n"); |
shina | 0:c7e17c2fd542 | 120 | led=1; |
shina | 3:8c4e42cba9cb | 121 | pwm=1; |
shina | 5:29971215ef50 | 122 | i2c.frequency(100000); |
shina | 5:29971215ef50 | 123 | slave.baud(115200); |
shina | 5:29971215ef50 | 124 | pc.baud(115200); |
shina | 1:99294241f2ba | 125 | initialization(); |
shina | 5:29971215ef50 | 126 | |
shina | 5:29971215ef50 | 127 | //pc.printf("ha\n"); |
yosino_adati | 2:e462c8257384 | 128 | while(true) { |
shina | 5:29971215ef50 | 129 | //pc.printf("hello\n"); |
shina | 5:29971215ef50 | 130 | //aaaaa=1; |
shina | 0:c7e17c2fd542 | 131 | emergency(); |
shina | 0:c7e17c2fd542 | 132 | get_data(); |
shina | 3:8c4e42cba9cb | 133 | mode_change(); |
shina | 0:c7e17c2fd542 | 134 | change_pwm(); |
shina | 4:4d9c0999a90a | 135 | change_data(); |
shina | 0:c7e17c2fd542 | 136 | change_servo(); |
shina | 1:99294241f2ba | 137 | change_rack_2(); |
shina | 4:4d9c0999a90a | 138 | //sequence_kaisyu(); |
shina | 4:4d9c0999a90a | 139 | kaisyu(); |
shina | 4:4d9c0999a90a | 140 | //sequence_kaisyu_2(); |
shina | 4:4d9c0999a90a | 141 | hakidashi(); |
shina | 0:c7e17c2fd542 | 142 | change_fan(); |
shina | 0:c7e17c2fd542 | 143 | send_data(0x10,data1); |
shina | 0:c7e17c2fd542 | 144 | send_data(0x12,data2); |
shina | 0:c7e17c2fd542 | 145 | send_data(0x14,data3); |
shina | 0:c7e17c2fd542 | 146 | send_data(0x16,data4); |
shina | 4:4d9c0999a90a | 147 | //send_data(0x18,data_servo); |
shina | 0:c7e17c2fd542 | 148 | send_data(0x20,data5); |
shina | 0:c7e17c2fd542 | 149 | send_data(0x22,data6); |
shina | 1:99294241f2ba | 150 | //send_data(0x24,data7); |
shina | 0:c7e17c2fd542 | 151 | send_data(0x26,data8); |
shina | 0:c7e17c2fd542 | 152 | send_data(0x28,data9); |
shina | 1:99294241f2ba | 153 | send_data(0x40,data10); |
shina | 1:99294241f2ba | 154 | //send_data(0x30,data11); |
yosino_adati | 2:e462c8257384 | 155 | |
shina | 5:29971215ef50 | 156 | //aaaaa=0; |
shina | 5:29971215ef50 | 157 | //pc.printf("world\n"); |
shina | 1:99294241f2ba | 158 | |
shina | 0:c7e17c2fd542 | 159 | } |
yosino_adati | 2:e462c8257384 | 160 | } |
yosino_adati | 2:e462c8257384 | 161 | |
yosino_adati | 2:e462c8257384 | 162 | |
shina | 1:99294241f2ba | 163 | //初期化 |
yosino_adati | 2:e462c8257384 | 164 | void initialization() |
yosino_adati | 2:e462c8257384 | 165 | { |
shina | 1:99294241f2ba | 166 | data1=0x80; |
shina | 1:99294241f2ba | 167 | data2=0x80; |
shina | 1:99294241f2ba | 168 | data3=0x80; |
shina | 1:99294241f2ba | 169 | data4=0x80; |
shina | 1:99294241f2ba | 170 | data5=0x80; |
shina | 1:99294241f2ba | 171 | data6=0x80; |
shina | 1:99294241f2ba | 172 | data7=0x10; |
shina | 1:99294241f2ba | 173 | data8=0x80; |
shina | 1:99294241f2ba | 174 | data9=0x80; |
shina | 1:99294241f2ba | 175 | data10=0x80; |
shina | 1:99294241f2ba | 176 | data11=0x80; |
shina | 4:4d9c0999a90a | 177 | data_servo=0x00; |
yosino_adati | 2:e462c8257384 | 178 | |
shina | 1:99294241f2ba | 179 | send_data(0x10,data1); |
shina | 1:99294241f2ba | 180 | send_data(0x12,data2); |
shina | 1:99294241f2ba | 181 | send_data(0x14,data3); |
shina | 1:99294241f2ba | 182 | send_data(0x16,data4); |
shina | 1:99294241f2ba | 183 | send_data(0x20,data5); |
shina | 1:99294241f2ba | 184 | send_data(0x22,data6); |
shina | 1:99294241f2ba | 185 | send_data(0x24,data7); |
shina | 1:99294241f2ba | 186 | send_data(0x26,data8); |
shina | 1:99294241f2ba | 187 | send_data(0x28,data9); |
shina | 1:99294241f2ba | 188 | send_data(0x40,data10); |
shina | 1:99294241f2ba | 189 | send_data(0x30,data11); |
shina | 4:4d9c0999a90a | 190 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 191 | |
yosino_adati | 2:e462c8257384 | 192 | |
shina | 3:8c4e42cba9cb | 193 | data10=0x3f; |
shina | 3:8c4e42cba9cb | 194 | send_data(0x40,data10); |
shina | 3:8c4e42cba9cb | 195 | |
shina | 3:8c4e42cba9cb | 196 | if(q==0){ |
shina | 3:8c4e42cba9cb | 197 | data10=0xdf; |
shina | 3:8c4e42cba9cb | 198 | send_data(0x40,data10); |
shina | 3:8c4e42cba9cb | 199 | }else if(q==1){ |
shina | 3:8c4e42cba9cb | 200 | data10=0xcf; |
shina | 3:8c4e42cba9cb | 201 | send_data(0x40,data10); |
shina | 3:8c4e42cba9cb | 202 | } |
shina | 3:8c4e42cba9cb | 203 | |
shina | 4:4d9c0999a90a | 204 | /*data_servo=0x03; |
shina | 4:4d9c0999a90a | 205 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 206 | data_servo=0x04; |
shina | 4:4d9c0999a90a | 207 | send_data(0x18,data_servo);*/ |
shina | 5:29971215ef50 | 208 | //timer2.reset(); |
shina | 3:8c4e42cba9cb | 209 | |
yosino_adati | 2:e462c8257384 | 210 | } |
shina | 1:99294241f2ba | 211 | |
yosino_adati | 2:e462c8257384 | 212 | |
shina | 0:c7e17c2fd542 | 213 | //データ読み込み |
yosino_adati | 2:e462c8257384 | 214 | void get_data() |
yosino_adati | 2:e462c8257384 | 215 | { |
shina | 5:29971215ef50 | 216 | int w; |
shina | 5:29971215ef50 | 217 | //int switch_judge=0; |
shina | 5:29971215ef50 | 218 | /* |
shina | 0:c7e17c2fd542 | 219 | Ry=ps3.getRightJoystickYaxis(); |
shina | 0:c7e17c2fd542 | 220 | Rx=ps3.getRightJoystickXaxis(); |
shina | 1:99294241f2ba | 221 | Ly=ps3.getLeftJoystickYaxis(); |
shina | 0:c7e17c2fd542 | 222 | circle=ps3.getButtonState(maru); |
shina | 0:c7e17c2fd542 | 223 | cross=ps3.getButtonState(batu); |
shina | 1:99294241f2ba | 224 | triangle=ps3.getButtonState(sankaku); |
shina | 1:99294241f2ba | 225 | square=ps3.getButtonState(sikaku); |
shina | 0:c7e17c2fd542 | 226 | left1=ps3.getButtonState(L1); |
shina | 0:c7e17c2fd542 | 227 | left2=ps3.getButtonState(L2); |
shina | 0:c7e17c2fd542 | 228 | right1=ps3.getButtonState(R1); |
shina | 1:99294241f2ba | 229 | right2=ps3.getButtonState(R2); |
shina | 0:c7e17c2fd542 | 230 | select=ps3.getSELECTState(); |
shina | 0:c7e17c2fd542 | 231 | start=ps3.getSTARTState(); |
shina | 0:c7e17c2fd542 | 232 | ue1=ps3.getButtonState(ue); |
shina | 0:c7e17c2fd542 | 233 | shita=ps3.getButtonState(sita); |
shina | 0:c7e17c2fd542 | 234 | hidari1=ps3.getButtonState(hidari); |
shina | 0:c7e17c2fd542 | 235 | migi1=ps3.getButtonState(migi); |
shina | 5:29971215ef50 | 236 | */ |
shina | 5:29971215ef50 | 237 | for(w=0;w<8;w++){ |
shina | 5:29971215ef50 | 238 | get_data_ps3[w]=ps3.PS3Data[w]; |
shina | 5:29971215ef50 | 239 | } |
shina | 5:29971215ef50 | 240 | // get_data_ps3[0]=(*(+(button>>4)) >> (button & 0x0f)) & 1; |
shina | 5:29971215ef50 | 241 | |
shina | 5:29971215ef50 | 242 | switch(get_data_ps3[2] & 0x0c){ |
shina | 5:29971215ef50 | 243 | case 0x0c: |
shina | 5:29971215ef50 | 244 | select=true; |
shina | 5:29971215ef50 | 245 | migi1=false; |
shina | 5:29971215ef50 | 246 | hidari1=false; |
shina | 5:29971215ef50 | 247 | break; |
shina | 5:29971215ef50 | 248 | case 0x04: |
shina | 5:29971215ef50 | 249 | select=false; |
shina | 5:29971215ef50 | 250 | migi1=true; |
shina | 5:29971215ef50 | 251 | hidari1=false; |
shina | 5:29971215ef50 | 252 | break; |
shina | 5:29971215ef50 | 253 | case 0x08: |
shina | 5:29971215ef50 | 254 | select=false; |
shina | 5:29971215ef50 | 255 | migi1=false; |
shina | 5:29971215ef50 | 256 | hidari1=true; |
shina | 5:29971215ef50 | 257 | break; |
shina | 5:29971215ef50 | 258 | default: |
shina | 5:29971215ef50 | 259 | select=false; |
shina | 5:29971215ef50 | 260 | migi1=false; |
shina | 5:29971215ef50 | 261 | hidari1=false; |
shina | 5:29971215ef50 | 262 | } |
shina | 5:29971215ef50 | 263 | |
shina | 5:29971215ef50 | 264 | switch(get_data_ps3[2] & 0x03){ |
shina | 5:29971215ef50 | 265 | case 0x03: |
shina | 5:29971215ef50 | 266 | start=true; |
shina | 5:29971215ef50 | 267 | ue1=false; |
shina | 5:29971215ef50 | 268 | shita=false; |
shina | 5:29971215ef50 | 269 | break; |
shina | 5:29971215ef50 | 270 | case 0x01: |
shina | 5:29971215ef50 | 271 | start=false; |
shina | 5:29971215ef50 | 272 | ue1=true; |
shina | 5:29971215ef50 | 273 | shita=false; |
shina | 5:29971215ef50 | 274 | break; |
shina | 5:29971215ef50 | 275 | case 0x02: |
shina | 5:29971215ef50 | 276 | start=false; |
shina | 5:29971215ef50 | 277 | ue1=false; |
shina | 5:29971215ef50 | 278 | shita=true; |
shina | 5:29971215ef50 | 279 | break; |
shina | 5:29971215ef50 | 280 | default: |
shina | 5:29971215ef50 | 281 | start=false; |
shina | 5:29971215ef50 | 282 | ue1=false; |
shina | 5:29971215ef50 | 283 | shita=false; |
shina | 5:29971215ef50 | 284 | } |
shina | 5:29971215ef50 | 285 | |
shina | 5:29971215ef50 | 286 | triangle=bool(get_data_ps3[2] & 0x10); |
shina | 5:29971215ef50 | 287 | cross=bool(get_data_ps3[2] & 0x20); |
shina | 5:29971215ef50 | 288 | circle=bool(get_data_ps3[2] & 0x40); |
shina | 5:29971215ef50 | 289 | square=bool(get_data_ps3[1] & 0x01); |
shina | 5:29971215ef50 | 290 | left1=bool(get_data_ps3[1] & 0x02); |
shina | 5:29971215ef50 | 291 | left2=bool(get_data_ps3[1] & 0x04); |
shina | 5:29971215ef50 | 292 | right1=bool(get_data_ps3[1] & 0x08); |
shina | 5:29971215ef50 | 293 | right2=bool(get_data_ps3[1] & 0x10); |
shina | 5:29971215ef50 | 294 | Ry=(int)get_data_ps3[6]*-1+64; |
shina | 5:29971215ef50 | 295 | Rx=(int)get_data_ps3[5]-64; |
shina | 5:29971215ef50 | 296 | |
shina | 5:29971215ef50 | 297 | |
shina | 5:29971215ef50 | 298 | //pc.printf("a\n"); |
shina | 5:29971215ef50 | 299 | if(slave.readable()==1){ |
shina | 5:29971215ef50 | 300 | //pc.printf("c\n"); |
shina | 1:99294241f2ba | 301 | get_data_rs232=slave.getc(); |
shina | 5:29971215ef50 | 302 | //pc.printf("d\n"); |
shina | 5:29971215ef50 | 303 | } |
shina | 5:29971215ef50 | 304 | //pc.printf("b\n"); |
yosino_adati | 2:e462c8257384 | 305 | |
shina | 1:99294241f2ba | 306 | |
yosino_adati | 2:e462c8257384 | 307 | if(get_data_rs232) { |
shina | 1:99294241f2ba | 308 | led1=1; |
yosino_adati | 2:e462c8257384 | 309 | } else { |
yosino_adati | 2:e462c8257384 | 310 | led1=0; |
yosino_adati | 2:e462c8257384 | 311 | } |
shina | 4:4d9c0999a90a | 312 | //pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,Ly,left1,left2,right1,right2,select,start,ue1,shita,triangle,square,migi1,hidari1,tushin_check,i); |
shina | 4:4d9c0999a90a | 313 | //pc.printf("0x%x\n",get_data_rs232); |
shina | 4:4d9c0999a90a | 314 | //pc.printf("%d\n",x); |
shina | 5:29971215ef50 | 315 | //pc.printf("%d,%d,%d\n",x,y,p); |
yosino_adati | 2:e462c8257384 | 316 | if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0) { |
shina | 0:c7e17c2fd542 | 317 | data_check=0; |
yosino_adati | 2:e462c8257384 | 318 | } else { |
yosino_adati | 2:e462c8257384 | 319 | data_check=1; |
shina | 0:c7e17c2fd542 | 320 | } |
yosino_adati | 2:e462c8257384 | 321 | |
yosino_adati | 2:e462c8257384 | 322 | } |
yosino_adati | 2:e462c8257384 | 323 | |
shina | 0:c7e17c2fd542 | 324 | //緊急停止 |
yosino_adati | 2:e462c8257384 | 325 | void emergency() |
yosino_adati | 2:e462c8257384 | 326 | { |
yosino_adati | 2:e462c8257384 | 327 | if(start!=old_start) { |
yosino_adati | 2:e462c8257384 | 328 | old_start=start; |
yosino_adati | 2:e462c8257384 | 329 | if(start==1) { |
yosino_adati | 2:e462c8257384 | 330 | if(j==1) { |
yosino_adati | 2:e462c8257384 | 331 | //緊急停止 |
yosino_adati | 2:e462c8257384 | 332 | led=0; |
yosino_adati | 2:e462c8257384 | 333 | stop=1; |
yosino_adati | 2:e462c8257384 | 334 | j=0; |
yosino_adati | 2:e462c8257384 | 335 | } else if(j==0) { |
yosino_adati | 2:e462c8257384 | 336 | //緊急停止解除 |
yosino_adati | 2:e462c8257384 | 337 | led=1; |
yosino_adati | 2:e462c8257384 | 338 | stop=0; |
yosino_adati | 2:e462c8257384 | 339 | j=1; |
shina | 0:c7e17c2fd542 | 340 | } |
shina | 0:c7e17c2fd542 | 341 | } |
yosino_adati | 2:e462c8257384 | 342 | } |
shina | 0:c7e17c2fd542 | 343 | } |
shina | 0:c7e17c2fd542 | 344 | |
yosino_adati | 2:e462c8257384 | 345 | |
shina | 3:8c4e42cba9cb | 346 | //モードチェンジ |
shina | 3:8c4e42cba9cb | 347 | void mode_change(){ |
shina | 3:8c4e42cba9cb | 348 | if(old_select!=select){ |
shina | 3:8c4e42cba9cb | 349 | old_select=select; |
shina | 3:8c4e42cba9cb | 350 | if(select==1){ |
shina | 3:8c4e42cba9cb | 351 | if(q==0){ |
shina | 3:8c4e42cba9cb | 352 | //送風モード |
shina | 3:8c4e42cba9cb | 353 | q=1; |
shina | 4:4d9c0999a90a | 354 | /* data_servo=0x03; |
shina | 4:4d9c0999a90a | 355 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 356 | data_servo=0x04; |
shina | 4:4d9c0999a90a | 357 | send_data(0x18,data_servo);*/ |
shina | 3:8c4e42cba9cb | 358 | data10=0xcf; |
shina | 3:8c4e42cba9cb | 359 | send_data(0x40,data10); |
shina | 3:8c4e42cba9cb | 360 | }else if(q==1){ |
shina | 3:8c4e42cba9cb | 361 | //回収モード |
shina | 3:8c4e42cba9cb | 362 | q=0; |
shina | 3:8c4e42cba9cb | 363 | data10=0xdf; |
shina | 3:8c4e42cba9cb | 364 | send_data(0x40,data10); |
shina | 3:8c4e42cba9cb | 365 | } |
shina | 3:8c4e42cba9cb | 366 | } |
shina | 3:8c4e42cba9cb | 367 | } |
shina | 3:8c4e42cba9cb | 368 | } |
shina | 3:8c4e42cba9cb | 369 | |
shina | 0:c7e17c2fd542 | 370 | //データ変化(メカナム) |
yosino_adati | 2:e462c8257384 | 371 | void change_data() |
yosino_adati | 2:e462c8257384 | 372 | { |
yosino_adati | 2:e462c8257384 | 373 | |
yosino_adati | 2:e462c8257384 | 374 | if(Ry!=0||Rx!=0) { |
shina | 1:99294241f2ba | 375 | square=0; |
shina | 3:8c4e42cba9cb | 376 | migi1=0; |
yosino_adati | 2:e462c8257384 | 377 | } |
yosino_adati | 2:e462c8257384 | 378 | |
yosino_adati | 2:e462c8257384 | 379 | if(Ry>30&&i==0) { |
yosino_adati | 2:e462c8257384 | 380 | if(right1==1) { |
yosino_adati | 2:e462c8257384 | 381 | data1=0xcf; |
yosino_adati | 2:e462c8257384 | 382 | data2=0x00; |
yosino_adati | 2:e462c8257384 | 383 | data3=0xcf; |
yosino_adati | 2:e462c8257384 | 384 | data4=0x00; |
yosino_adati | 2:e462c8257384 | 385 | } else if(left1==1) { |
yosino_adati | 2:e462c8257384 | 386 | data1=0x00; |
yosino_adati | 2:e462c8257384 | 387 | data2=0xcf; |
yosino_adati | 2:e462c8257384 | 388 | data3=0x00; |
yosino_adati | 2:e462c8257384 | 389 | data4=0xcf; |
yosino_adati | 2:e462c8257384 | 390 | } else { |
yosino_adati | 2:e462c8257384 | 391 | data1=0x00; |
yosino_adati | 2:e462c8257384 | 392 | data2=0x00; |
yosino_adati | 2:e462c8257384 | 393 | data3=0x00; |
yosino_adati | 2:e462c8257384 | 394 | data4=0x00; |
shina | 1:99294241f2ba | 395 | } |
yosino_adati | 2:e462c8257384 | 396 | } else if(Ry<-30&&i==0) { |
shina | 3:8c4e42cba9cb | 397 | if(right1==1){ |
shina | 3:8c4e42cba9cb | 398 | data1=0xdf; |
shina | 3:8c4e42cba9cb | 399 | data2=0xff; |
shina | 3:8c4e42cba9cb | 400 | data3=0xdf; |
shina | 3:8c4e42cba9cb | 401 | data4=0xff; |
shina | 3:8c4e42cba9cb | 402 | }else if(left1==1){ |
shina | 3:8c4e42cba9cb | 403 | data1=0xff; |
shina | 3:8c4e42cba9cb | 404 | data2=0xdf; |
shina | 3:8c4e42cba9cb | 405 | data3=0xff; |
shina | 3:8c4e42cba9cb | 406 | data4=0xdf; |
shina | 3:8c4e42cba9cb | 407 | }else{ |
shina | 3:8c4e42cba9cb | 408 | data1=0xff; |
shina | 3:8c4e42cba9cb | 409 | data2=0xff; |
shina | 3:8c4e42cba9cb | 410 | data3=0xff; |
shina | 3:8c4e42cba9cb | 411 | data4=0xff; |
shina | 3:8c4e42cba9cb | 412 | } |
yosino_adati | 2:e462c8257384 | 413 | } else if(Rx>30&&i==0) { |
yosino_adati | 2:e462c8257384 | 414 | if(right1==1) { |
yosino_adati | 2:e462c8257384 | 415 | data1=0xff; |
yosino_adati | 2:e462c8257384 | 416 | data2=0xdf; |
yosino_adati | 2:e462c8257384 | 417 | data3=0xcf; |
yosino_adati | 2:e462c8257384 | 418 | data4=0x00; |
yosino_adati | 2:e462c8257384 | 419 | } else if(left1==1) { |
yosino_adati | 2:e462c8257384 | 420 | data1=0xdf; |
yosino_adati | 2:e462c8257384 | 421 | data2=0xff; |
yosino_adati | 2:e462c8257384 | 422 | data3=0x00; |
yosino_adati | 2:e462c8257384 | 423 | data4=0xcf; |
yosino_adati | 2:e462c8257384 | 424 | } else { |
yosino_adati | 2:e462c8257384 | 425 | data1=0xff; |
yosino_adati | 2:e462c8257384 | 426 | data2=0xff; |
yosino_adati | 2:e462c8257384 | 427 | data3=0x00; |
yosino_adati | 2:e462c8257384 | 428 | data4=0x00; |
shina | 1:99294241f2ba | 429 | } |
yosino_adati | 2:e462c8257384 | 430 | } else if(Rx<-30&&i==0) { |
yosino_adati | 2:e462c8257384 | 431 | if(right1==1) { |
yosino_adati | 2:e462c8257384 | 432 | data1=0xcf; |
yosino_adati | 2:e462c8257384 | 433 | data2=0x00; |
yosino_adati | 2:e462c8257384 | 434 | data3=0xff; |
yosino_adati | 2:e462c8257384 | 435 | data4=0xdf; |
yosino_adati | 2:e462c8257384 | 436 | } else if(left1==1) { |
yosino_adati | 2:e462c8257384 | 437 | data1=0x00; |
yosino_adati | 2:e462c8257384 | 438 | data2=0xcf; |
yosino_adati | 2:e462c8257384 | 439 | data3=0xdf; |
yosino_adati | 2:e462c8257384 | 440 | data4=0xff; |
yosino_adati | 2:e462c8257384 | 441 | } else { |
yosino_adati | 2:e462c8257384 | 442 | data1=0x00; |
yosino_adati | 2:e462c8257384 | 443 | data2=0x00; |
yosino_adati | 2:e462c8257384 | 444 | data3=0xff; |
yosino_adati | 2:e462c8257384 | 445 | data4=0xff; |
yosino_adati | 2:e462c8257384 | 446 | } |
yosino_adati | 2:e462c8257384 | 447 | } else if(right1==1&&i==0) { |
shina | 1:99294241f2ba | 448 | data1=0xff; |
shina | 1:99294241f2ba | 449 | data2=0x00; |
shina | 1:99294241f2ba | 450 | data3=0xff; |
shina | 1:99294241f2ba | 451 | data4=0x00; |
yosino_adati | 2:e462c8257384 | 452 | } else if(left1==1&&i==0) { |
shina | 1:99294241f2ba | 453 | data1=0x00; |
shina | 1:99294241f2ba | 454 | data2=0xff; |
shina | 1:99294241f2ba | 455 | data3=0x00; |
shina | 1:99294241f2ba | 456 | data4=0xff; |
yosino_adati | 2:e462c8257384 | 457 | } else if(Ry>30&&i==1) { |
yosino_adati | 2:e462c8257384 | 458 | if(right1==1) { |
yosino_adati | 2:e462c8257384 | 459 | data1=0x4f; |
yosino_adati | 2:e462c8257384 | 460 | data2=0x3f; |
yosino_adati | 2:e462c8257384 | 461 | data3=0x4f; |
yosino_adati | 2:e462c8257384 | 462 | data4=0x3f; |
yosino_adati | 2:e462c8257384 | 463 | } else if(left1==1) { |
yosino_adati | 2:e462c8257384 | 464 | data1=0x3f; |
yosino_adati | 2:e462c8257384 | 465 | data2=0x4f; |
yosino_adati | 2:e462c8257384 | 466 | data3=0x3f; |
yosino_adati | 2:e462c8257384 | 467 | data4=0x4f; |
yosino_adati | 2:e462c8257384 | 468 | } else { |
yosino_adati | 2:e462c8257384 | 469 | data1=0x3f; |
yosino_adati | 2:e462c8257384 | 470 | data2=0x3f; |
yosino_adati | 2:e462c8257384 | 471 | data3=0x3f; |
yosino_adati | 2:e462c8257384 | 472 | data4=0x3f; |
shina | 1:99294241f2ba | 473 | } |
yosino_adati | 2:e462c8257384 | 474 | } else if(Ry<-30&&i==1) { |
shina | 3:8c4e42cba9cb | 475 | if(right1==1){ |
shina | 3:8c4e42cba9cb | 476 | data1=0x5f; |
shina | 3:8c4e42cba9cb | 477 | data2=0xbf; |
shina | 3:8c4e42cba9cb | 478 | data3=0x5f; |
shina | 3:8c4e42cba9cb | 479 | data4=0xbf; |
shina | 3:8c4e42cba9cb | 480 | }else if(left1==1){ |
shina | 3:8c4e42cba9cb | 481 | data1=0xbf; |
shina | 3:8c4e42cba9cb | 482 | data2=0x5f; |
shina | 3:8c4e42cba9cb | 483 | data3=0xbf; |
shina | 3:8c4e42cba9cb | 484 | data4=0x5f; |
shina | 3:8c4e42cba9cb | 485 | }else{ |
shina | 3:8c4e42cba9cb | 486 | data1=0xbf; |
shina | 3:8c4e42cba9cb | 487 | data2=0xbf; |
shina | 3:8c4e42cba9cb | 488 | data3=0xbf; |
shina | 3:8c4e42cba9cb | 489 | data4=0xbf; |
shina | 3:8c4e42cba9cb | 490 | } |
yosino_adati | 2:e462c8257384 | 491 | } else if(Rx>30&&i==1) { |
yosino_adati | 2:e462c8257384 | 492 | if(right1==1) { |
yosino_adati | 2:e462c8257384 | 493 | data1=0xbf; |
yosino_adati | 2:e462c8257384 | 494 | data2=0x5f; |
yosino_adati | 2:e462c8257384 | 495 | data3=0x4f; |
yosino_adati | 2:e462c8257384 | 496 | data4=0x3f; |
yosino_adati | 2:e462c8257384 | 497 | } else if(left1==1) { |
yosino_adati | 2:e462c8257384 | 498 | data1=0x5f; |
yosino_adati | 2:e462c8257384 | 499 | data2=0xbf; |
yosino_adati | 2:e462c8257384 | 500 | data3=0x3f; |
yosino_adati | 2:e462c8257384 | 501 | data4=0x4f; |
yosino_adati | 2:e462c8257384 | 502 | } else { |
yosino_adati | 2:e462c8257384 | 503 | data1=0xbf; |
yosino_adati | 2:e462c8257384 | 504 | data2=0xbf; |
yosino_adati | 2:e462c8257384 | 505 | data3=0x3f; |
yosino_adati | 2:e462c8257384 | 506 | data4=0x3f; |
shina | 1:99294241f2ba | 507 | } |
yosino_adati | 2:e462c8257384 | 508 | } else if(Rx<-30&&i==1) { |
yosino_adati | 2:e462c8257384 | 509 | if(right1==1) { |
yosino_adati | 2:e462c8257384 | 510 | data1=0x4f; |
yosino_adati | 2:e462c8257384 | 511 | data2=0x3f; |
yosino_adati | 2:e462c8257384 | 512 | data3=0xbf; |
yosino_adati | 2:e462c8257384 | 513 | data4=0x5f; |
yosino_adati | 2:e462c8257384 | 514 | } else if(left1==1) { |
yosino_adati | 2:e462c8257384 | 515 | data1=0x3f; |
yosino_adati | 2:e462c8257384 | 516 | data2=0x4f; |
yosino_adati | 2:e462c8257384 | 517 | data3=0x5f; |
yosino_adati | 2:e462c8257384 | 518 | data4=0xbf; |
yosino_adati | 2:e462c8257384 | 519 | } else { |
yosino_adati | 2:e462c8257384 | 520 | data1=0x3f; |
yosino_adati | 2:e462c8257384 | 521 | data2=0x3f; |
yosino_adati | 2:e462c8257384 | 522 | data3=0xbf; |
yosino_adati | 2:e462c8257384 | 523 | data4=0xbf; |
shina | 1:99294241f2ba | 524 | } |
yosino_adati | 2:e462c8257384 | 525 | } else if(right1==1&&i==1) { |
shina | 1:99294241f2ba | 526 | data1=0xbf; |
shina | 0:c7e17c2fd542 | 527 | data2=0x3f; |
shina | 1:99294241f2ba | 528 | data3=0xbf; |
shina | 0:c7e17c2fd542 | 529 | data4=0x3f; |
yosino_adati | 2:e462c8257384 | 530 | } else if(left1==1&&i==1) { |
shina | 1:99294241f2ba | 531 | data1=0x3f; |
shina | 0:c7e17c2fd542 | 532 | data2=0xbf; |
shina | 1:99294241f2ba | 533 | data3=0x3f; |
shina | 0:c7e17c2fd542 | 534 | data4=0xbf; |
yosino_adati | 2:e462c8257384 | 535 | } else { |
shina | 0:c7e17c2fd542 | 536 | data1=0x80; |
shina | 0:c7e17c2fd542 | 537 | data2=0x80; |
shina | 0:c7e17c2fd542 | 538 | data3=0x80; |
shina | 0:c7e17c2fd542 | 539 | data4=0x80; |
shina | 0:c7e17c2fd542 | 540 | } |
yosino_adati | 2:e462c8257384 | 541 | } |
shina | 0:c7e17c2fd542 | 542 | |
shina | 1:99294241f2ba | 543 | |
yosino_adati | 2:e462c8257384 | 544 | //pwm変化 |
yosino_adati | 2:e462c8257384 | 545 | void change_pwm() |
yosino_adati | 2:e462c8257384 | 546 | { |
shina | 3:8c4e42cba9cb | 547 | if(cross!=old_cross) { |
shina | 3:8c4e42cba9cb | 548 | old_cross=cross; |
shina | 3:8c4e42cba9cb | 549 | if(cross==1) { |
yosino_adati | 2:e462c8257384 | 550 | if(i==1) { |
yosino_adati | 2:e462c8257384 | 551 | pwm=0; |
shina | 3:8c4e42cba9cb | 552 | data10=0xbf; |
shina | 3:8c4e42cba9cb | 553 | send_data(0x40,data10); |
yosino_adati | 2:e462c8257384 | 554 | i=0; |
yosino_adati | 2:e462c8257384 | 555 | } else if(i==0) { |
yosino_adati | 2:e462c8257384 | 556 | pwm=1; |
shina | 3:8c4e42cba9cb | 557 | data10=0x3f; |
shina | 3:8c4e42cba9cb | 558 | send_data(0x40,data10); |
yosino_adati | 2:e462c8257384 | 559 | i=1; |
shina | 0:c7e17c2fd542 | 560 | } |
shina | 0:c7e17c2fd542 | 561 | } |
shina | 0:c7e17c2fd542 | 562 | } |
shina | 0:c7e17c2fd542 | 563 | } |
shina | 0:c7e17c2fd542 | 564 | |
shina | 0:c7e17c2fd542 | 565 | //サーボモーター |
yosino_adati | 2:e462c8257384 | 566 | void change_servo() |
yosino_adati | 2:e462c8257384 | 567 | { |
yosino_adati | 2:e462c8257384 | 568 | if(right2!=old_right2) { |
yosino_adati | 2:e462c8257384 | 569 | old_right2=right2; |
shina | 4:4d9c0999a90a | 570 | //ハンガー機構&バスタオル機構initialize処理 |
yosino_adati | 2:e462c8257384 | 571 | if(right2==1) { |
yosino_adati | 2:e462c8257384 | 572 | if(o==1) { |
yosino_adati | 2:e462c8257384 | 573 | data_servo=0x01; |
shina | 4:4d9c0999a90a | 574 | send_data(0x18,data_servo); |
yosino_adati | 2:e462c8257384 | 575 | o=2; |
shina | 5:29971215ef50 | 576 | kaisyu_arm=1; |
yosino_adati | 2:e462c8257384 | 577 | } else if(o==2) { |
yosino_adati | 2:e462c8257384 | 578 | data_servo=0x02; |
shina | 4:4d9c0999a90a | 579 | send_data(0x18,data_servo); |
yosino_adati | 2:e462c8257384 | 580 | o=3; |
yosino_adati | 2:e462c8257384 | 581 | } |
yosino_adati | 2:e462c8257384 | 582 | } |
shina | 4:4d9c0999a90a | 583 | } |
shina | 4:4d9c0999a90a | 584 | |
shina | 4:4d9c0999a90a | 585 | if(circle!=old_circle) { |
yosino_adati | 2:e462c8257384 | 586 | old_circle=circle; |
yosino_adati | 2:e462c8257384 | 587 | //バスタオル挟む機構 |
yosino_adati | 2:e462c8257384 | 588 | if(circle==1) { |
yosino_adati | 2:e462c8257384 | 589 | if(k==1) { |
shina | 4:4d9c0999a90a | 590 | data_servo=0xf3; |
shina | 4:4d9c0999a90a | 591 | send_data(0x18,data_servo); |
yosino_adati | 2:e462c8257384 | 592 | k=0; |
yosino_adati | 2:e462c8257384 | 593 | } else if(k==0) { |
shina | 4:4d9c0999a90a | 594 | data_servo=0xf4; |
shina | 4:4d9c0999a90a | 595 | send_data(0x18,data_servo); |
yosino_adati | 2:e462c8257384 | 596 | k=1; |
yosino_adati | 2:e462c8257384 | 597 | } |
yosino_adati | 2:e462c8257384 | 598 | } |
shina | 4:4d9c0999a90a | 599 | } |
shina | 5:29971215ef50 | 600 | |
shina | 5:29971215ef50 | 601 | if(q==0){ |
shina | 5:29971215ef50 | 602 | if(triangle!=old_triangle){ |
shina | 4:4d9c0999a90a | 603 | old_triangle=triangle; |
shina | 4:4d9c0999a90a | 604 | if(triangle==1){ |
shina | 4:4d9c0999a90a | 605 | if(y==0){ |
shina | 4:4d9c0999a90a | 606 | data_servo=0x08; |
shina | 4:4d9c0999a90a | 607 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 608 | y=1; |
shina | 4:4d9c0999a90a | 609 | }else if(y==1){ |
shina | 4:4d9c0999a90a | 610 | data_servo=0x07; |
shina | 4:4d9c0999a90a | 611 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 612 | y=0; |
shina | 4:4d9c0999a90a | 613 | } |
shina | 4:4d9c0999a90a | 614 | } |
shina | 5:29971215ef50 | 615 | } |
shina | 5:29971215ef50 | 616 | } |
yosino_adati | 2:e462c8257384 | 617 | } |
yosino_adati | 2:e462c8257384 | 618 | |
yosino_adati | 2:e462c8257384 | 619 | |
shina | 1:99294241f2ba | 620 | |
shina | 1:99294241f2ba | 621 | //ラック左右 |
yosino_adati | 2:e462c8257384 | 622 | void change_rack_2() |
yosino_adati | 2:e462c8257384 | 623 | { |
shina | 3:8c4e42cba9cb | 624 | /* |
yosino_adati | 2:e462c8257384 | 625 | if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) { |
yosino_adati | 2:e462c8257384 | 626 | if(get_data_rs232 & 0x01) { |
shina | 1:99294241f2ba | 627 | data5=0x10; |
yosino_adati | 2:e462c8257384 | 628 | } else { |
shina | 1:99294241f2ba | 629 | data5=0x00; |
yosino_adati | 2:e462c8257384 | 630 | } |
yosino_adati | 2:e462c8257384 | 631 | if(get_data_rs232 & 0x02) { |
shina | 1:99294241f2ba | 632 | data6=0x10; |
yosino_adati | 2:e462c8257384 | 633 | } else { |
shina | 1:99294241f2ba | 634 | data6=0xff; |
shina | 1:99294241f2ba | 635 | } |
yosino_adati | 2:e462c8257384 | 636 | } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) { |
yosino_adati | 2:e462c8257384 | 637 | if(get_data_rs232 & 0x04) { |
shina | 1:99294241f2ba | 638 | data5=0x10; |
yosino_adati | 2:e462c8257384 | 639 | } else { |
shina | 1:99294241f2ba | 640 | data5=0xff; |
yosino_adati | 2:e462c8257384 | 641 | } |
yosino_adati | 2:e462c8257384 | 642 | if(get_data_rs232 & 0x08) { |
shina | 1:99294241f2ba | 643 | data6=0x10; |
yosino_adati | 2:e462c8257384 | 644 | } else { |
shina | 1:99294241f2ba | 645 | data6=0x00; |
shina | 1:99294241f2ba | 646 | } |
yosino_adati | 2:e462c8257384 | 647 | } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) { |
shina | 1:99294241f2ba | 648 | data5=0x80; |
shina | 1:99294241f2ba | 649 | data6=0x80; |
shina | 0:c7e17c2fd542 | 650 | } |
shina | 3:8c4e42cba9cb | 651 | */ |
shina | 3:8c4e42cba9cb | 652 | |
shina | 3:8c4e42cba9cb | 653 | if(ue1!=old_ue1){ |
shina | 3:8c4e42cba9cb | 654 | old_ue1=ue1; |
shina | 4:4d9c0999a90a | 655 | if(ue1==1&&start==0){ |
shina | 3:8c4e42cba9cb | 656 | data5=0x00; |
shina | 3:8c4e42cba9cb | 657 | data6=0xff; |
shina | 4:4d9c0999a90a | 658 | |
shina | 4:4d9c0999a90a | 659 | /*data_servo=0x03; |
shina | 4:4d9c0999a90a | 660 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 661 | data_servo=0x04; |
shina | 4:4d9c0999a90a | 662 | send_data(0x18,data_servo);*/ |
shina | 3:8c4e42cba9cb | 663 | } |
shina | 3:8c4e42cba9cb | 664 | }else if(shita!=old_shita){ |
shina | 3:8c4e42cba9cb | 665 | old_shita=shita; |
shina | 4:4d9c0999a90a | 666 | if(shita==1&&start==0){ |
shina | 3:8c4e42cba9cb | 667 | data5=0xff; |
shina | 3:8c4e42cba9cb | 668 | data6=0x00; |
shina | 3:8c4e42cba9cb | 669 | } |
shina | 3:8c4e42cba9cb | 670 | } |
shina | 3:8c4e42cba9cb | 671 | |
shina | 3:8c4e42cba9cb | 672 | if((get_data_rs232 & 0x01)&&data5==0x00){ |
shina | 3:8c4e42cba9cb | 673 | data5=0x10; |
shina | 3:8c4e42cba9cb | 674 | send_data(0x20,data5); |
shina | 3:8c4e42cba9cb | 675 | }else if((get_data_rs232 & 0x04)&&data5==0xff){ |
shina | 3:8c4e42cba9cb | 676 | data5=0x10; |
shina | 3:8c4e42cba9cb | 677 | send_data(0x20,data5); |
shina | 3:8c4e42cba9cb | 678 | } |
shina | 3:8c4e42cba9cb | 679 | |
shina | 3:8c4e42cba9cb | 680 | if((get_data_rs232 & 0x02)&&data6==0xff){ |
shina | 3:8c4e42cba9cb | 681 | data6=0x10; |
shina | 3:8c4e42cba9cb | 682 | send_data(0x22,data6); |
shina | 3:8c4e42cba9cb | 683 | }else if((get_data_rs232 & 0x08)&&data6==0x00){ |
shina | 3:8c4e42cba9cb | 684 | data6=0x10; |
shina | 3:8c4e42cba9cb | 685 | send_data(0x22,data6); |
shina | 3:8c4e42cba9cb | 686 | } |
shina | 3:8c4e42cba9cb | 687 | |
shina | 3:8c4e42cba9cb | 688 | |
shina | 1:99294241f2ba | 689 | } |
shina | 0:c7e17c2fd542 | 690 | |
shina | 0:c7e17c2fd542 | 691 | //回収機構 |
shina | 4:4d9c0999a90a | 692 | /* |
yosino_adati | 2:e462c8257384 | 693 | void sequence_kaisyu() |
yosino_adati | 2:e462c8257384 | 694 | { |
shina | 3:8c4e42cba9cb | 695 | if(q==0){ |
yosino_adati | 2:e462c8257384 | 696 | if(square!=old_square) { |
yosino_adati | 2:e462c8257384 | 697 | old_square=square; |
shina | 4:4d9c0999a90a | 698 | if(square==1&&select==0) { |
shina | 4:4d9c0999a90a | 699 | if(x==0){ |
shina | 4:4d9c0999a90a | 700 | data_servo=0x07; |
shina | 4:4d9c0999a90a | 701 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 702 | wait(1.2); |
shina | 4:4d9c0999a90a | 703 | data7=0x00; |
shina | 4:4d9c0999a90a | 704 | get_data_rs232=get_data_rs232 & 0b11101111; |
shina | 4:4d9c0999a90a | 705 | while(!(get_data_rs232 & 0x10)){ |
shina | 4:4d9c0999a90a | 706 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 707 | get_data_rs232=slave.getc(); |
shina | 4:4d9c0999a90a | 708 | } |
shina | 4:4d9c0999a90a | 709 | data7=0x10; |
shina | 4:4d9c0999a90a | 710 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 711 | data_servo=0x08; |
shina | 4:4d9c0999a90a | 712 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 713 | wait(1.2); |
shina | 4:4d9c0999a90a | 714 | data7=0xff; |
shina | 4:4d9c0999a90a | 715 | get_data_rs232=get_data_rs232 & 0b11011111; |
shina | 4:4d9c0999a90a | 716 | while(!(get_data_rs232 & 0x20)){ |
shina | 4:4d9c0999a90a | 717 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 718 | get_data_rs232=slave.getc(); |
shina | 4:4d9c0999a90a | 719 | } |
shina | 4:4d9c0999a90a | 720 | data7=0x10; |
shina | 4:4d9c0999a90a | 721 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 722 | x=0; |
shina | 4:4d9c0999a90a | 723 | } |
shina | 4:4d9c0999a90a | 724 | } |
shina | 4:4d9c0999a90a | 725 | } |
shina | 4:4d9c0999a90a | 726 | } |
shina | 4:4d9c0999a90a | 727 | } |
shina | 4:4d9c0999a90a | 728 | */ |
shina | 1:99294241f2ba | 729 | |
shina | 4:4d9c0999a90a | 730 | //回収機構switch文version |
shina | 4:4d9c0999a90a | 731 | void kaisyu(){ |
shina | 4:4d9c0999a90a | 732 | if(q==0){ |
shina | 5:29971215ef50 | 733 | if(kaisyu_arm!=0){ |
shina | 4:4d9c0999a90a | 734 | if(old_square!=square){ |
shina | 4:4d9c0999a90a | 735 | old_square=square; |
shina | 4:4d9c0999a90a | 736 | if(square==1){ |
shina | 4:4d9c0999a90a | 737 | if(x==0){ |
shina | 4:4d9c0999a90a | 738 | p=1; |
shina | 4:4d9c0999a90a | 739 | x=1; |
shina | 5:29971215ef50 | 740 | timer2.start(); |
shina | 4:4d9c0999a90a | 741 | }else if(x==1){ |
shina | 4:4d9c0999a90a | 742 | p=4; |
shina | 4:4d9c0999a90a | 743 | x=0; |
shina | 4:4d9c0999a90a | 744 | } |
shina | 4:4d9c0999a90a | 745 | } |
shina | 4:4d9c0999a90a | 746 | } |
shina | 4:4d9c0999a90a | 747 | |
shina | 4:4d9c0999a90a | 748 | switch(p){ |
shina | 4:4d9c0999a90a | 749 | |
shina | 4:4d9c0999a90a | 750 | case 1: |
shina | 5:29971215ef50 | 751 | y=0; |
shina | 4:4d9c0999a90a | 752 | data_servo=0x07; |
shina | 4:4d9c0999a90a | 753 | send_data(0x18,data_servo); |
shina | 5:29971215ef50 | 754 | //timer2.start(); |
shina | 5:29971215ef50 | 755 | if(timer2.read()>1.5f){ |
shina | 4:4d9c0999a90a | 756 | timer2.stop(); |
shina | 4:4d9c0999a90a | 757 | timer2.reset(); |
shina | 4:4d9c0999a90a | 758 | p=2; |
shina | 4:4d9c0999a90a | 759 | } |
shina | 4:4d9c0999a90a | 760 | break; |
shina | 4:4d9c0999a90a | 761 | |
shina | 4:4d9c0999a90a | 762 | case 2: |
shina | 4:4d9c0999a90a | 763 | data7=0x00; |
shina | 4:4d9c0999a90a | 764 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 765 | if(get_data_rs232 & 0x10){ |
shina | 4:4d9c0999a90a | 766 | data7=0x10; |
shina | 4:4d9c0999a90a | 767 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 768 | p=3; |
shina | 4:4d9c0999a90a | 769 | } |
shina | 4:4d9c0999a90a | 770 | break; |
shina | 4:4d9c0999a90a | 771 | |
shina | 4:4d9c0999a90a | 772 | case 3: |
shina | 5:29971215ef50 | 773 | /* |
shina | 4:4d9c0999a90a | 774 | if(triangle!=old_triangle){ |
shina | 4:4d9c0999a90a | 775 | old_triangle=triangle; |
shina | 4:4d9c0999a90a | 776 | if(triangle==1){ |
shina | 4:4d9c0999a90a | 777 | if(y==0){ |
shina | 4:4d9c0999a90a | 778 | data_servo=0x08; |
shina | 4:4d9c0999a90a | 779 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 780 | y=1; |
shina | 4:4d9c0999a90a | 781 | }else if(y==1){ |
shina | 4:4d9c0999a90a | 782 | data_servo=0x07; |
shina | 4:4d9c0999a90a | 783 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 784 | y=0; |
shina | 4:4d9c0999a90a | 785 | } |
shina | 4:4d9c0999a90a | 786 | } |
shina | 5:29971215ef50 | 787 | }*/ |
shina | 4:4d9c0999a90a | 788 | |
shina | 4:4d9c0999a90a | 789 | break; |
shina | 4:4d9c0999a90a | 790 | |
shina | 4:4d9c0999a90a | 791 | case 4: |
shina | 4:4d9c0999a90a | 792 | data7=0xff; |
shina | 4:4d9c0999a90a | 793 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 794 | if(get_data_rs232 & 0x20){ |
shina | 4:4d9c0999a90a | 795 | data7=0x10; |
shina | 4:4d9c0999a90a | 796 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 797 | p=5; |
shina | 4:4d9c0999a90a | 798 | } |
shina | 4:4d9c0999a90a | 799 | break; |
shina | 4:4d9c0999a90a | 800 | |
shina | 4:4d9c0999a90a | 801 | default: |
shina | 4:4d9c0999a90a | 802 | p=5; |
shina | 4:4d9c0999a90a | 803 | break; |
shina | 4:4d9c0999a90a | 804 | |
shina | 4:4d9c0999a90a | 805 | } |
shina | 5:29971215ef50 | 806 | } |
shina | 4:4d9c0999a90a | 807 | } |
shina | 4:4d9c0999a90a | 808 | } |
shina | 4:4d9c0999a90a | 809 | |
shina | 4:4d9c0999a90a | 810 | /* |
shina | 4:4d9c0999a90a | 811 | void sequence_kaisyu_2(){ |
shina | 4:4d9c0999a90a | 812 | if(q==0){ |
shina | 4:4d9c0999a90a | 813 | if(old_migi1!=migi1){ |
shina | 4:4d9c0999a90a | 814 | if(migi1==1){ |
shina | 4:4d9c0999a90a | 815 | p=1; |
shina | 4:4d9c0999a90a | 816 | } |
shina | 4:4d9c0999a90a | 817 | } |
shina | 4:4d9c0999a90a | 818 | |
shina | 4:4d9c0999a90a | 819 | switch(p){ |
shina | 4:4d9c0999a90a | 820 | case 1: |
yosino_adati | 2:e462c8257384 | 821 | data_servo=0x07; |
yosino_adati | 2:e462c8257384 | 822 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 823 | timer.start(); |
shina | 4:4d9c0999a90a | 824 | if(timer.read>1.2f){ |
shina | 4:4d9c0999a90a | 825 | p=2; |
shina | 4:4d9c0999a90a | 826 | timer.stop(); |
shina | 4:4d9c0999a90a | 827 | timer.reset(); |
shina | 4:4d9c0999a90a | 828 | } |
shina | 4:4d9c0999a90a | 829 | break; |
shina | 4:4d9c0999a90a | 830 | |
shina | 4:4d9c0999a90a | 831 | case 2: |
yosino_adati | 2:e462c8257384 | 832 | data7=0x00; |
shina | 4:4d9c0999a90a | 833 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 834 | if(get_data_rs232 & 0x10){ |
shina | 4:4d9c0999a90a | 835 | timer.stop(); |
shina | 4:4d9c0999a90a | 836 | timer.reset(); |
shina | 4:4d9c0999a90a | 837 | data7=0x10; |
yosino_adati | 2:e462c8257384 | 838 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 839 | p=3; |
yosino_adati | 2:e462c8257384 | 840 | } |
shina | 4:4d9c0999a90a | 841 | break(); |
shina | 4:4d9c0999a90a | 842 | |
shina | 4:4d9c0999a90a | 843 | case 3: |
yosino_adati | 2:e462c8257384 | 844 | data_servo=0x08; |
yosino_adati | 2:e462c8257384 | 845 | send_data(0x18,data_servo); |
shina | 4:4d9c0999a90a | 846 | timer.start(); |
shina | 4:4d9c0999a90a | 847 | if(timer.read()>1.5f()){ |
shina | 4:4d9c0999a90a | 848 | timer.stop(); |
shina | 4:4d9c0999a90a | 849 | timer.reset(); |
shina | 4:4d9c0999a90a | 850 | p=4; |
shina | 4:4d9c0999a90a | 851 | } |
shina | 4:4d9c0999a90a | 852 | break(); |
shina | 4:4d9c0999a90a | 853 | |
shina | 4:4d9c0999a90a | 854 | case 4: |
yosino_adati | 2:e462c8257384 | 855 | data7=0xff; |
shina | 4:4d9c0999a90a | 856 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 857 | if(get_data_rs232 & 0x20){ |
shina | 4:4d9c0999a90a | 858 | data7=0x10; |
yosino_adati | 2:e462c8257384 | 859 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 860 | p=5; |
shina | 4:4d9c0999a90a | 861 | } |
shina | 4:4d9c0999a90a | 862 | break; |
shina | 4:4d9c0999a90a | 863 | |
shina | 4:4d9c0999a90a | 864 | default: |
shina | 4:4d9c0999a90a | 865 | break(); |
shina | 4:4d9c0999a90a | 866 | |
shina | 4:4d9c0999a90a | 867 | |
shina | 4:4d9c0999a90a | 868 | } |
shina | 4:4d9c0999a90a | 869 | |
shina | 4:4d9c0999a90a | 870 | } |
shina | 4:4d9c0999a90a | 871 | } |
shina | 4:4d9c0999a90a | 872 | */ |
shina | 4:4d9c0999a90a | 873 | |
shina | 4:4d9c0999a90a | 874 | |
shina | 4:4d9c0999a90a | 875 | |
shina | 4:4d9c0999a90a | 876 | void hakidashi(){ |
shina | 4:4d9c0999a90a | 877 | if(q==0){ |
shina | 4:4d9c0999a90a | 878 | if(old_migi1!=migi1){ |
shina | 4:4d9c0999a90a | 879 | old_migi1=migi1; |
shina | 4:4d9c0999a90a | 880 | if(migi1==1&&select==0){ |
shina | 4:4d9c0999a90a | 881 | r=1; |
shina | 5:29971215ef50 | 882 | timer2.start(); |
shina | 4:4d9c0999a90a | 883 | } |
shina | 4:4d9c0999a90a | 884 | } |
shina | 4:4d9c0999a90a | 885 | |
shina | 4:4d9c0999a90a | 886 | switch(r){ |
shina | 4:4d9c0999a90a | 887 | case 1: |
shina | 4:4d9c0999a90a | 888 | data_servo=0x07; |
shina | 4:4d9c0999a90a | 889 | send_data(0x18,data_servo); |
shina | 5:29971215ef50 | 890 | //timer2.start(); |
shina | 4:4d9c0999a90a | 891 | if(timer2.read()>1.5f){ |
shina | 4:4d9c0999a90a | 892 | timer2.stop(); |
shina | 4:4d9c0999a90a | 893 | timer2.reset(); |
shina | 5:29971215ef50 | 894 | timer2.start(); |
shina | 4:4d9c0999a90a | 895 | r=2; |
yosino_adati | 2:e462c8257384 | 896 | } |
shina | 4:4d9c0999a90a | 897 | break; |
shina | 4:4d9c0999a90a | 898 | |
shina | 4:4d9c0999a90a | 899 | case 2: |
shina | 4:4d9c0999a90a | 900 | data7=0x00; |
yosino_adati | 2:e462c8257384 | 901 | send_data(0x24,data7); |
shina | 5:29971215ef50 | 902 | //timer2.start(); |
shina | 4:4d9c0999a90a | 903 | if(timer2.read()>0.3f){ |
shina | 4:4d9c0999a90a | 904 | timer2.stop(); |
shina | 4:4d9c0999a90a | 905 | timer2.reset(); |
shina | 4:4d9c0999a90a | 906 | data7=0x10; |
shina | 4:4d9c0999a90a | 907 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 908 | r=3; |
shina | 4:4d9c0999a90a | 909 | } |
shina | 4:4d9c0999a90a | 910 | break; |
shina | 4:4d9c0999a90a | 911 | |
shina | 4:4d9c0999a90a | 912 | case 3: |
shina | 4:4d9c0999a90a | 913 | data11=0xff; |
shina | 4:4d9c0999a90a | 914 | send_data(0x30,data11); |
shina | 4:4d9c0999a90a | 915 | if(get_data_rs232 & 0x40){ |
shina | 4:4d9c0999a90a | 916 | data11=0x10; |
shina | 4:4d9c0999a90a | 917 | send_data(0x30,data11); |
shina | 4:4d9c0999a90a | 918 | r=4; |
shina | 3:8c4e42cba9cb | 919 | } |
shina | 4:4d9c0999a90a | 920 | break; |
shina | 4:4d9c0999a90a | 921 | |
shina | 4:4d9c0999a90a | 922 | case 4: |
shina | 4:4d9c0999a90a | 923 | data11=0x00; |
shina | 4:4d9c0999a90a | 924 | send_data(0x30,data11); |
shina | 4:4d9c0999a90a | 925 | if(get_data_rs232 & 0x80){ |
shina | 4:4d9c0999a90a | 926 | data11=0x10; |
shina | 4:4d9c0999a90a | 927 | send_data(0x30,data11); |
shina | 4:4d9c0999a90a | 928 | r=5; |
shina | 4:4d9c0999a90a | 929 | } |
shina | 4:4d9c0999a90a | 930 | break; |
shina | 4:4d9c0999a90a | 931 | |
shina | 4:4d9c0999a90a | 932 | case 5: |
shina | 4:4d9c0999a90a | 933 | data7=0xff; |
shina | 4:4d9c0999a90a | 934 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 935 | if(get_data_rs232 & 0x20){ |
shina | 4:4d9c0999a90a | 936 | data7=0x10; |
shina | 4:4d9c0999a90a | 937 | send_data(0x24,data7); |
shina | 4:4d9c0999a90a | 938 | r=6; |
shina | 4:4d9c0999a90a | 939 | } |
shina | 4:4d9c0999a90a | 940 | break; |
shina | 4:4d9c0999a90a | 941 | |
shina | 4:4d9c0999a90a | 942 | default: |
shina | 4:4d9c0999a90a | 943 | r=6; |
shina | 4:4d9c0999a90a | 944 | break; |
yosino_adati | 2:e462c8257384 | 945 | } |
yosino_adati | 2:e462c8257384 | 946 | } |
shina | 1:99294241f2ba | 947 | } |
shina | 1:99294241f2ba | 948 | |
shina | 1:99294241f2ba | 949 | |
shina | 1:99294241f2ba | 950 | |
shina | 0:c7e17c2fd542 | 951 | //ファン |
yosino_adati | 2:e462c8257384 | 952 | void change_fan() |
yosino_adati | 2:e462c8257384 | 953 | { |
shina | 3:8c4e42cba9cb | 954 | if(q==1){ |
shina | 3:8c4e42cba9cb | 955 | if(left2==1&&(old_triangle==triangle)) { |
shina | 0:c7e17c2fd542 | 956 | data8=0xff; |
shina | 0:c7e17c2fd542 | 957 | data9=0xff; |
shina | 1:99294241f2ba | 958 | data10=0xff; |
shina | 3:8c4e42cba9cb | 959 | } else if(left2==0&&(old_triangle==triangle)) { |
yosino_adati | 2:e462c8257384 | 960 | data8=0x00; |
yosino_adati | 2:e462c8257384 | 961 | data9=0x00; |
yosino_adati | 2:e462c8257384 | 962 | data10=0x00; |
shina | 4:4d9c0999a90a | 963 | }else if(left2==0&&(old_triangle!=triangle)&&select==0&&(get_data_rs232 & 0b00000011)){ |
shina | 3:8c4e42cba9cb | 964 | old_triangle=triangle; |
shina | 3:8c4e42cba9cb | 965 | if(triangle==1){ |
shina | 3:8c4e42cba9cb | 966 | data8=0xff; |
shina | 3:8c4e42cba9cb | 967 | data9=0xff; |
shina | 3:8c4e42cba9cb | 968 | data10=0xff; |
shina | 4:4d9c0999a90a | 969 | data_servo=0xf3; |
shina | 3:8c4e42cba9cb | 970 | send_data(0x26,data8); |
shina | 3:8c4e42cba9cb | 971 | send_data(0x28,data9); |
shina | 3:8c4e42cba9cb | 972 | send_data(0x40,data10); |
shina | 4:4d9c0999a90a | 973 | wait(0.5); |
shina | 3:8c4e42cba9cb | 974 | send_data(0x18,data_servo); |
shina | 3:8c4e42cba9cb | 975 | wait(1); |
shina | 3:8c4e42cba9cb | 976 | data8=0x00; |
shina | 3:8c4e42cba9cb | 977 | data9=0x00; |
shina | 3:8c4e42cba9cb | 978 | data10=0x00; |
shina | 3:8c4e42cba9cb | 979 | send_data(0x26,data8); |
shina | 3:8c4e42cba9cb | 980 | send_data(0x28,data9); |
shina | 3:8c4e42cba9cb | 981 | send_data(0x40,data10); |
shina | 3:8c4e42cba9cb | 982 | wait(0.5); |
shina | 4:4d9c0999a90a | 983 | data_servo=0xf4; |
shina | 3:8c4e42cba9cb | 984 | send_data(0x18,data_servo); |
shina | 3:8c4e42cba9cb | 985 | } |
shina | 0:c7e17c2fd542 | 986 | } |
yosino_adati | 2:e462c8257384 | 987 | } |
shina | 3:8c4e42cba9cb | 988 | |
shina | 3:8c4e42cba9cb | 989 | } |
shina | 1:99294241f2ba | 990 | |
shina | 0:c7e17c2fd542 | 991 | //i2c |
yosino_adati | 2:e462c8257384 | 992 | void send_data(char address,char data) |
yosino_adati | 2:e462c8257384 | 993 | { |
shina | 0:c7e17c2fd542 | 994 | tsushin=1; |
shina | 0:c7e17c2fd542 | 995 | tushin_check=0; |
shina | 0:c7e17c2fd542 | 996 | i2c.start(); |
shina | 0:c7e17c2fd542 | 997 | i2c.write(address); |
shina | 0:c7e17c2fd542 | 998 | tushin_check=i2c.write(data); |
shina | 0:c7e17c2fd542 | 999 | i2c.stop(); |
yosino_adati | 2:e462c8257384 | 1000 | } |
yosino_adati | 2:e462c8257384 | 1001 | |
shina | 4:4d9c0999a90a | 1002 |