2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_10_6_pm

NHK2019ROBOCON team A manual robot program 10/4 version (ロボコン当日使用プログラム)

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@7:59b18749a41b, 2019-10-08 (annotated)

Committer:
shina
Date:
Tue Oct 08 06:25:20 2019 +0000
Revision:
7:59b18749a41b
Parent:
6:2675289c6463 
NHK2019 team A manual robot program 10/6

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