2019NHK_teamA / Mbed 2 deprecated NHK2019robokonaaaaaaaaaaaaa

NHK 2019 team A manual machine program. 改良版(回収動作がおかしい)

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@2:830db42bae7e, 2019-09-17 (annotated)

Committer:
shina
Date:
Tue Sep 17 08:54:21 2019 +0000
Revision:
2:830db42bae7e
Parent:
1:99294241f2ba 
aaaaaaaaa

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