2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_11_18

NHK 2019 manual program 11_18（送風シーケンスバグ解決ver）

Dependencies:   mbed PS3_10_26

main.cpp@7:c423a6579c3c, 2019-10-30 (annotated)

Committer:
shina
Date:
Wed Oct 30 09:18:08 2019 +0000
Revision:
7:c423a6579c3c
Parent:
5:29971215ef50
Child:
8:959fe4717597
NHK2019 manual program 10/30 version

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