2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_11_19

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

Dependencies:   mbed PS3_10_26

main.cpp@9:01146bd5850c, 2019-11-19 (annotated)

Committer:
shina
Date:
Tue Nov 19 06:16:20 2019 +0000
Revision:
9:01146bd5850c
Parent:
8:959fe4717597 
NHK2019 manual program 11/19version

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 9:01146bd5850c 86 int p=6;
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 9:01146bd5850c 744 if(old_square!=square){
shina 9:01146bd5850c 745 old_square=square;
shina 9:01146bd5850c 746 if(square==1){
shina 9:01146bd5850c 747 if(x==0&&p==6){
shina 9:01146bd5850c 748 /*
shina 9:01146bd5850c 749 data_servo=0x02;
shina 9:01146bd5850c 750 send_data(0x18,data_servo);
shina 9:01146bd5850c 751 */
shina 9:01146bd5850c 752 p=1;
shina 9:01146bd5850c 753 x=1;
shina 9:01146bd5850c 754 timer2.start();
shina 9:01146bd5850c 755 }else if(x==1&&p==3){
shina 9:01146bd5850c 756 p=4;
shina 9:01146bd5850c 757 x=0;
shina 9:01146bd5850c 758 }
shina 4:4d9c0999a90a 759 }
shina 4:4d9c0999a90a 760 }
shina 4:4d9c0999a90a 761
shina 9:01146bd5850c 762 switch(p){
shina 9:01146bd5850c 763
shina 9:01146bd5850c 764 case 1:
shina 9:01146bd5850c 765 y=0;
shina 9:01146bd5850c 766 data_servo=0x07;
shina 9:01146bd5850c 767 send_data(0x18,data_servo);
shina 9:01146bd5850c 768 //timer2.start();
shina 9:01146bd5850c 769 if(timer2.read()>1.5f){
shina 9:01146bd5850c 770 timer2.stop();
shina 9:01146bd5850c 771 timer2.reset();
shina 9:01146bd5850c 772 p=2;
shina 9:01146bd5850c 773 }
shina 9:01146bd5850c 774 break;
shina 9:01146bd5850c 775
shina 9:01146bd5850c 776 case 2:
shina 9:01146bd5850c 777 data7=0x00;
shina 4:4d9c0999a90a 778 send_data(0x24,data7);
shina 9:01146bd5850c 779 if(get_data_rs232 & 0x10){
shina 9:01146bd5850c 780 data7=0x10;
shina 9:01146bd5850c 781 send_data(0x24,data7);
shina 9:01146bd5850c 782 p=3;
shina 9:01146bd5850c 783 }
shina 9:01146bd5850c 784 break;
shina 9:01146bd5850c 785
shina 9:01146bd5850c 786 case 3:
shina 9:01146bd5850c 787 /*
shina 9:01146bd5850c 788 if(triangle!=old_triangle){
shina 9:01146bd5850c 789 old_triangle=triangle;
shina 9:01146bd5850c 790 if(triangle==1){
shina 9:01146bd5850c 791 if(y==0){
shina 9:01146bd5850c 792 data_servo=0x08;
shina 9:01146bd5850c 793 send_data(0x18,data_servo);
shina 9:01146bd5850c 794 y=1;
shina 9:01146bd5850c 795 }else if(y==1){
shina 9:01146bd5850c 796 data_servo=0x07;
shina 9:01146bd5850c 797 send_data(0x18,data_servo);
shina 9:01146bd5850c 798 y=0;
shina 9:01146bd5850c 799 }
shina 4:4d9c0999a90a 800 }
shina 9:01146bd5850c 801 }*/
shina 9:01146bd5850c 802
shina 9:01146bd5850c 803 break;
shina 9:01146bd5850c 804
shina 9:01146bd5850c 805 case 4:
shina 9:01146bd5850c 806 data7=0xff;
shina 4:4d9c0999a90a 807 send_data(0x24,data7);
shina 4:4d9c0999a90a 808 p=5;
shina 9:01146bd5850c 809 /*
shina 9:01146bd5850c 810 if(get_data_rs232 & 0x20){
shina 9:01146bd5850c 811 data7=0x10;
shina 9:01146bd5850c 812 send_data(0x24,data7);
shina 9:01146bd5850c 813 p=5;
shina 9:01146bd5850c 814 }*/
shina 9:01146bd5850c 815 break;
shina 9:01146bd5850c 816
shina 9:01146bd5850c 817 case 5:
shina 9:01146bd5850c 818 if(get_data_rs232 & 0x20){
shina 9:01146bd5850c 819 data7=0x10;
shina 9:01146bd5850c 820 send_data(0x24,data7);
shina 9:01146bd5850c 821 p=6;
shina 9:01146bd5850c 822 }
shina 9:01146bd5850c 823 break;
shina 9:01146bd5850c 824
shina 9:01146bd5850c 825 default:
shina 9:01146bd5850c 826 p=6;
shina 9:01146bd5850c 827 break;
shina 9:01146bd5850c 828
shina 9:01146bd5850c 829
shina 4:4d9c0999a90a 830 }
shina 5:29971215ef50 831 }
shina 4:4d9c0999a90a 832 }
shina 4:4d9c0999a90a 833 }
shina 4:4d9c0999a90a 834
shina 4:4d9c0999a90a 835 /*
shina 4:4d9c0999a90a 836 void sequence_kaisyu_2(){
shina 4:4d9c0999a90a 837 if(q==0){
shina 4:4d9c0999a90a 838 if(old_migi1!=migi1){
shina 4:4d9c0999a90a 839 if(migi1==1){
shina 4:4d9c0999a90a 840 p=1;
shina 4:4d9c0999a90a 841 }
shina 4:4d9c0999a90a 842 }
shina 4:4d9c0999a90a 843
shina 4:4d9c0999a90a 844 switch(p){
shina 4:4d9c0999a90a 845 case 1:
yosino_adati 2:e462c8257384 846 data_servo=0x07;
yosino_adati 2:e462c8257384 847 send_data(0x18,data_servo);
shina 4:4d9c0999a90a 848 timer.start();
shina 4:4d9c0999a90a 849 if(timer.read>1.2f){
shina 4:4d9c0999a90a 850 p=2;
shina 4:4d9c0999a90a 851 timer.stop();
shina 4:4d9c0999a90a 852 timer.reset();
shina 4:4d9c0999a90a 853 }
shina 4:4d9c0999a90a 854 break;
shina 4:4d9c0999a90a 855
shina 4:4d9c0999a90a 856 case 2:
yosino_adati 2:e462c8257384 857 data7=0x00;
shina 4:4d9c0999a90a 858 send_data(0x24,data7);
shina 4:4d9c0999a90a 859 if(get_data_rs232 & 0x10){
shina 4:4d9c0999a90a 860 timer.stop();
shina 4:4d9c0999a90a 861 timer.reset();
shina 4:4d9c0999a90a 862 data7=0x10;
yosino_adati 2:e462c8257384 863 send_data(0x24,data7);
shina 4:4d9c0999a90a 864 p=3;
yosino_adati 2:e462c8257384 865 }
shina 4:4d9c0999a90a 866 break();
shina 4:4d9c0999a90a 867
shina 4:4d9c0999a90a 868 case 3:
yosino_adati 2:e462c8257384 869 data_servo=0x08;
yosino_adati 2:e462c8257384 870 send_data(0x18,data_servo);
shina 4:4d9c0999a90a 871 timer.start();
shina 4:4d9c0999a90a 872 if(timer.read()>1.5f()){
shina 4:4d9c0999a90a 873 timer.stop();
shina 4:4d9c0999a90a 874 timer.reset();
shina 4:4d9c0999a90a 875 p=4;
shina 4:4d9c0999a90a 876 }
shina 4:4d9c0999a90a 877 break();
shina 4:4d9c0999a90a 878
shina 4:4d9c0999a90a 879 case 4:
yosino_adati 2:e462c8257384 880 data7=0xff;
shina 4:4d9c0999a90a 881 send_data(0x24,data7);
shina 4:4d9c0999a90a 882 if(get_data_rs232 & 0x20){
shina 4:4d9c0999a90a 883 data7=0x10;
yosino_adati 2:e462c8257384 884 send_data(0x24,data7);
shina 4:4d9c0999a90a 885 p=5;
shina 4:4d9c0999a90a 886 }
shina 4:4d9c0999a90a 887 break;
shina 4:4d9c0999a90a 888
shina 4:4d9c0999a90a 889 default:
shina 4:4d9c0999a90a 890 break();
shina 4:4d9c0999a90a 891
shina 4:4d9c0999a90a 892
shina 4:4d9c0999a90a 893 }
shina 4:4d9c0999a90a 894
shina 4:4d9c0999a90a 895 }
shina 4:4d9c0999a90a 896 }
shina 4:4d9c0999a90a 897 */
shina 4:4d9c0999a90a 898
shina 4:4d9c0999a90a 899
shina 4:4d9c0999a90a 900
shina 4:4d9c0999a90a 901 void hakidashi(){
shina 4:4d9c0999a90a 902 if(q==0){
shina 4:4d9c0999a90a 903 if(old_migi1!=migi1){
shina 4:4d9c0999a90a 904 old_migi1=migi1;
shina 4:4d9c0999a90a 905 if(migi1==1&&select==0){
shina 4:4d9c0999a90a 906 r=1;
shina 5:29971215ef50 907 timer2.start();
shina 4:4d9c0999a90a 908 }
shina 4:4d9c0999a90a 909 }
shina 4:4d9c0999a90a 910
shina 4:4d9c0999a90a 911 switch(r){
shina 4:4d9c0999a90a 912 case 1:
shina 4:4d9c0999a90a 913 data_servo=0x07;
shina 4:4d9c0999a90a 914 send_data(0x18,data_servo);
shina 5:29971215ef50 915 //timer2.start();
shina 4:4d9c0999a90a 916 if(timer2.read()>1.5f){
shina 4:4d9c0999a90a 917 timer2.stop();
shina 4:4d9c0999a90a 918 timer2.reset();
shina 5:29971215ef50 919 timer2.start();
shina 4:4d9c0999a90a 920 r=2;
yosino_adati 2:e462c8257384 921 }
shina 4:4d9c0999a90a 922 break;
shina 4:4d9c0999a90a 923
shina 4:4d9c0999a90a 924 case 2:
shina 4:4d9c0999a90a 925 data7=0x00;
yosino_adati 2:e462c8257384 926 send_data(0x24,data7);
shina 5:29971215ef50 927 //timer2.start();
shina 4:4d9c0999a90a 928 if(timer2.read()>0.3f){
shina 4:4d9c0999a90a 929 timer2.stop();
shina 4:4d9c0999a90a 930 timer2.reset();
shina 4:4d9c0999a90a 931 data7=0x10;
shina 4:4d9c0999a90a 932 send_data(0x24,data7);
shina 4:4d9c0999a90a 933 r=3;
shina 4:4d9c0999a90a 934 }
shina 4:4d9c0999a90a 935 break;
shina 4:4d9c0999a90a 936
shina 4:4d9c0999a90a 937 case 3:
shina 4:4d9c0999a90a 938 data11=0xff;
shina 4:4d9c0999a90a 939 send_data(0x30,data11);
shina 4:4d9c0999a90a 940 if(get_data_rs232 & 0x40){
shina 4:4d9c0999a90a 941 data11=0x10;
shina 4:4d9c0999a90a 942 send_data(0x30,data11);
shina 4:4d9c0999a90a 943 r=4;
shina 3:8c4e42cba9cb 944 }
shina 4:4d9c0999a90a 945 break;
shina 4:4d9c0999a90a 946
shina 4:4d9c0999a90a 947 case 4:
shina 4:4d9c0999a90a 948 data11=0x00;
shina 4:4d9c0999a90a 949 send_data(0x30,data11);
shina 4:4d9c0999a90a 950 if(get_data_rs232 & 0x80){
shina 4:4d9c0999a90a 951 data11=0x10;
shina 4:4d9c0999a90a 952 send_data(0x30,data11);
shina 4:4d9c0999a90a 953 r=5;
shina 4:4d9c0999a90a 954 }
shina 4:4d9c0999a90a 955 break;
shina 4:4d9c0999a90a 956
shina 4:4d9c0999a90a 957 case 5:
shina 4:4d9c0999a90a 958 data7=0xff;
shina 4:4d9c0999a90a 959 send_data(0x24,data7);
shina 4:4d9c0999a90a 960 if(get_data_rs232 & 0x20){
shina 4:4d9c0999a90a 961 data7=0x10;
shina 4:4d9c0999a90a 962 send_data(0x24,data7);
shina 4:4d9c0999a90a 963 r=6;
shina 4:4d9c0999a90a 964 }
shina 8:959fe4717597 965 data_servo=0x10;
shina 8:959fe4717597 966 send_data(0x18,data_servo);
shina 4:4d9c0999a90a 967 break;
shina 4:4d9c0999a90a 968
shina 4:4d9c0999a90a 969 default:
shina 4:4d9c0999a90a 970 r=6;
shina 4:4d9c0999a90a 971 break;
yosino_adati 2:e462c8257384 972 }
yosino_adati 2:e462c8257384 973 }
shina 1:99294241f2ba 974 }
shina 1:99294241f2ba 975
shina 1:99294241f2ba 976
shina 1:99294241f2ba 977
shina 0:c7e17c2fd542 978 //ファン
yosino_adati 2:e462c8257384 979 void change_fan()
yosino_adati 2:e462c8257384 980 {
shina 3:8c4e42cba9cb 981 if(q==1){
shina 3:8c4e42cba9cb 982 if(left2==1&&(old_triangle==triangle)) {
shina 0:c7e17c2fd542 983 data8=0xff;
shina 0:c7e17c2fd542 984 data9=0xff;
shina 1:99294241f2ba 985 data10=0xff;
shina 3:8c4e42cba9cb 986 } else if(left2==0&&(old_triangle==triangle)) {
yosino_adati 2:e462c8257384 987 data8=0x00;
yosino_adati 2:e462c8257384 988 data9=0x00;
yosino_adati 2:e462c8257384 989 data10=0x00;
shina 4:4d9c0999a90a 990 }else if(left2==0&&(old_triangle!=triangle)&&select==0&&(get_data_rs232 & 0b00000011)){
shina 3:8c4e42cba9cb 991 old_triangle=triangle;
shina 3:8c4e42cba9cb 992 if(triangle==1){
shina 8:959fe4717597 993 if(!flug_servo){
shina 8:959fe4717597 994 data_servo=0xf4;
shina 8:959fe4717597 995 send_data(0x18,data_servo);
shina 8:959fe4717597 996 data10=0xff;
shina 8:959fe4717597 997 send_data(0x40,data10);
shina 8:959fe4717597 998 wait(2);
shina 8:959fe4717597 999 flug_servo=1;
shina 8:959fe4717597 1000 }
shina 3:8c4e42cba9cb 1001 data8=0xff;
shina 3:8c4e42cba9cb 1002 data9=0xff;
shina 3:8c4e42cba9cb 1003 data10=0xff;
shina 4:4d9c0999a90a 1004 data_servo=0xf3;
shina 3:8c4e42cba9cb 1005 send_data(0x26,data8);
shina 3:8c4e42cba9cb 1006 send_data(0x28,data9);
shina 3:8c4e42cba9cb 1007 send_data(0x40,data10);
shina 4:4d9c0999a90a 1008 wait(0.5);
shina 3:8c4e42cba9cb 1009 send_data(0x18,data_servo);
shina 3:8c4e42cba9cb 1010 wait(1);
shina 3:8c4e42cba9cb 1011 data8=0x00;
shina 3:8c4e42cba9cb 1012 data9=0x00;
shina 3:8c4e42cba9cb 1013 data10=0x00;
shina 3:8c4e42cba9cb 1014 send_data(0x26,data8);
shina 3:8c4e42cba9cb 1015 send_data(0x28,data9);
shina 3:8c4e42cba9cb 1016 send_data(0x40,data10);
shina 3:8c4e42cba9cb 1017 wait(0.5);
shina 4:4d9c0999a90a 1018 data_servo=0xf4;
shina 3:8c4e42cba9cb 1019 send_data(0x18,data_servo);
shina 3:8c4e42cba9cb 1020 }
shina 0:c7e17c2fd542 1021 }
yosino_adati 2:e462c8257384 1022 }
shina 3:8c4e42cba9cb 1023
shina 3:8c4e42cba9cb 1024 }
shina 1:99294241f2ba 1025
shina 0:c7e17c2fd542 1026 //i2c
yosino_adati 2:e462c8257384 1027 void send_data(char address,char data)
yosino_adati 2:e462c8257384 1028 {
shina 0:c7e17c2fd542 1029 tsushin=1;
shina 0:c7e17c2fd542 1030 tushin_check=0;
shina 0:c7e17c2fd542 1031 i2c.start();
shina 0:c7e17c2fd542 1032 i2c.write(address);
shina 0:c7e17c2fd542 1033 tushin_check=i2c.write(data);
shina 0:c7e17c2fd542 1034 i2c.stop();
yosino_adati 2:e462c8257384 1035 }
yosino_adati 2:e462c8257384 1036
shina 4:4d9c0999a90a 1037