2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_10_6_pm

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

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@5:1548821bebf3, 2019-10-05 (annotated)

Committer:
shina
Date:
Sat Oct 05 09:04:33 2019 +0000
Revision:
5:1548821bebf3
Parent:
4:ab73021bbce1
Child:
6:2675289c6463
NHK2019 manual robot program_10/5;

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