2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_11_19

NHK2019 manual program(回収機構ストール問題解決)

Dependencies:   mbed PS3_10_26

main.cpp@8:959fe4717597, 2019-11-18 (annotated)

Committer:
shina
Date:
Mon Nov 18 06:18:13 2019 +0000
Revision:
8:959fe4717597
Parent:
7:c423a6579c3c
Child:
9:01146bd5850c
NHK2019manual program 11/18version

Who changed what in which revision?

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