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

Dependencies:   mbed PS3_10_26

Committer:
shina
Date:
Sat Oct 26 04:30:54 2019 +0000
Revision:
5:29971215ef50
Parent:
4:4d9c0999a90a
Child:
7:c423a6579c3c
NHK 2019 manual program 10/26 version

Who changed what in which revision?

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