2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_11_19

NHK2019 manual program(回収機構ストール問題解決)

Dependencies:   mbed PS3_10_26

main.cpp@4:4d9c0999a90a, 2019-10-21 (annotated)

Committer:
shina
Date:
Mon Oct 21 07:51:27 2019 +0000
Revision:
4:4d9c0999a90a
Parent:
3:8c4e42cba9cb
Child:
5:29971215ef50
NHK 2019 manual program 10/21

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