2019NHK_teamA / Mbed 2 deprecated NHK2019robokon

NHK 2019 team A manual machine program.

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@3:8c4e42cba9cb, 2019-09-12 (annotated)

Committer:
shina
Date:
Thu Sep 12 09:03:59 2019 +0000
Revision:
3:8c4e42cba9cb
Parent:
2:e462c8257384 
This program is about manual robot.

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