2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_10_6_pm

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

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@6:2675289c6463, 2019-10-06 (annotated)

Committer:
shina
Date:
Sun Oct 06 00:48:42 2019 +0000
Revision:
6:2675289c6463
Parent:
5:1548821bebf3
Child:
7:59b18749a41b
NHK2019 manual program ver10.6a.m

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